diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5b4eead0e..ff1b1a5a4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -158,5 +158,8 @@ public static final class AutonomousConstants { public static final PIDConstants POSE_PID_CONSTANTS = new PIDConstants(0, 0, 0); // X and Y position PID public static final PathConstraints CUBE_CHASE_CONSTRAINTS = new PathConstraints(2, 2); // TODO TEST FOR MAX + + public static final double MAX_VELOCITY = 2; + public static final double MAX_ACCELERATION = 1; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 143ddd8f4..946301981 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,9 +3,11 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.auto.Autonomous; +import frc.lib.auto.AutonomousCommandFactory; public class Robot extends TimedRobot { - private Command m_autonomousCommand; + private Command autonomousCommand; private RobotContainer m_robotContainer; @@ -18,6 +20,8 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + Autonomous.load(); + AutonomousCommandFactory.connectToServer(5811); } /** @@ -46,11 +50,11 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + autonomousCommand = Autonomous.getAutonomous(); // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + if (autonomousCommand != null) { + autonomousCommand.schedule(); } } @@ -64,8 +68,8 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b0cb9e53b..aa842557c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,6 @@ package frc.robot; +import java.util.HashMap; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; @@ -7,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.auto.AutonomousCommandFactory; +import frc.lib.pathplanner.com.pathplanner.lib.PathConstraints; import frc.lib.pathplanner.com.pathplanner.lib.PathPoint; import frc.lib.util.Limelight; import frc.robot.Constants.AutonomousConstants; @@ -44,5 +46,15 @@ private void configureDefaultCommands() { drivetrain.setDefaultCommand(new TeleopSwerve(drivetrain, () -> driver.getLeftY(), () -> driver.getLeftX(), () -> driver.getRightX(), () -> (driver.getRightTriggerAxis() > 0.75))); } - public Command getAutonomousCommand() { return autoFactory} + private void configureAutonomousCommands(){ + autoFactory.makeTrajectory("B2[1]-M-C-HIGH", new HashMap<>(), + new PathConstraints(AutonomousConstants.MAX_VELOCITY, AutonomousConstants.MAX_ACCELERATION)); + + } + + protected AutonomousCommandFactory getCommandFactory(){ + return autoFactory; + } + + public Command getAutonomousCommand() { return null;} }