diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 86c23ce..6366d4e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.Superstructure.SSStates; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -50,7 +49,7 @@ public void autonomousInit() { Timer.delay(0.05); // m_robotContainer.getSuperstructure().setState(SSStates.STOWED); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } @@ -65,13 +64,12 @@ public void autonomousExit() {} @Override public void teleopInit() { Timer.delay(0.05); - m_robotContainer.getSuperstructure().setStowed(); + // Timer.delay(0.15); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - m_robotContainer.getSuperstructure().setStowed(); // m_robotContainer.getSuperstructure().setState(SSStates.STOWED); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 630c0fd..fef4e21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,11 +26,11 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.*; -import frc.robot.subsystems.Superstructure.SSStates; -import frc.robot.subsystems.Vision.AlignStates; -import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; -import frc.robot.subsystems.swerve.Telemetry; -import frc.robot.subsystems.swerve.TunerConstants; +// import frc.robot.subsystems.Superstructure.SSStates; +// import frc.robot.subsystems.Vision.AlignStates; +// import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; +// import frc.robot.subsystems.swerve.Telemetry; +// import frc.robot.subsystems.swerve.TunerConstants; import static edu.wpi.first.units.Units.*; // import com.pathplanner.lib.auto.AutoBuilder; @@ -42,384 +42,384 @@ import choreo.auto.AutoTrajectory; public class RobotContainer { - private final CommandXboxController driver = new CommandXboxController(0); // My joystick - private final CommandXboxController operator = new CommandXboxController(1); - - private final TunerConstants tunerConst = new TunerConstants(); - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - Elevator elevator = new Elevator(); - Intake intake = new Intake(); - Outtake outtake = new Outtake(); - Pivot pivot = new Pivot(); - Vision vision = new Vision(drivetrain); - Climb climb = new Climb(); - - public final AutoFactory autoFactory; - - double speedMultiplier = 1.0; - Superstructure superstructure = new Superstructure(elevator, intake, outtake, pivot, climb); - - // ------- Swerve Generated ------- - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - - /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(MaxSpeed); - - private Timer timer = new Timer(); - private Pose2d targetPose = new Pose2d(); - - private AutoChooser autoChooser; - - public SendableChooser autonChooser = new SendableChooser(); - - double alignTimeout = 2.0; //TUNE - double intakeTimeout = 3.0; //TUNE - double scoreTimeout = 0.75; //TUNE - - public RobotContainer() { - // drivetrain.configureAutoBuilder(); - autoFactory = new AutoFactory( - drivetrain::getAutoBuilderPose, - drivetrain::resetTeleopPose, - drivetrain::followTrajectory, - true, - drivetrain - ); - configureBindings(); +// private final CommandXboxController driver = new CommandXboxController(0); // My joystick +// private final CommandXboxController operator = new CommandXboxController(1); + +// private final TunerConstants tunerConst = new TunerConstants(); +// public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + +// Elevator elevator = new Elevator(); +// Intake intake = new Intake(); +// Outtake outtake = new Outtake(); +// Pivot pivot = new Pivot(); +// Vision vision = new Vision(drivetrain); +// Climb climb = new Climb(); + +// public final AutoFactory autoFactory; + +// double speedMultiplier = 1.0; +// Superstructure superstructure = new Superstructure(elevator, intake, outtake, pivot, climb); + +// // ------- Swerve Generated ------- +// private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed +// private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + +// /* Setting up bindings for necessary control of the swerve drive platform */ +// private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() +// .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband +// .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors +// private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); +// private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); +// private final Telemetry logger = new Telemetry(MaxSpeed); + +// private Timer timer = new Timer(); +// private Pose2d targetPose = new Pose2d(); + +// private AutoChooser autoChooser; + +// public SendableChooser autonChooser = new SendableChooser(); + +// double alignTimeout = 2.0; //TUNE +// double intakeTimeout = 3.0; //TUNE +// double scoreTimeout = 0.75; //TUNE + +// public RobotContainer() { +// // drivetrain.configureAutoBuilder(); +// autoFactory = new AutoFactory( +// drivetrain::getAutoBuilderPose, +// drivetrain::resetTeleopPose, +// drivetrain::followTrajectory, +// true, +// drivetrain +// ); +// configureBindings(); - // initNamedCommands(); - initAutons(); - } +// // initNamedCommands(); +// initAutons(); +// } - public void initAutons() { +// public void initAutons() { - autoChooser = new AutoChooser(); +// autoChooser = new AutoChooser(); - autoChooser.addRoutine("STM_BL_L4L_SL_BL_L4R", this::STR_BR_L4L_SR_BR_L4R); - autoChooser.addRoutine("test", this::test); - - // autoChooser.addCmd("toReef", this::goToReef); - // autoChooser.addCmd("toReefL4", this::moveToReefL4); - // autoChooser.addCmd("testCircle", this::testCircle); - // autoChooser.addCmd("rotationTest", this::rotationTest); - // autoChooser.addCmd("test1", this::test); - // autoChooser.addCmd("test2", this::test2); - // autoChooser.addCmd("testBoth", this::testBoth); - - SmartDashboard.putData("Select Auto", autoChooser); +// autoChooser.addRoutine("STM_BL_L4L_SL_BL_L4R", this::STR_BR_L4L_SR_BR_L4R); +// autoChooser.addRoutine("test", this::test); + +// // autoChooser.addCmd("toReef", this::goToReef); +// // autoChooser.addCmd("toReefL4", this::moveToReefL4); +// // autoChooser.addCmd("testCircle", this::testCircle); +// // autoChooser.addCmd("rotationTest", this::rotationTest); +// // autoChooser.addCmd("test1", this::test); +// // autoChooser.addCmd("test2", this::test2); +// // autoChooser.addCmd("testBoth", this::testBoth); + +// SmartDashboard.putData("Select Auto", autoChooser); - RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler()); +// RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler()); - } +// } - // public AutoRoutine routine() { - // AutoRoutine routine = autoFactory.newRoutine("Routine"); - // AutoTrajectory traj1 = routine.trajectory("test path"); - // AutoTrajectory traj2 = routine.trajectory("back out"); - - // routine.active().onTrue( - // Commands.sequence( - // new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), - // traj1.resetOdometry(), - // traj1.cmd() - // ) - // ); - - public AutoRoutine STM_BL_L4L_SL_BL_L4R() { - AutoRoutine routine = autoFactory.newRoutine("STM_BL_L4L_SL_BL_L4R"); //ROUTINE NAME - AutoTrajectory traj1 = routine.trajectory("STM_BL"); //LOAD ALL PATHS HERE - AutoTrajectory traj2 = routine.trajectory("BLL_SL"); - AutoTrajectory traj3 = routine.trajectory("SL_BL"); - - routine.active().onTrue( - Commands.sequence( - new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), - superstructure.setState(SSStates.STOWED), - traj1.resetOdometry(), - traj1.cmd() - ) - ); +// // public AutoRoutine routine() { +// // AutoRoutine routine = autoFactory.newRoutine("Routine"); +// // AutoTrajectory traj1 = routine.trajectory("test path"); +// // AutoTrajectory traj2 = routine.trajectory("back out"); - - // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); - traj1.done().onTrue(scoreL4Left().andThen(traj2.cmd())); - traj2.done().onTrue(intake().andThen(traj3.cmd())); - traj3.done().onTrue(scoreL4Right()); - return routine; - } +// // routine.active().onTrue( +// // Commands.sequence( +// // new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), +// // traj1.resetOdometry(), +// // traj1.cmd() +// // ) +// // ); - public AutoRoutine STR_BR_L4L_SR_BR_L4R() { - AutoRoutine routine = autoFactory.newRoutine("STR_BR_L4L_SR_BR_L4R"); //ROUTINE NAME - AutoTrajectory traj1 = routine.trajectory("STR_BR"); //LOAD ALL PATHS HERE - AutoTrajectory traj2 = routine.trajectory("BRL_SR"); - AutoTrajectory traj3 = routine.trajectory("SR_BR"); +// public AutoRoutine STM_BL_L4L_SL_BL_L4R() { +// AutoRoutine routine = autoFactory.newRoutine("STM_BL_L4L_SL_BL_L4R"); //ROUTINE NAME +// AutoTrajectory traj1 = routine.trajectory("STM_BL"); //LOAD ALL PATHS HERE +// AutoTrajectory traj2 = routine.trajectory("BLL_SL"); +// AutoTrajectory traj3 = routine.trajectory("SL_BL"); - routine.active().onTrue( - - Commands.sequence( - new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), - superstructure.setState(SSStates.STOWED), - traj1.resetOdometry(), - traj1.cmd() - ) - ); +// routine.active().onTrue( +// Commands.sequence( +// new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), +// superstructure.setState(SSStates.STOWED), +// traj1.resetOdometry(), +// traj1.cmd() +// ) +// ); - // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); - traj1.done().onTrue(scoreL4Left().andThen(traj2.cmd())); - traj2.done().onTrue(intake().andThen(traj3.cmd())); - traj3.done().onTrue(scoreL4Right()); - return routine; - } - - public AutoRoutine STR_BM_L4L_SR_BM_L4R() { - AutoRoutine routine = autoFactory.newRoutine("STR_BM_L4L_SR_BM_L4R"); //ROUTINE NAME - AutoTrajectory traj1 = routine.trajectory("STR_BM"); //LOAD ALL PATHS HERE - AutoTrajectory traj2 = routine.trajectory("BML_SR"); - AutoTrajectory traj3 = routine.trajectory("SR_BM"); - - routine.active().onTrue( - Commands.sequence( - // new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), - superstructure.setState(SSStates.STOWED), - traj1.resetOdometry(), - traj1.cmd() - ) - ); +// // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); +// traj1.done().onTrue(scoreL4Left().andThen(traj2.cmd())); +// traj2.done().onTrue(intake().andThen(traj3.cmd())); +// traj3.done().onTrue(scoreL4Right()); +// return routine; +// } + +// public AutoRoutine STR_BR_L4L_SR_BR_L4R() { +// AutoRoutine routine = autoFactory.newRoutine("STR_BR_L4L_SR_BR_L4R"); //ROUTINE NAME +// AutoTrajectory traj1 = routine.trajectory("STR_BR"); //LOAD ALL PATHS HERE +// AutoTrajectory traj2 = routine.trajectory("BRL_SR"); +// AutoTrajectory traj3 = routine.trajectory("SR_BR"); + +// routine.active().onTrue( + +// Commands.sequence( +// new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), +// superstructure.setState(SSStates.STOWED), +// traj1.resetOdometry(), +// traj1.cmd() +// ) +// ); - // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); - traj1.done().onTrue(scoreL4Left().andThen(traj2.cmd())); - traj2.done().onTrue(intake().andThen(traj3.cmd())); - traj3.done().onTrue(scoreL4Right()); - return routine; - } - - public AutoRoutine test() { - AutoRoutine routine = autoFactory.newRoutine("test"); //ROUTINE NAME - AutoTrajectory traj1 = routine.trajectory("STM_BM"); //LOAD ALL PATHS HERE - - - routine.active().onTrue( - Commands.sequence( - new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), - superstructure.setState(SSStates.STOWED), - traj1.resetOdometry(), - traj1.cmd() - ) - ); +// // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); +// traj1.done().onTrue(scoreL4Left().andThen(traj2.cmd())); +// traj2.done().onTrue(intake().andThen(traj3.cmd())); +// traj3.done().onTrue(scoreL4Right()); +// return routine; +// } + +// public AutoRoutine STR_BM_L4L_SR_BM_L4R() { +// AutoRoutine routine = autoFactory.newRoutine("STR_BM_L4L_SR_BM_L4R"); //ROUTINE NAME +// AutoTrajectory traj1 = routine.trajectory("STR_BM"); //LOAD ALL PATHS HERE +// AutoTrajectory traj2 = routine.trajectory("BML_SR"); +// AutoTrajectory traj3 = routine.trajectory("SR_BM"); + +// routine.active().onTrue( +// Commands.sequence( +// // new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), +// superstructure.setState(SSStates.STOWED), +// traj1.resetOdometry(), +// traj1.cmd() +// ) +// ); - // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); - return routine; - } - - public Command alignLeft() { - return choreoAlignLeft().withTimeout(alignTimeout).andThen(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); - } - - public Command alignRight() { - return choreoAlignRight().withTimeout(alignTimeout).andThen(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); - } - - public Command scoreL2Left() { - return alignLeft().andThen(superstructure.setState(SSStates.CORAL_2)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - } - - public Command scoreL2Right() { - return alignRight().andThen(superstructure.setState(SSStates.CORAL_2)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - } - - public Command scoreL3Left() { - return alignLeft().andThen(superstructure.setState(SSStates.CORAL_3)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - } - - public Command scoreL3Right() { - return alignRight().andThen(superstructure.setState(SSStates.CORAL_3)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - } - - public Command scoreL4Left() { - return alignLeft().andThen(superstructure.setState(SSStates.CORAL_4)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - // return superstructure.setState(SSStates.CORAL_4).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - - } +// // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); +// traj1.done().onTrue(scoreL4Left().andThen(traj2.cmd())); +// traj2.done().onTrue(intake().andThen(traj3.cmd())); +// traj3.done().onTrue(scoreL4Right()); +// return routine; +// } + +// public AutoRoutine test() { +// AutoRoutine routine = autoFactory.newRoutine("test"); //ROUTINE NAME +// AutoTrajectory traj1 = routine.trajectory("STM_BM"); //LOAD ALL PATHS HERE + + +// routine.active().onTrue( +// Commands.sequence( +// new InstantCommand(()->vision.setAlignState(AlignStates.NONE)), +// superstructure.setState(SSStates.STOWED), +// traj1.resetOdometry(), +// traj1.cmd() +// ) +// ); - public Command scoreL4Right() { - return alignRight().andThen(superstructure.setState(SSStates.CORAL_4)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - // return superstructure.setState(SSStates.CORAL_4).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - - } - - public Command intake() { - return superstructure.setState(SSStates.INTAKE).andThen(Commands.waitSeconds(intakeTimeout)).andThen(superstructure.setState(SSStates.STOWED)); - } - - public Command getAutonomousCommand() { - return autoChooser.selectedCommand(); - } - - public void initNamedCommands() { - } - - public Command choreoAlignLeft() { - return drivetrain.applyRequest( - () -> new ApplyFieldSpeeds() - .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsLeft()[0], vision.getAlignOffsetsLeft()[1], vision.getRotationalAlignSpeedLeft())) - ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING))); - } - public Command choreoAlignRight() { - return drivetrain.applyRequest( - () -> new ApplyFieldSpeeds() - .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsRight()[0], vision.getAlignOffsetsRight()[1], vision.getRotationalAlignSpeedRight())) - ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING))); - } - - public void configureBindings() { - // --------------------=Driver=-------------------- - - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> - drive.withVelocityX(-driver.getLeftY() * MaxSpeed * speedMultiplier * 0.5) // Drive forward with negative Y (forward) - .withVelocityY(-driver.getLeftX() * MaxSpeed * speedMultiplier * 0.5) // Drive left with negative X (left) - .withRotationalRate(-driver.getRightX() * MaxAngularRate * 0.6 - ) // Drive counterclockwise with negative X (left) - ) - ); - - driver.a().whileTrue(drivetrain.applyRequest(() -> brake)); - driver.b().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-driver.getLeftY(), -driver.getLeftX())) - )); - - driver.povDown().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(-0.5, 0.0, 0.0)))); - driver.povUp().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0.5, 0.0, 0.0)))); - driver.povRight().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, -0.5, 0.0)))); - driver.povLeft().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0.5, 0.0)))); - drivetrain.registerTelemetry(logger::telemeterize); +// // traj1.active().whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)); +// return routine; +// } + +// public Command alignLeft() { +// return choreoAlignLeft().withTimeout(alignTimeout).andThen(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); +// } + +// public Command alignRight() { +// return choreoAlignRight().withTimeout(alignTimeout).andThen(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); +// } + +// public Command scoreL2Left() { +// return alignLeft().andThen(superstructure.setState(SSStates.CORAL_2)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// } + +// public Command scoreL2Right() { +// return alignRight().andThen(superstructure.setState(SSStates.CORAL_2)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// } + +// public Command scoreL3Left() { +// return alignLeft().andThen(superstructure.setState(SSStates.CORAL_3)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// } + +// public Command scoreL3Right() { +// return alignRight().andThen(superstructure.setState(SSStates.CORAL_3)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// } + +// public Command scoreL4Left() { +// return alignLeft().andThen(superstructure.setState(SSStates.CORAL_4)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// // return superstructure.setState(SSStates.CORAL_4).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); + +// } + +// public Command scoreL4Right() { +// return alignRight().andThen(superstructure.setState(SSStates.CORAL_4)).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// // return superstructure.setState(SSStates.CORAL_4).andThen(Commands.waitSeconds(scoreTimeout)).andThen(superstructure.setState(SSStates.STOWED)); + +// } + +// public Command intake() { +// return superstructure.setState(SSStates.INTAKE).andThen(Commands.waitSeconds(intakeTimeout)).andThen(superstructure.setState(SSStates.STOWED)); +// } + +// public Command getAutonomousCommand() { +// return autoChooser.selectedCommand(); +// } + +// public void initNamedCommands() { +// } + +// public Command choreoAlignLeft() { +// return drivetrain.applyRequest( +// () -> new ApplyFieldSpeeds() +// .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsLeft()[0], vision.getAlignOffsetsLeft()[1], vision.getRotationalAlignSpeedLeft())) +// ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING))); +// } +// public Command choreoAlignRight() { +// return drivetrain.applyRequest( +// () -> new ApplyFieldSpeeds() +// .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsRight()[0], vision.getAlignOffsetsRight()[1], vision.getRotationalAlignSpeedRight())) +// ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING))); +// } + +// public void configureBindings() { +// // --------------------=Driver=-------------------- + +// drivetrain.setDefaultCommand( +// // Drivetrain will execute this command periodically +// drivetrain.applyRequest(() -> +// drive.withVelocityX(-driver.getLeftY() * MaxSpeed * speedMultiplier * 0.5) // Drive forward with negative Y (forward) +// .withVelocityY(-driver.getLeftX() * MaxSpeed * speedMultiplier * 0.5) // Drive left with negative X (left) +// .withRotationalRate(-driver.getRightX() * MaxAngularRate * 0.6 +// ) // Drive counterclockwise with negative X (left) +// ) +// ); + +// driver.a().whileTrue(drivetrain.applyRequest(() -> brake)); +// driver.b().whileTrue(drivetrain.applyRequest(() -> +// point.withModuleDirection(new Rotation2d(-driver.getLeftY(), -driver.getLeftX())) +// )); + +// driver.povDown().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(-0.5, 0.0, 0.0)))); +// driver.povUp().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0.5, 0.0, 0.0)))); +// driver.povRight().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, -0.5, 0.0)))); +// driver.povLeft().whileTrue(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0.5, 0.0)))); + +// drivetrain.registerTelemetry(logger::telemeterize); - // reset the field-centric heading on left bumper press - driver.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - driver.rightBumper().onTrue(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); +// // reset the field-centric heading on left bumper press +// driver.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); +// driver.rightBumper().onTrue(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); - driver.y().onTrue(new InstantCommand(() -> vision.updateAlignPose())); +// driver.y().onTrue(new InstantCommand(() -> vision.updateAlignPose())); - // driver.y().onTrue(new InstantCommand(() -> speedMultiplier = 0.3)) - // .onFalse(new InstantCommand(() -> speedMultiplier = 0.1)); +// // driver.y().onTrue(new InstantCommand(() -> speedMultiplier = 0.3)) +// // .onFalse(new InstantCommand(() -> speedMultiplier = 0.1)); - driver.rightTrigger() - .whileTrue( - drivetrain.applyRequest( - () -> new ApplyFieldSpeeds() - .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsRight()[0], vision.getAlignOffsetsRight()[1], vision.getRotationalAlignSpeedRight())) - ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING)))) - .onFalse(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); +// driver.rightTrigger() +// .whileTrue( +// drivetrain.applyRequest( +// () -> new ApplyFieldSpeeds() +// .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsRight()[0], vision.getAlignOffsetsRight()[1], vision.getRotationalAlignSpeedRight())) +// ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING)))) +// .onFalse(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); - driver.rightTrigger().onFalse(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0.5, 0.0, 0.0))).withTimeout(0.2).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.NONE)))); +// driver.rightTrigger().onFalse(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0.5, 0.0, 0.0))).withTimeout(0.2).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.NONE)))); - driver.leftTrigger() - .whileTrue( - drivetrain.applyRequest( - () -> new ApplyFieldSpeeds() - .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsLeft()[0], vision.getAlignOffsetsLeft()[1], vision.getRotationalAlignSpeedLeft())) - ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING)))) - .onFalse(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); +// driver.leftTrigger() +// .whileTrue( +// drivetrain.applyRequest( +// () -> new ApplyFieldSpeeds() +// .withSpeeds(new ChassisSpeeds(vision.getAlignOffsetsLeft()[0], vision.getAlignOffsetsLeft()[1], vision.getRotationalAlignSpeedLeft())) +// ).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.ALIGNING)))) +// .onFalse(new InstantCommand(()->vision.setAlignState(AlignStates.NONE))); - driver.leftTrigger().onFalse(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0.5, 0.0, 0.0))).withTimeout(0.2).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.NONE)))); +// driver.leftTrigger().onFalse(drivetrain.applyRequest(() -> new ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0.5, 0.0, 0.0))).withTimeout(0.2).alongWith(new InstantCommand(()->vision.setAlignState(AlignStates.NONE)))); - // --------------------=Operator=-------------------- +// // --------------------=Operator=-------------------- - new Trigger(operator.leftTrigger()) - .whileTrue(superstructure.setState(SSStates.INTAKE)) - .whileFalse(superstructure.setState(SSStates.STOWED)); +// new Trigger(operator.leftTrigger()) +// .whileTrue(superstructure.setState(SSStates.INTAKE)) +// .whileFalse(superstructure.setState(SSStates.STOWED)); - new Trigger(operator.a()) - .whileTrue(superstructure.setState(SSStates.CORAL_1)) - .whileFalse(superstructure.setState(SSStates.STOWED)); +// new Trigger(operator.a()) +// .whileTrue(superstructure.setState(SSStates.CORAL_1)) +// .whileFalse(superstructure.setState(SSStates.STOWED)); - new Trigger(operator.b()) - .whileTrue(superstructure.setState(SSStates.CORAL_2)) - .onFalse(superstructure.setState(SSStates.STOWED)); +// new Trigger(operator.b()) +// .whileTrue(superstructure.setState(SSStates.CORAL_2)) +// .onFalse(superstructure.setState(SSStates.STOWED)); - new Trigger (operator.x()) - .whileTrue(superstructure.setState(SSStates.CORAL_3)) - .onFalse(superstructure.setState(SSStates.STOWED)); +// new Trigger (operator.x()) +// .whileTrue(superstructure.setState(SSStates.CORAL_3)) +// .onFalse(superstructure.setState(SSStates.STOWED)); - new Trigger(() -> climb.notAtPosition()) - .whileTrue(superstructure.setState(SSStates.STOWED)); - - new Trigger (operator.y()) - .whileTrue(superstructure.setState(SSStates.CORAL_4)) - .onFalse(superstructure.setState(SSStates.STOWED)); - - new Trigger(operator.rightBumper()) - .whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_2)) - .whileFalse(superstructure.setState(SSStates.STOWED)); - - new Trigger(operator.leftBumper()) - .whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)) - .whileFalse(superstructure.setState(SSStates.STOWED)); - - new Trigger(operator.povUp()) - .whileTrue(new InstantCommand(() -> outtake.runOuttake())) - .whileFalse(superstructure.setState(SSStates.STOWED)); - - new Trigger(operator.povDown()) - .whileTrue(superstructure.setState(SSStates.EJECT)) - .whileFalse(superstructure.setState(SSStates.STOWED)); - - new Trigger (operator.button(8).and(() -> climb.notAtPosition())) - .whileTrue(superstructure.setState(SSStates.CLIMB)) - .whileFalse(superstructure.setState(SSStates.STOWED)); - - new Trigger(operator.povRight().and(() -> climb.inClimbState())) - .whileTrue(new InstantCommand(() -> climb.manualClimb())) - .whileFalse(superstructure.setState(SSStates.STOWED)); - - } - - public Superstructure getSuperstructure() { - return superstructure; - } - - public Command rumbleControllers() { - return Commands.runOnce(() -> - CommandScheduler.getInstance().schedule( - Commands.sequence( - Commands.waitSeconds(0.5), - Commands.runOnce(() -> { - operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1); - driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1); - }), - Commands.waitSeconds(0.5), - Commands.runOnce(() -> { - operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); - driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); - }) - ) - ) - ); - } - - public void waitTime(double duration) { - Timer timer = new Timer(); - // timer.delay(duration); - timer.start(); - while (timer.get() < duration) { - - } - timer.stop(); - timer.reset(); - } +// new Trigger(() -> climb.notAtPosition()) +// .whileTrue(superstructure.setState(SSStates.STOWED)); + +// new Trigger (operator.y()) +// .whileTrue(superstructure.setState(SSStates.CORAL_4)) +// .onFalse(superstructure.setState(SSStates.STOWED)); + +// new Trigger(operator.rightBumper()) +// .whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_2)) +// .whileFalse(superstructure.setState(SSStates.STOWED)); + +// new Trigger(operator.leftBumper()) +// .whileTrue(superstructure.setState(SSStates.ALGAE_REMOVE_3)) +// .whileFalse(superstructure.setState(SSStates.STOWED)); + +// new Trigger(operator.povUp()) +// .whileTrue(new InstantCommand(() -> outtake.runOuttake())) +// .whileFalse(superstructure.setState(SSStates.STOWED)); + +// new Trigger(operator.povDown()) +// .whileTrue(superstructure.setState(SSStates.EJECT)) +// .whileFalse(superstructure.setState(SSStates.STOWED)); + +// new Trigger (operator.button(8).and(() -> climb.notAtPosition())) +// .whileTrue(superstructure.setState(SSStates.CLIMB)) +// .whileFalse(superstructure.setState(SSStates.STOWED)); + +// new Trigger(operator.povRight().and(() -> climb.inClimbState())) +// .whileTrue(new InstantCommand(() -> climb.manualClimb())) +// .whileFalse(superstructure.setState(SSStates.STOWED)); + +// } + +// public Superstructure getSuperstructure() { +// return superstructure; +// } + +// public Command rumbleControllers() { +// return Commands.runOnce(() -> +// CommandScheduler.getInstance().schedule( +// Commands.sequence( +// Commands.waitSeconds(0.5), +// Commands.runOnce(() -> { +// operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1); +// driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1); +// }), +// Commands.waitSeconds(0.5), +// Commands.runOnce(() -> { +// operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); +// driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); +// }) +// ) +// ) +// ); +// } + +// public void waitTime(double duration) { +// Timer timer = new Timer(); +// // timer.delay(duration); +// timer.start(); +// while (timer.get() < duration) { + +// } +// timer.stop(); +// timer.reset(); +// } } diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java deleted file mode 100644 index 30825a2..0000000 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ /dev/null @@ -1,85 +0,0 @@ -package frc.robot.subsystems; - -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.StrictFollower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import frc.robot.Constants; -import frc.robot.RobotMap; -import frc.robot.subsystems.Climb; -import frc.util.Util; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Climb extends SubsystemBase { - MotionMagicVoltage climbmmv = new MotionMagicVoltage(0); - - private final TalonFX climbpivot = new TalonFX(RobotMap.Climb.CLIMB_PIVOT); - - private ClimbStates State = ClimbStates.NONE; - - public enum ClimbStates { - NONE, - STOWED, - CLIMB - } - - /** Creates a new Climb. */ - public Climb() { - TalonFXConfiguration ClimbConfig = new TalonFXConfiguration(); - climbpivot.setPosition(0); - - climbpivot.getConfigurator().apply(ClimbConfig); - climbpivot.setNeutralMode(NeutralModeValue.Brake); - } - - @Override - public void periodic() { - // SmartDashboard.putNumber("climb position", getPosition()); - // SmartDashboard.putBoolean("not at postition", notAtPosition()); - // This method will be called once per scheduler run - switch (State) { - case NONE: - climbpivot.set(0); - break; - - case STOWED: - climbpivot.setVoltage(0); - break; - - case CLIMB: - climbpivot.setVoltage(2); //-2 - break; - } - } - - private double getPosition() { - return climbpivot.getPosition().getValueAsDouble(); - } - - public boolean notAtPosition() { - return !(getPosition() > Constants.Climb.kClimbPosition); //490 - } - - public boolean inClimbState() { - return State == ClimbStates.CLIMB; - } - - public void manualClimb() { - climbpivot.setVoltage(2); - } - - public void setState(ClimbStates state) { - this.State = state; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java deleted file mode 100644 index 922c2ae..0000000 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ /dev/null @@ -1,166 +0,0 @@ - -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.StrictFollower; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.RobotMap; -import frc.util.Util; -import frc.util.Alert; - -public class Elevator extends SubsystemBase { - - private final TalonFX elevatorMotor = new TalonFX(RobotMap.Elevator.ELEVATOR_LEFT); - private final TalonFX elevatorFollowerMotor = new TalonFX(RobotMap.Elevator.ELEVATOR_RIGHT); - - private final Alert elevatorMotorDisconnected = new Alert("Elevator Motor Is Disconnected", Alert.AlertType.WARNING); - - private MotionMagicVoltage mmControl = new MotionMagicVoltage(0); - - //ELEVATOR STATES - public static enum ElevatorStates { - NONE, - STOWED, - CORAL_2, - CORAL_3, - CORAL_4, - ALGAE_REMOVE_2, - ALGAE_REMOVE_3 - } - - private ElevatorStates state = ElevatorStates.NONE; - - private final SendableChooser stateChooser = new SendableChooser<>(); - - public Elevator() { - configMotors(); - - stateChooser.setDefaultOption("NONE", ElevatorStates.NONE); - stateChooser.addOption("STOWED", ElevatorStates.STOWED); - stateChooser.addOption("CORAL_2", ElevatorStates.CORAL_2); - stateChooser.addOption("CORAL_3", ElevatorStates.CORAL_3); - stateChooser.addOption("ALGAE_REMOVE_2", ElevatorStates.ALGAE_REMOVE_2); - stateChooser.addOption("ALGAE_REMOVE_3", ElevatorStates.ALGAE_REMOVE_3); - SmartDashboard.putData("Elevator State Chooser", stateChooser); - - elevatorMotor.setPosition(0); - elevatorFollowerMotor.setPosition(0); - } - - @Override - public void periodic() { - // setState(stateChooser.getSelected()); - SmartDashboard.putString("ELEVATOR STATE", state.toString()); - switch (state) { - case NONE: - elevatorMotor.set(0); - break; - - case STOWED: - moveToHeight(Constants.Elevator.kHeightStowed); - break; - - case CORAL_2: - moveToHeight(Constants.Elevator.kHeightCoral2); - break; - - case CORAL_3: - moveToHeight(Constants.Elevator.kHeightCoral3); - break; - - case CORAL_4: - moveToHeight(Constants.Elevator.kHeightCoral4); - break; - - case ALGAE_REMOVE_2: - moveToHeight(Constants.Elevator.kHeightAlgaeRemove2); - break; - - case ALGAE_REMOVE_3: - moveToHeight(Constants.Elevator.kHeightAlgaeRemove3); - break; - } - - SmartDashboard.putNumber("Elevator Position", elevatorMotor.getPosition().getValueAsDouble()); - SmartDashboard.putString("Elevator State", state.toString()); - } - - public void setState(ElevatorStates newState) { - this.state = newState; - } - - private void moveToHeight(double targetHeight) { - mmControl = mmControl.withPosition(targetHeight); - elevatorMotor.setControl(mmControl); - } - - public boolean atSetpoint() { - return Util.inRange( - elevatorMotor.getPosition().getValueAsDouble() - mmControl.Position, - -1 * Constants.Superstructure.kAtGoalTolerance, - Constants.Superstructure.kAtGoalTolerance - ); - } - - private void configMotors() { - TalonFXConfiguration config = new TalonFXConfiguration(); - - config.withFeedback( - new FeedbackConfigs() - .withSensorToMechanismRatio(Constants.Elevator.kElevatorGearRatio) - ); - - config.withMotionMagic( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(Constants.Elevator.kMotionMagicCruiseVelocity) - .withMotionMagicAcceleration(Constants.Elevator.kMotionMagicAcceleration) - ); - - config.withSlot0( - new Slot0Configs() - .withKP(Constants.Elevator.kP) - .withKI(Constants.Elevator.kI) - .withKD(Constants.Elevator.kD) - .withKS(Constants.Elevator.kS) - .withKV(Constants.Elevator.kV) - .withKA(Constants.Elevator.kA) - .withKG(Constants.Elevator.kG) - .withGravityType(GravityTypeValue.Elevator_Static) - ); - - // config.withFeedback( - // new FeedbackConfigs() - // .withSensorToMechanismRatio(Constants.Elevator.kElevatorGearRatio) - // ); - - config.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - ); - - elevatorMotor.getConfigurator().apply(config); - elevatorFollowerMotor.getConfigurator().apply(config); - - elevatorFollowerMotor.getConfigurator().apply( - new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive) - ); - - elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - elevatorFollowerMotor.setNeutralMode(NeutralModeValue.Brake); - - elevatorFollowerMotor.setControl(new StrictFollower(elevatorMotor.getDeviceID())); - - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java deleted file mode 100644 index 99918bf..0000000 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ /dev/null @@ -1,118 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.RobotMap; -import frc.robot.subsystems.Outtake.OuttakeStates; - -public class Intake extends SubsystemBase { - - private final TalonFX intakemotor = new TalonFX(RobotMap.Intake.ROLL_INTAKE); - private final PWM servo = new PWM(0); - - private VelocityVoltage velocityVoltage = new VelocityVoltage(0); - private IntakeStates state = IntakeStates.NONE; - - private final SendableChooser stateChooser = new SendableChooser<>(); - - public enum IntakeStates { - NONE, - STOWED, - INTAKE, - EJECT, - CLIMB - } - - /** Creates a new Intake. */ - public Intake() { - - stateChooser.setDefaultOption("NONE", IntakeStates.NONE); - stateChooser.addOption("STOWED", IntakeStates.STOWED); - stateChooser.addOption("INTAKE", IntakeStates.INTAKE); - stateChooser.addOption("EJECT", IntakeStates.EJECT); - SmartDashboard.putData("Intake State Chooser", stateChooser); - - TalonFXConfiguration IntakeConfig = new TalonFXConfiguration(); - - IntakeConfig.withSlot0( - new Slot0Configs() - .withKS(Constants.Intake.kIntakeS) - .withKV(Constants.Intake.kIntakeV) - .withKG(Constants.Intake.kIntakeG) - .withKA(Constants.Intake.kIntakeA) - .withKP(Constants.Intake.kIntakeP) - .withKI(Constants.Intake.kIntakeI) - .withKD(Constants.Intake.kIntakeD) - ); - - IntakeConfig.withFeedback( - new FeedbackConfigs() - .withSensorToMechanismRatio(Constants.Intake.kIntakeGearRatio) - ); - - intakemotor.getConfigurator().apply(IntakeConfig); - intakemotor.setNeutralMode(NeutralModeValue.Brake); - - } - - @Override - public void periodic() { - // SmartDashboard.putString("INTAKE STATE", state.toString()); - // setState(stateChooser.getSelected()); // TODO: remove later - switch (state) { - case NONE: - intakemotor.setControl(velocityVoltage.withVelocity(0)); - servo.setSpeed(0); - break; - - case STOWED: - intakemotor.setControl(velocityVoltage.withVelocity(Constants.Intake.kSpeedStowed)); - servo.setSpeed(0); - break; - - case INTAKE: - intakemotor.setControl(velocityVoltage.withVelocity(Constants.Intake.kSpeedIntake)); - servo.setSpeed(0); - break; - - case EJECT: - intakemotor.setControl(velocityVoltage.withVelocity(Constants.Intake.kSpeedEject)); - servo.setSpeed(0); - break; - - case CLIMB: - intakemotor.setControl(velocityVoltage.withVelocity(0)); - servo.setSpeed(-1); - break; - // This method will be called once per scheduler run - } - } - - public void setState(IntakeStates state) { - this.state = state; - } - - public void setNeutralMode(NeutralModeValue neutralModeValue) { - intakemotor.setNeutralMode(neutralModeValue); - } - - public IntakeStates getState() { - return state; - } - - public double getIntakeSpeed() { - return intakemotor.getVelocity().getValueAsDouble(); - } -} diff --git a/src/main/java/frc/robot/subsystems/Outtake.java b/src/main/java/frc/robot/subsystems/Outtake.java deleted file mode 100644 index 7ad9cce..0000000 --- a/src/main/java/frc/robot/subsystems/Outtake.java +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SlotConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.RobotMap; - -public class Outtake extends SubsystemBase { -private final TalonFX motor1 = new TalonFX(RobotMap.Outtake.ROLL_OUTTAKE); - -private VelocityVoltage velocityVoltage = new VelocityVoltage(0); - - public enum OuttakeStates { - NONE, - STOWED, - INTAKE, - CORAL_OUTTAKE, - CORAL1, - ALGAE_REMOVE, - CORAL_RECLAIM - } - - private OuttakeStates state = OuttakeStates.NONE; - private final SendableChooser stateChooser = new SendableChooser<>(); - - private void configMotors(){ - - TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); - - talonFXConfigs.withSlot0( - new Slot0Configs() - .withKP(Constants.Outtake.kOuttakeP) //35 - .withKI(0) - .withKD(0) - .withKS(Constants.Outtake.kOuttakeS) //0.312 - .withKV(Constants.Outtake.kOuttakeV) //0.1563 - .withKA(0)); - - talonFXConfigs.withFeedback( - new FeedbackConfigs() - .withSensorToMechanismRatio(Constants.Outtake.kOuttakeGearRatio) - ); - - motor1.setNeutralMode(NeutralModeValue.Brake); - - motor1.getConfigurator().apply(talonFXConfigs, 0); - motor1.getVelocity(); - motor1.setNeutralMode(NeutralModeValue.Brake); - } - - /** Creates a new Outtake. */ - public Outtake() { - // stateChooser.setDefaultOption("NONE", OuttakeStates.NONE); - // stateChooser.addOption("STOWED", OuttakeStates.STOWED); - // stateChooser.addOption("CORAL_2", OuttakeStates.CORAL_OUTTAKE); - // stateChooser.addOption("ALGAE_REMOVE_2", OuttakeStates.ALGAE_REMOVE); - // SmartDashboard.putData("Elevator State Chooser", stateChooser); - configMotors(); - } - - @Override - public void periodic() { - // SmartDashboard.putString("OUTTALKE STATE", state.toString()); - // setState(stateChooser.getSelected()); // TODO: remove this when done testing - - switch(state){ - case NONE: - motor1.setControl(velocityVoltage.withVelocity(0)); - break; - - case STOWED: - motor1.setControl(velocityVoltage.withVelocity(Constants.Outtake.kSpeedStowed)); - break; - - case INTAKE: - motor1.setControl(velocityVoltage.withVelocity(Constants.Outtake.kSpeedIntake)); - break; - - case ALGAE_REMOVE: - motor1.setControl(velocityVoltage.withVelocity(Constants.Outtake.kSpeedAlgaeRemoval)); - break; - - case CORAL_OUTTAKE: - motor1.setControl(velocityVoltage.withVelocity(Constants.Outtake.kSpeedOuttake)); - break; - - case CORAL_RECLAIM: - motor1.setControl(velocityVoltage.withVelocity(-4)); - break; - - case CORAL1: - motor1.setControl(velocityVoltage.withVelocity(3)); - break; - - } - // This method will be called once per scheduler run - } - - public void runOuttake() { - setState(OuttakeStates.CORAL_OUTTAKE); - } - - public void revertOuttake() { - setState(OuttakeStates.CORAL_RECLAIM); - } - - public void setState(OuttakeStates state) { - this.state = state; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java deleted file mode 100644 index abd5ac5..0000000 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ /dev/null @@ -1,133 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.RobotMap; -import frc.util.*; - -public class Pivot extends SubsystemBase { - MotionMagicVoltage mm = new MotionMagicVoltage(0); - private final TalonFX motor = new TalonFX(RobotMap.AlgaePivot.ALGAE_PIVOT); - PivotStates currentState = PivotStates.NONE; - - private final SendableChooser stateChooser = new SendableChooser<>(); - - public enum PivotStates { - NONE, - STOWED, - ALGAE_REMOVE, - INTAKE, - L4 - } - - - public Pivot(){ - - stateChooser.setDefaultOption("NONE", PivotStates.NONE); - stateChooser.addOption("STOWED", PivotStates.STOWED); - stateChooser.addOption("ALGAE_REMOVE_2", PivotStates.ALGAE_REMOVE); - SmartDashboard.putData("Pivot State Chooser", stateChooser); - - TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); - talonFXConfigs.withSlot0( - new Slot0Configs() - .withKS(Constants.Pivot.kS)//0.24 - .withKV(Constants.Pivot.kV)//7 - .withKA(Constants.Pivot.kA)//0.001 - .withKG(Constants.Pivot.kG) //-0.42 - .withKP(Constants.Pivot.kP) - .withKI(Constants.Pivot.kI) - .withKD(Constants.Pivot.kD) - .withGravityType(GravityTypeValue.Arm_Cosine) - ); - - talonFXConfigs.withFeedback( - new FeedbackConfigs() - .withSensorToMechanismRatio(Constants.Pivot.kPivotGearRatio) - ); - - talonFXConfigs.withMotionMagic(new MotionMagicConfigs() - .withMotionMagicAcceleration(Constants.Pivot.kMotionMagicAcceleration) - .withMotionMagicCruiseVelocity(Constants.Pivot.kMotionMagicCruiseVelocity) - ); - - motor.getConfigurator().apply(talonFXConfigs, 0.050); - mm.Slot = 0; - motor.setNeutralMode(NeutralModeValue.Brake); - - motor.setPosition(-0.2744); - } - - @Override - public void periodic() { - // SmartDashboard.putString("State [AP]", currentState.toString()); - // setState(stateChooser.getSelected()); - - switch(currentState){ - case NONE: - motor.set(0); - break; - - case STOWED: - // motor.setNeutralMode(NeutralModeValue.Brake); - motor.setControl(mm.withPosition(Constants.Pivot.kAngleStowed)); - break; - - case ALGAE_REMOVE: - // motor.setNeutralMode(NeutralModeValue.Coast); - motor.setControl(mm.withPosition(Constants.Pivot.kAngleAlgaeRemove)); - break; - - case INTAKE: - // motor.setNeutralMode(NeutralModeValue.Brake); - motor.setControl(mm.withPosition(Constants.Pivot.kAngleIntake)); - break; - - case L4: - // motor.setNeutralMode(NeutralModeValue.Coast); - motor.setControl(mm.withPosition(Constants.Pivot.kAngleL4)); - break; - - } - } - - public void setState(PivotStates state) { - this.currentState = state; - } - - public Command setStateCmd() { - return new InstantCommand(() -> setState(PivotStates.STOWED)); - } - - public boolean atSetpoint() { - return Util.inRange( - motor.getPosition().getValueAsDouble() - mm.Position, - -1 * Constants.Superstructure.kAtGoalTolerance, - Constants.Superstructure.kAtGoalTolerance - ); - } - - public double getShooterPivotAngle() { - double deg = Conversions.falconToDegrees(motor.getRotorPosition().getValueAsDouble(), Constants.Pivot.kPivotGearRatio); - - deg %= 360; - if (deg > 180) { - deg -= (360); - } - return deg; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java new file mode 100644 index 0000000..b64a961 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -0,0 +1,104 @@ + +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.StrictFollower; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.RobotMap; +import frc.util.Util; +import frc.util.Alert; + +public class Subsystem extends SubsystemBase { + private final TalonFX elevatormotor1 = new TalonFX(0); + private final TalonFX elevatormotor2 = new TalonFX(1); + + private final MotionMagicVoltage motionmagic = new MotionMagicVoltage(0.0); + + //PIDSVAG + + public void motorconfigs(){ + TalonFXConfiguration configs = new TalonFXConfiguration(); + configs.withSlot0( + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(0) + .withKA(0) + .withKG(0) + .withGravityType(GravityTypeValue.Elevator_Static) + + ); + + } + + public Subsystem() { + } + + private void gotoheight(int num) + { + motionmagic = motionmagic.withPosition(num); + elevatormotor1.setControl(motionmagic); + elevatormotor2.setControl(motionmagic); + + } + + private SubsystemStates state = SubsystemStates.LOW; + + + public static enum SubsystemStates { + LOW, + MEDIUM, + HIGH + } + + @Override + public void periodic() { + switch (state) + { + case LOW: + gotoheight(0); + + break; + case MEDIUM: + gotoheight(1); + break; + case HIGH: + gotoheight(2); + break; + + } + + } + + + // + public SubsystemStates settheState(SubsystemStates setstate) + { + this.state=setstate; + return state; + } + + + public SubsystemStates gettheState() + { + return state; + } + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java deleted file mode 100644 index 5af74a8..0000000 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ /dev/null @@ -1,254 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.subsystems.Climb.ClimbStates; -import frc.robot.subsystems.Elevator.ElevatorStates; -import frc.robot.subsystems.Intake.IntakeStates; -import frc.robot.subsystems.Outtake.OuttakeStates; -import frc.robot.subsystems.Pivot.PivotStates; - -public class Superstructure extends SubsystemBase { - - public static enum SSStates { - NONE, - STOWED, - INTAKE, - CORAL_1, - CORAL_2, - CORAL_3, - CORAL_4, - ALGAE_REMOVE_2, - ALGAE_REMOVE_3, - CLIMB, - EJECT - } - - /** - * funnel top roller intake - can align with 2 thingies - only need to spin wheels - * fixed angle top and bottom outtake - to remove algae just bump into reef and spin wheels out then back away - just spin wheels for outtake - * 28 by 28 in swerve drivebase - * subsystems: intake (rollers mayb beam break or some sort of detection)(Emily), elevator (TBD)(Jai), shooter (rollers)(Bella), vision (Zack, Aaron, Eric), pathplanner (Jai, Bella, Emily), superstructure (Denielle) - */ - - public SSStates currentState = SSStates.NONE; - - Elevator elevator; - Intake intake; - Pivot pivot; - Outtake outtake; - Climb climb; - - private Timer timer = new Timer(); - - public Superstructure(Elevator elevator, Intake intake, Outtake outtake, Pivot pivot, Climb climb) { - timer.restart(); - - this.elevator = elevator; - this.intake = intake; - this.pivot = pivot; - this.outtake = outtake; - this.climb = climb; - } - - @Override - public void periodic() { - SmartDashboard.putString("SUPERSTRUCTURE STATE CURRENT", currentState.toString()); - } - - // States - - private Command stowed() { - return Commands.runOnce(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.STOWED); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }); - } - - private Command intake() { - return Commands.sequence( - new InstantCommand(() -> { - outtake.setState(OuttakeStates.INTAKE); - elevator.setState(ElevatorStates.STOWED); - pivot.setState(PivotStates.INTAKE); - climb.setState(ClimbStates.STOWED); - }), - new WaitCommand(0.5), - new InstantCommand(()->intake.setState(IntakeStates.INTAKE))); - } - - private Command coral2() { // test method - return Commands.sequence( - new InstantCommand(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.CORAL_2); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }), - new WaitUntilCommand(() -> elevator.atSetpoint()), - new InstantCommand(() -> outtake.setState(OuttakeStates.CORAL_OUTTAKE)) - ); - } - - private Command coral3() { - return Commands.sequence( - new InstantCommand(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.CORAL_3); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }), - new WaitUntilCommand(() -> elevator.atSetpoint()), - new InstantCommand(() -> outtake.setState(OuttakeStates.CORAL_OUTTAKE)) - ); - } - - private Command coral4() { - return Commands.sequence( - new InstantCommand(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.CORAL_4); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }), - new WaitUntilCommand(() -> elevator.atSetpoint()), - new WaitCommand(0.2), - // new InstantCommand(() -> pivot.setState(PivotStates.L4)), - // new WaitUntilCommand(() -> pivot.atSetpoint()), - // new WaitCommand(0.2), - new InstantCommand(() -> outtake.setState(OuttakeStates.CORAL_OUTTAKE)) - ); - } - - private Command algaeRemove2() { - return Commands.sequence( - new InstantCommand(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.ALGAE_REMOVE_2); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }), - new WaitUntilCommand(() -> elevator.atSetpoint()), - new InstantCommand(() -> {outtake.setState(OuttakeStates.ALGAE_REMOVE); pivot.setState(PivotStates.ALGAE_REMOVE);}) - ); - } - - private Command algaeRemove3() { - return Commands.sequence( - new InstantCommand(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.ALGAE_REMOVE_3); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }), - new WaitUntilCommand(() -> elevator.atSetpoint()), - new InstantCommand(() -> {outtake.setState(OuttakeStates.ALGAE_REMOVE); pivot.setState(PivotStates.ALGAE_REMOVE);}) - ); - } - - private Command eject() { - return new InstantCommand(() -> { - intake.setState(IntakeStates.EJECT); - outtake.setState(OuttakeStates.CORAL_RECLAIM); - elevator.setState(ElevatorStates.STOWED); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }); - } - - private Command coral1() { - return new InstantCommand(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.CORAL1); - elevator.setState(ElevatorStates.STOWED); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }); - } - private Command climb() { - return new InstantCommand(() -> { - climb.setState(ClimbStates.CLIMB); - intake.setState(IntakeStates.CLIMB); - outtake.setState(OuttakeStates.STOWED); - pivot.setState(PivotStates.STOWED); - elevator.setState(ElevatorStates.STOWED); - }); - } - - // ------ commands ------- - /** - * Sets the superstructure target state - * @param currentState Target state - */ - - public void setStowed() { - Commands.runOnce(() -> { - intake.setState(IntakeStates.STOWED); - outtake.setState(OuttakeStates.STOWED); - elevator.setState(ElevatorStates.STOWED); - pivot.setState(PivotStates.STOWED); - climb.setState(ClimbStates.STOWED); - }); - } - - public Command setState(SSStates wantedState) { - switch (wantedState) { - case NONE: - break; - - case STOWED: - return stowed(); - - case INTAKE: - return intake(); - - case CORAL_1: - return coral1(); - - case CORAL_2: - return coral2(); - - case CORAL_3: - return coral3(); - - case CORAL_4: - return coral4(); - - case ALGAE_REMOVE_2: - return algaeRemove2(); - - case ALGAE_REMOVE_3: - return algaeRemove3(); - - case EJECT: - return eject(); - - case CLIMB: - return climb(); - - default: - return Commands.print("failed"); - } - return Commands.print("failed"); - } - - // ------ methods ------ - - public SSStates getCurrentState() { - return currentState; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java deleted file mode 100644 index a0609a7..0000000 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ /dev/null @@ -1,449 +0,0 @@ -package frc.robot.subsystems; - -import java.time.zone.ZoneOffsetTransitionRule.TimeDefinition; -import java.util.Arrays; -import java.util.Optional; - -// import com.pathplanner.lib.auto.AutoBuilder; -// import com.pathplanner.lib.path.PathConstraints; - -import edu.wpi.first.hal.AllianceStationID; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; -import frc.util.LimelightHelper; -import frc.util.ShuffleboardIO; -import frc.util.Util; - -public class Vision extends SubsystemBase { - StructPublisher publisher = NetworkTableInstance.getDefault().getStructTopic("Current Pose", Pose2d.struct).publish(); - StructPublisher publisher2 = NetworkTableInstance.getDefault().getStructTopic("Target Pose", Pose2d.struct).publish(); - - private PIDController pidRotationAlign = new PIDController(4.5, 0, 0); //3.5 0 0 - private PIDController pidXAlign = new PIDController(1.75, 0, 0); //2.5 0 0 - private PIDController pidYAlign = new PIDController(1.75, 0, 0); //2.5 0 0 - - Timer timer = new Timer(); - - Pose2d testPose = new Pose2d(); - - public enum AlignStates{ - ALIGNING, - NONE - } - - AlignStates currentAlignState = AlignStates.NONE; - - Alliance allianceColor = Alliance.Blue; - - private int[] blueReefAprilTag = {17, 18, 19, 20, 21, 22}; - private int[] redReefAprilTag = {6, 7, 8, 9, 10, 11}; - - CommandSwerveDrivetrain drivetrain; - - String name = "limelight-front"; - int counter = 0; - int tagOutside = 1; - - - Pose2d lastPose = new Pose2d(); - double lastTimeStamp = 0.0; - - LimelightHelper.PoseEstimate estimate; - - Command pathL; - Command pathR; - - double distToAprilLeft = 0.0; - double distToAprilRight = 0.0; - boolean updateFirst = true; - - double fiducialID; - - - public Vision(CommandSwerveDrivetrain drive) { - drivetrain = drive; - timer.reset(); - timer.start(); - - - ShuffleboardIO.addSlider("Alignment X", 0, 7, 0); - ShuffleboardIO.addSlider("Alignment Y", 0, 7, 0); - } - - @Override - public void periodic() { - testPose = getPoseTesting(); - - SmartDashboard.putNumber("Target Speed X", getAlignOffsetsRight()[0]); - SmartDashboard.putNumber("Target Speed Y", getAlignOffsetsRight()[1]); - - if (timer.get() > 0.2 && currentAlignState == AlignStates.ALIGNING) { - updateAlignPose(); - // System.out.println("UPDATING"); - timer.reset(); - timer.start(); - } - - debug(); - } - - public void setAlignState(AlignStates alignState) { - currentAlignState = alignState; - } - - // public Pose2d getPoseLeft() { - // fiducialID = getTargetTagLeft(); - - // if (getTargetTagLeft() == 17) { - // return Constants.Vision.poseAlignBlueLeft17; - // } else if (getTargetTagLeft() == 18) { - // return Constants.Vision.poseAlignBlueLeft18; - // } else if (getTargetTagLeft() == 19) { - // return Constants.Vision.poseAlignBlueLeft19; - // } else if (getTargetTagLeft() == 20) { - // return Constants.Vision.poseAlignBlueLeft19; - // } else if (getTargetTagLeft() == 21) { - // return Constants.Vision.poseAlignBlueLeft19; - // } else if (getTargetTagLeft() == 22) { - // return Constants.Vision.poseAlignBlueLeft19; - // } else if (getTargetTagLeft() == 6) { - // return Constants.Vision.poseAlignRedLeft6; - // } else if (getTargetTagLeft() == 7) { - // return Constants.Vision.poseAlignRedLeft7; - // } else if (getTargetTagLeft() == 8) { - // return Constants.Vision.poseAlignRedLeft8; - // } else if (getTargetTagLeft() == 9) { - // return Constants.Vision.poseAlignRedLeft9; - // } else if (getTargetTagLeft() == 10) { - // return Constants.Vision.poseAlignRedLeft10; - // } else if (getTargetTagLeft() == 11) { - // return Constants.Vision.poseAlignRedLeft11; - // } else { - // return drivetrain.getAutoBuilderPose(); - // } - // } - - // public Pose2d getPoseRight() { - // fiducialID = getTargetTagRight(); - - // if (getTargetTagRight() == 17) { - // return Constants.Vision.poseAlignBlueRight17; - // } else if (getTargetTagRight() == 18) { - // return Constants.Vision.poseAlignBlueRight18; - // } else if (getTargetTagRight() == 19) { - // return Constants.Vision.poseAlignBlueRight19; - // } else if (getTargetTagRight() == 20) { - // return Constants.Vision.poseAlignBlueRight19; - // } else if (getTargetTagRight() == 21) { - // return Constants.Vision.poseAlignBlueRight19; - // } else if (getTargetTagRight() == 22) { - // return Constants.Vision.poseAlignBlueRight19; - // } else if (getTargetTagRight() == 6) { - // return Constants.Vision.poseAlignRedRight6; - // } else if (getTargetTagRight() == 7) { - // return Constants.Vision.poseAlignRedRight7; - // } else if (getTargetTagRight() == 8) { - // return Constants.Vision.poseAlignRedRight8; - // } else if (getTargetTagRight() == 9) { - // return Constants.Vision.poseAlignRedRight9; - // } else if (getTargetTagRight() == 10) { - // return Constants.Vision.poseAlignRedRight10; - // } else if (getTargetTagRight() == 11) { - // return Constants.Vision.poseAlignRedRight11; - // } else { - // return drivetrain.getAutoBuilderPose(); - // } - - // // AutoBuilder.pathfindToPose( - // // endpointL, - // // new PathConstraints(2, 2, 3, 2), 0.0 - // // ); - // } - - public Pose2d getPoseTesting() { - testPose = new Pose2d( - // ShuffleboardIO.getDouble("Alignment X"), - // ShuffleboardIO.getDouble("Alignment Y"), - // Rotation2d.fromDegrees(180)); - 12.684, 3.078,Rotation2d.fromDegrees(60)); - return testPose; - } - - public Pose2d getTargetTagLeft() { - int tag = -1; - double minDistance = Integer.MAX_VALUE; - Pose2d targetPose = new Pose2d(); - for (int i = 6; i <= 11; i++) { - Pose2d target = new Pose2d(0, 0, new Rotation2d(0)); - if (i == 6) target = Constants.Vision.poseAlignRedLeft6; - else if (i == 7) target = Constants.Vision.poseAlignRedLeft7; - else if (i == 8) target = Constants.Vision.poseAlignRedLeft8; - else if (i == 9) target = Constants.Vision.poseAlignRedLeft9; - else if (i == 10) target = Constants.Vision.poseAlignRedLeft10; - else if (i == 11) target = Constants.Vision.poseAlignRedLeft11; - - double distance = Util.distance(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY(), target.getX(), target.getY()); - if (distance < minDistance) { - minDistance = distance; - tag = i; - targetPose = target; - } - } - - for (int i = 17; i <= 22; i++) { - Pose2d target = new Pose2d(0, 0, new Rotation2d(0)); - if (i == 17) target = Constants.Vision.poseAlignBlueLeft17; - else if (i == 18) target = Constants.Vision.poseAlignBlueLeft18; - else if (i == 19) target = Constants.Vision.poseAlignBlueLeft19; - else if (i == 20) target = Constants.Vision.poseAlignBlueLeft20; - else if (i == 21) target = Constants.Vision.poseAlignBlueLeft21; - else if (i == 22) target = Constants.Vision.poseAlignBlueLeft22; - - double distance = Util.distance(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY(), target.getX(), target.getY()); - if (distance < minDistance) { - minDistance = distance; - tag = i; - targetPose = target; - } - } - - tagOutside = tag; - - return targetPose; - } - - public Pose2d getTargetTagRight() { - int tag = -1; - double minDistance = Integer.MAX_VALUE; - Pose2d targetPose = new Pose2d(); - for (int i = 6; i <= 11; i++) { - Pose2d target = new Pose2d(0, 0, new Rotation2d(0)); - if (i == 6) target = Constants.Vision.poseAlignRedRight6; - else if (i == 7) target = Constants.Vision.poseAlignRedRight7; - else if (i == 8) target = Constants.Vision.poseAlignRedRight8; - else if (i== 9) target = Constants.Vision.poseAlignRedRight9; - else if (i == 10) target = Constants.Vision.poseAlignRedRight10; - else if (i == 11) target = Constants.Vision.poseAlignRedRight11; - - double distance = Util.distance(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY(), target.getX(), target.getY()); - if (distance < minDistance) { - minDistance = distance; - tag = i; - targetPose = target; - } - } - - for (int i = 17; i <= 22; i++) { - Pose2d target = new Pose2d(0, 0, new Rotation2d(0)); - if (i == 17) target = Constants.Vision.poseAlignBlueRight17; - else if (i == 18) target = Constants.Vision.poseAlignBlueRight18; - else if (i == 19) target = Constants.Vision.poseAlignBlueRight19; - else if (i == 20) target = Constants.Vision.poseAlignBlueRight20; - else if (i == 21) target = Constants.Vision.poseAlignBlueRight21; - else if (i == 22) target = Constants.Vision.poseAlignBlueRight22; - - double distance = Util.distance(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY(), target.getX(), target.getY()); - if (distance < minDistance) { - minDistance = distance; - tag = i; - targetPose = target; - } - } - - tagOutside = tag; - - return targetPose; - } - - /** - * @return {xCoord, yCoord, timestamp} - */ - public Translation2d getTranslation2d() { - LimelightHelper.PoseEstimate estimate; - if (LimelightHelper.getTV(name)) { - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Blue) { - estimate = LimelightHelper.getBotPoseEstimate_wpiBlue_MegaTag2(name); - } - else { - estimate = LimelightHelper.getBotPoseEstimate_wpiRed_MegaTag2(name); - } - return new Translation2d(estimate.pose.getX(), estimate.pose.getY()); - } else { - return new Translation2d(0.0, 0.0); - } - } - - public Pose2d getPose2d() { - if (LimelightHelper.getTV(name)) { - estimate = LimelightHelper.getBotPoseEstimate_wpiBlue(name); - lastPose = estimate.pose; - return estimate.pose; - } else { - return lastPose; - } - } - - public double getTX() { - if (hasTargets()) { - return LimelightHelper.getTX(name); - } else { - return 0.0; - } - } - - // public void updateAlignPose() { - // if (LimelightHelper.getTV(name) && updatePose) { - // estimate = LimelightHelper.getBotPoseEstimate_wpiBlue(name); - // drivetrain.resetPose(estimate.pose); - // } - // } - - public void updateAlignPose() { - if (LimelightHelper.getTV(name)) { - estimate = LimelightHelper.getBotPoseEstimate_wpiBlue(name); - drivetrain.resetPose(estimate.pose); - } - } - - public double getTimeStamp() { - if (LimelightHelper.getTV(name)) { - estimate = LimelightHelper.getBotPoseEstimate_wpiBlue(name); - lastTimeStamp = estimate.timestampSeconds; - return estimate.timestampSeconds; - } else { - return lastTimeStamp; - } - } - - public double getDistToTarget() { - if (LimelightHelper.getTV(name)) { - double dist = (0.3048 - Constants.Vision.kLimelightHeightMeters) / Math.tan(LimelightHelper.getTY(name)); // tan(vertical angle) = height of robot to apriltag / distance to april tag - return dist; - } else { - return 1.5; // if there is no target return a value greater than 1 so pose doesn't update - } - } - - public boolean hasTargets() { - return LimelightHelper.getTV(name); - } - - public boolean hasReefTargets(){ - if(LimelightHelper.getTV(name) ){ - for (int reefID : redReefAprilTag) { - if (LimelightHelper.getFiducialID(name) == reefID) return true; - } - for (int reefID : blueReefAprilTag) { - if (LimelightHelper.getFiducialID(name) == reefID) return true; - } - } - return false; - } - - // public Pose2d addVisionPose() { - // if (hasTargets()) { - // Pose2d pose = getPose2d(); - // drivetrain.addVisionMeasurement(pose, getTimeStamp()); - // return pose; - // } else { - // return drivetrain.getAutoBuilderPose(); - // } - // } - - public Pose2d resetPose() { - if (hasTargets()) { - Pose2d pose = getPose2d(); - drivetrain.resetTeleopPose(pose); - return pose; - } else { - return drivetrain.getAutoBuilderPose(); - } - } - - public double[] getAlignOffsetsRight() { - double speedX = pidXAlign.calculate(drivetrain.getState().Pose.getX(), getTargetTagRight().getX()); - double speedY = pidYAlign.calculate(drivetrain.getState().Pose.getY(), getTargetTagRight().getY()); - - if (Util.inRange(speedX, -0.05, 0.05)) { - speedX = 0.0; - } - - if (Util.inRange(speedY, -0.05, 0.05)) { - speedY = 0.0; - } - - double[] values = { - speedX, speedY - }; - return values; - } - - public double[] getAlignOffsetsLeft() { - double speedX = pidXAlign.calculate(drivetrain.getState().Pose.getX(), getTargetTagLeft().getX()); - double speedY = pidYAlign.calculate(drivetrain.getState().Pose.getY(), getTargetTagLeft().getY()); - - if (Util.inRange(speedX, -0.0001, 0.0001)) { - speedX = 0.0; - } - - if (Util.inRange(speedY, -0.0001, 0.0001)) { - speedY = 0.0; - } - - double[] values = { - speedX, speedY - }; - return values; - } - - public double getRotationalAlignSpeedRight() { - pidRotationAlign.enableContinuousInput(0, 2*Math.PI); - double currentRotation = drivetrain.getState().Pose.getRotation().getRadians(); - double targetRotation = getTargetTagRight().getRotation().getRadians(); - - double targetSpeed = pidRotationAlign.calculate(currentRotation, targetRotation); - // System.out.println(targetSpeed); - return targetSpeed; - } - - public double getRotationalAlignSpeedLeft() { - pidRotationAlign.enableContinuousInput(0, 2*Math.PI); - double currentRotation = drivetrain.getState().Pose.getRotation().getRadians(); - double targetRotation = getTargetTagLeft().getRotation().getRadians(); - - double targetSpeed = pidRotationAlign.calculate(currentRotation, targetRotation); - System.out.println(currentRotation); - System.out.println(targetRotation); - // System.out.println(targetSpeed); - return targetSpeed; - } - - private void debug() { - SmartDashboard.putBoolean("Has Reef Target [VI]", hasReefTargets()); - SmartDashboard.putNumber("APRILTAG POSE", getPose2d().getX()); - SmartDashboard.putNumber("dist to left", distToAprilLeft); - SmartDashboard.putNumber("dist to right", distToAprilRight); - SmartDashboard.putNumber("times reset", counter); - SmartDashboard.putBoolean("has targets", hasTargets()); - SmartDashboard.putString("ALIGN STATE", currentAlignState.toString()); - - publisher.set(drivetrain.getState().Pose); - publisher2.set(getTargetTagRight()); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java deleted file mode 100644 index 5f2b5f2..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ /dev/null @@ -1,370 +0,0 @@ -package frc.robot.subsystems.swerve; - -import static edu.wpi.first.units.Units.*; - -import java.util.List; -import java.util.function.BiConsumer; -import java.util.function.Supplier; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import choreo.trajectory.SwerveSample; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import frc.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; -import frc.robot.Constants; -import frc.robot.subsystems.Vision; -import frc.robot.subsystems.Vision.*; - - - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - SwerveDriveKinematics m_kinematics; - - private PIDController C_PID_Translation = new PIDController(8, 0.0, 0.0); //10 0 0 - private PIDController C_PID_Rotation = new PIDController(50, 0, 1); //0.2 0 0 - - private double pathX = 0.0; - private double pathY = 0.0; - private double pathHeading = 0.0; - private double pidOutputX = 0.0; - private double pidOutputY = 0.0; - private double pidOutputOmega = 0.0; - - // PIDConstants PP_PID_Translation = new PIDConstants(0.25, 0, 0); //0.25, 0, 0 - // PIDConstants PP_PID_Rotation = new PIDConstants(1.85, 0, 0.65); //1.85, 0, 0.65 - - // Vision vision = new Vision(); - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds(); - - StructPublisher publisher = NetworkTableInstance.getDefault().getStructTopic("Align Pose", Pose2d.struct).publish(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - public Rotation2d getYaw() { - return new Rotation2d(Math.toRadians(getPigeon2().getYaw().getValueAsDouble())); - } - - public Pose2d getAutoBuilderPose() { - return getState().Pose; - } - - public ChassisSpeeds getCurrentRobotChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); - } - - public void updateOdometry(Pose2d pose, double timestamp) { - // this.resetPose(pose); - this.addVisionMeasurement(pose, timestamp); // try using this instead of reset pose to prevent jitter - } - - public void resetTeleopPose(Pose2d pose) { - this.resetPose(pose); - } - - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - publisher.set(getState().Pose); - } - - public double getContinuousRadians ( double radians){ - if (radians < 0) { - radians += 2 * Math.PI * (Math.abs(radians) / 360 + 1);; - // * (Math.abs(radians) / 360 + 1 - } - // if (radians > 2*Math.PI) { - // radians -= - // } - - return radians; - } - - public double getOmegaPID() { - Pose2d pose = getAutoBuilderPose(); - - double roboRotation = getContinuousRadians(pose.getRotation().getRadians()); - double heading = pathHeading; - if (Math.abs(roboRotation-heading) > Math.PI) { - if (heading > roboRotation) roboRotation = roboRotation + 2 * Math.PI; - if (heading < roboRotation) roboRotation = roboRotation - 2 * Math.PI; - } - return C_PID_Translation.calculate(roboRotation,heading) ; - } - - public void followTrajectory(SwerveSample path) { - C_PID_Rotation.enableContinuousInput(0, 2 * Math.PI); - Pose2d pose = getAutoBuilderPose(); - - pathX = path.x; - pathY = path.y; - pathHeading = path.heading; - - pidOutputX = C_PID_Translation.calculate(pose.getX(), path.x); - pidOutputY = C_PID_Translation.calculate(pose.getY(), path.y); - pidOutputOmega = C_PID_Rotation.calculate(getContinuousRadians(pose.getRotation().getRadians()), path.heading); - - // Generate the next speeds for the robot - ChassisSpeeds speeds = new ChassisSpeeds( - path.vx + C_PID_Translation.calculate(pose.getX(), path.x), - path.vy + C_PID_Translation.calculate(pose.getY(), path.y), - path.omega + getOmegaPID() - // C_PID_Rotation.calculate(getContinuousRadians(pose.getRotation().getRadians()), path.heading) - - // path.vx, - // path.vy, - // path.omega - ); - - setControl(m_pathApplyFieldSpeeds.withSpeeds(speeds)); - - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/Telemetry.java b/src/main/java/frc/robot/subsystems/swerve/Telemetry.java deleted file mode 100644 index aa4b01a..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/Telemetry.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot.subsystems.swerve; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; - -public class Telemetry { - private final double MaxSpeed; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; - - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; - - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java deleted file mode 100644 index 0c1ad0d..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java +++ /dev/null @@ -1,286 +0,0 @@ -package frc.robot.subsystems.swerve; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -// import frc.robot.subsystems.CommandSwerveDrivetrain; - -// Generated by the Tuner X Swerve Project Generator -// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.66).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; - - private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 2; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 22; - private static final int kFrontLeftSteerMotorId = 21; - private static final int kFrontLeftEncoderId = 31; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.074951171875); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(11.25); - private static final Distance kFrontLeftYPos = Inches.of(11.25); - - // Front Right - private static final int kFrontRightDriveMotorId = 28; - private static final int kFrontRightSteerMotorId = 27; - private static final int kFrontRightEncoderId = 34; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.21630859375); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(11.25); - private static final Distance kFrontRightYPos = Inches.of(-11.25); - - // Back Left - private static final int kBackLeftDriveMotorId = 26; - private static final int kBackLeftSteerMotorId = 25; - private static final int kBackLeftEncoderId = 33; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.35693359375); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-11.25); - private static final Distance kBackLeftYPos = Inches.of(11.25); - - // Back Right - private static final int kBackRightDriveMotorId = 24; - private static final int kBackRightSteerMotorId = 23; - private static final int kBackRightEncoderId = 32; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.3505859375); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-11.25); - private static final Distance kBackRightYPos = Inches.of(-11.25); - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. - */ - public static CommandSwerveDrivetrain createDrivetrain() { - return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); - } - - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } - } -}