From 47c29c9b34700f0ff7014f72498932395cbeb859 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 4 Feb 2023 11:22:43 -0700 Subject: [PATCH 1/6] integrated gyro + odometry, trying to fix drift --- simgui-ds.json | 92 ++++++++++ simgui.json | 7 + .../java/frc4388/robot/RobotContainer.java | 14 +- .../frc4388/robot/subsystems/SwerveDrive.java | 161 +++++++++++------- .../robot/subsystems/SwerveModule.java | 29 +++- 5 files changed, 234 insertions(+), 69 deletions(-) create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..449f4b1 --- /dev/null +++ b/simgui.json @@ -0,0 +1,7 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + } +} diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 031cd46..5a2d29d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,7 +30,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); // private final LED m_robotLED = new LED(m_robotMap.LEDController); @@ -50,7 +50,7 @@ public class RobotContainer { () -> getDriverController().getLeftXAxis(), () -> getDriverController().getLeftYAxis(), () -> getDriverController().getRightXAxis(), - false)); + true)); // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); @@ -68,14 +68,16 @@ public class RobotContainer { /* Driver Buttons */ new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); // .onFalse() - /* Operator Buttons */ - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - + + /* Operator Buttons */ // interrupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .onTrue(new InstantCommand()); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d93f892..f35a557 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,20 +4,28 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { - public SwerveModule leftFront; - public SwerveModule rightFront; - public SwerveModule leftBack; - public SwerveModule rightBack; + private SwerveModule leftFront; + private SwerveModule rightFront; + private SwerveModule leftBack; + private SwerveModule rightBack; private SwerveModule[] modules; @@ -26,27 +34,32 @@ public class SwerveDrive extends SubsystemBase { private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - // private SwerveDriveOdometry odometry = new SwerveDriveOdometry( - // kinematics, - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // } - // ); + private RobotGyro gyro; + + private SwerveDriveOdometry odometry; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) { + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; this.rightBack = rightBack; + this.gyro = gyro; + + this.odometry = new SwerveDriveOdometry( + kinematics, + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + } + ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } @@ -54,11 +67,23 @@ public class SwerveDrive extends SubsystemBase { // WPILib swerve drive example public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) + // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, Rotation2d.fromDegrees(-gyro.getRotation2d().getDegrees())) // : new ChassisSpeeds(xSpeed, ySpeed, rot) // ); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); + + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(0.3, 0, 0)); + + // // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); setModuleStates(states); + // modules[0].getDriveMotor().set(0.2); + // modules[1].getDriveMotor().set(0.2); + // modules[2].getDriveMotor().set(0.2); + // modules[3].getDriveMotor().set(0.2); + + // modules[0].getAngleMotor().set(TalonFXControlMode.Position, 1017); + // modules[1].getAngleMotor().set(TalonFXControlMode.Position, 509); + // modules[2].getAngleMotor().set(TalonFXControlMode.Position, 683); + // modules[3].getAngleMotor().set(TalonFXControlMode.Position, 1816); } /** @@ -74,62 +99,62 @@ public class SwerveDrive extends SubsystemBase { } } + public double getGyroAngle() { + return gyro.getAngle(); + } + + public void resetGyro() { + gyro.reset(); + setOdometry(getOdometry()); + } + /** * Updates the odometry of the SwerveDrive. */ - // public void updateOdometry() { - // odometry.update( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // } - // ); - // } + public void updateOdometry() { + odometry.update( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + } + ); + } /** * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - // public Pose2d getOdometry() { - // return odometry.getPoseMeters(); - // } + public Pose2d getOdometry() { + return odometry.getPoseMeters(); + } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - // public void setOdometry(Pose2d pose) { - // odometry.resetPosition( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // pose - // ); - // } + public void setOdometry(Pose2d pose) { + odometry.resetPosition( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + }, + pose + ); + } /** * Resets the odometry of the SwerveDrive to 0. - * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. + * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. */ - // public void resetOdometry() { - // odometry.resetPosition( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // new Pose2d() - // ); - // } + public void resetOdometry() { + setOdometry(new Pose2d()); + } public SwerveDriveKinematics getKinematics() { return this.kinematics; @@ -138,7 +163,27 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - // updateOdometry(); + updateOdometry(); + + // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); + + // SmartDashboard.putNumber("LF CC Angle", modules[0].getAngle().getDegrees()); + // SmartDashboard.putNumber("RF CC Angle", modules[1].getAngle().getDegrees()); + // SmartDashboard.putNumber("LB CC Angle", modules[2].getAngle().getDegrees()); + // SmartDashboard.putNumber("RB CC Angle", modules[3].getAngle().getDegrees()); + + SmartDashboard.putNumber("LF Vel", modules[0].getDriveVel()); + SmartDashboard.putNumber("RF Vel", modules[1].getDriveVel()); + SmartDashboard.putNumber("LB Vel", modules[2].getDriveVel()); + SmartDashboard.putNumber("RB Vel", modules[3].getDriveVel()); + + SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + + SmartDashboard.putNumber("LF Pos", modules[0].getDrivePos()); + SmartDashboard.putNumber("RF Pos", modules[1].getDrivePos()); + SmartDashboard.putNumber("LB Pos", modules[2].getDrivePos()); + SmartDashboard.putNumber("RB Pos", modules[3].getDrivePos()); + } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 4bbbefb..efdae40 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - public WPI_TalonFX driveMotor; - public WPI_TalonFX angleMotor; + private WPI_TalonFX driveMotor; + private WPI_TalonFX angleMotor; private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -44,6 +44,9 @@ public class SwerveModule extends SubsystemBase { angleMotor.configAllSettings(angleConfig); encoder.configMagnetOffset(offset); + + driveMotor.setSelectedSensorPosition(0); + driveMotor.config_kP(0, 0.2); } /** @@ -79,6 +82,18 @@ public class SwerveModule extends SubsystemBase { return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } + public double getAngularVel() { + return this.angleMotor.getSelectedSensorVelocity(); + } + + public double getDrivePos() { + return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + } + + public double getDriveVel() { + return this.driveMotor.getSelectedSensorVelocity(0); + } + public void stop() { driveMotor.set(0); angleMotor.set(0); @@ -117,19 +132,23 @@ public class SwerveModule extends SubsystemBase { SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); + Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative // calculate the new absolute position of the SwerveModule based on the difference in rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12; - driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); + // driveMotor.set(0.1); + // double angleCorrection = getAngularVel() * 2.69; + driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); } public void reset(double position) { From 5cfd6f971ae4ce0048b21695b55841a20be0682f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 4 Feb 2023 15:06:42 -0700 Subject: [PATCH 2/6] DRIVING WORKS FIELD RELATIVE NO PROBLEMS --- src/main/java/frc4388/robot/Constants.java | 11 ++- .../java/frc4388/robot/RobotContainer.java | 66 +++++++++------- src/main/java/frc4388/robot/RobotMap.java | 7 ++ .../frc4388/robot/commands/AutoBalance.java | 5 +- .../robot/commands/DriveWithInput.java | 75 ------------------- .../frc4388/robot/subsystems/SwerveDrive.java | 63 +++++++--------- .../robot/subsystems/SwerveModule.java | 7 +- .../controller/DeadbandedXboxController.java | 27 +++++++ 8 files changed, 115 insertions(+), 146 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveWithInput.java create mode 100644 src/main/java/frc4388/utility/controller/DeadbandedXboxController.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4b8c518..1b94ed1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,7 +24,7 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 2.0; + public static final double ROTATION_SPEED = -0.7; public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; @@ -65,7 +65,7 @@ public final class Constants { public static final int CANCODER_TICKS_PER_ROTATION = 4096; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value @@ -105,7 +105,7 @@ public final class Constants { } - public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value // dimensions public static final double WIDTH = 18.5; // TODO: find the actual value @@ -137,5 +137,10 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; + + public static final double LEFT_AXIS_DEADBAND = 0.1; + public static final double RIGHT_AXIS_DEADBAND = 0.6; + + public static final boolean SKEW_STICKS = true; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5a2d29d..4a03806 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,11 +10,12 @@ package frc4388.robot; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; -import frc4388.robot.commands.DriveWithInput; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -35,8 +36,11 @@ public class RobotContainer { /* Controllers */ - private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + // private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); + // private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -46,11 +50,9 @@ public class RobotContainer { /* Default Commands */ - m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, - () -> getDriverController().getLeftXAxis(), - () -> getDriverController().getLeftYAxis(), - () -> getDriverController().getRightXAxis(), - true)); + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true) + , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); @@ -67,20 +69,20 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); - // .onFalse() + // new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // // .onFalse() - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - /* Operator Buttons */ - // interrupt button - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .onTrue(new InstantCommand()); + // /* Operator Buttons */ + // // interrupt button + // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand()); } /** @@ -96,28 +98,36 @@ public class RobotContainer { /** * Add your docs here. */ - public IHandController getDriverController() { - return m_driverXbox; + // public IHandController getDriverController() { + // return m_driverXbox; + // } + + public DeadbandedXboxController getDeadbandedDriverController() { + return this.m_driverXbox; + } + + public DeadbandedXboxController getDeadbandedOperatorController() { + return this.m_operatorXbox; } /** * Add your docs here. */ - public IHandController getOperatorController() { - return m_operatorXbox; - } + // public IHandController getOperatorController() { + // return m_operatorXbox; + // } /** * Add your docs here. */ - public Joystick getOperatorJoystick() { - return m_operatorXbox.getJoyStick(); - } + // public Joystick getOperatorJoystick() { + // return m_operatorXbox.getJoyStick(); + // } /** * Add your docs here. */ - public Joystick getDriverJoystick() { - return m_driverXbox.getJoyStick(); - } + // public Joystick getDriverJoystick() { + // return m_driverXbox.getJoyStick(); + // } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 93a114d..9b5ae04 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,6 +7,7 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; @@ -113,6 +114,12 @@ public class RobotMap { rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // set neutral mode + leftFrontSteer.setNeutralMode(NeutralMode.Brake); + rightFrontSteer.setNeutralMode(NeutralMode.Brake); + leftBackSteer.setNeutralMode(NeutralMode.Brake); + rightBackSteer.setNeutralMode(NeutralMode.Brake); + // initialize SwerveModules this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 374de0a..ffccee4 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -5,6 +5,7 @@ package frc4388.robot.commands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; @@ -15,7 +16,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { /** Creates a new AutoBalanceTF2. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(1.0, 0, 0, 0); + super(0.6, 0, 0, 0); this.gyro = gyro; this.drive = drive; @@ -34,7 +35,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); if (Math.abs(gyro.getPitch()) < 3) out2 = 0; - drive.drive(out2, 0, 0, false); + drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); } @Override diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java deleted file mode 100644 index 3b4d638..0000000 --- a/src/main/java/frc4388/robot/commands/DriveWithInput.java +++ /dev/null @@ -1,75 +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 frc4388.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.SwerveDrive; - -public class DriveWithInput extends CommandBase { - - private final SwerveDrive swerve; - - private final Supplier xSpeed; - private final Supplier ySpeed; - private final Supplier rot; - private final boolean fieldRelative; - - private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; - - /** Creates a new DriveWithInput. */ - public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { - // Use addRequirements() here to declare subsystem dependencies. - this.swerve = swerve; - this.xSpeed = xSpeed; - this.ySpeed = ySpeed; - this.rot = rot; - this.fieldRelative = fieldRelative; - - this.xLimiter = new SlewRateLimiter(3); - this.yLimiter = new SlewRateLimiter(3); - this.rotLimiter = new SlewRateLimiter(3); - - addRequirements(swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double x = xSpeed.get(); - double y = ySpeed.get(); - double r = rot.get(); - - x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI)); - - swerve.drive(x, y, r, fieldRelative); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - System.out.println("----------------------------------------------------------------"); - System.out.println("DriveWithInput ended"); - System.out.println("Interrupted: " + interrupted); - System.out.println("----------------------------------------------------------------"); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f35a557..38e7415 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,7 +4,6 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -41,6 +40,8 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveOdometry odometry; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + public Rotation2d rotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { @@ -64,26 +65,31 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - // WPILib swerve drive example - public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, Rotation2d.fromDegrees(-gyro.getRotation2d().getDegrees())) - // : new ChassisSpeeds(xSpeed, ySpeed, rot) - // ); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(0.3, 0, 0)); + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (fieldRelative) { - // // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); - setModuleStates(states); - // modules[0].getDriveMotor().set(0.2); - // modules[1].getDriveMotor().set(0.2); - // modules[2].getDriveMotor().set(0.2); - // modules[3].getDriveMotor().set(0.2); + if (rightStick.getNorm() > 0.1) { + rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); + } - // modules[0].getAngleMotor().set(TalonFXControlMode.Position, 1017); - // modules[1].getAngleMotor().set(TalonFXControlMode.Position, 509); - // modules[2].getAngleMotor().set(TalonFXControlMode.Position, 683); - // modules[3].getAngleMotor().set(TalonFXControlMode.Position, 1816); + double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + + + // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + + if (rightStick.getNorm() < 0.1) { + rot = 0; + } + + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + + // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } /** @@ -91,11 +97,11 @@ public class SwerveDrive extends SubsystemBase { * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); + module.setDesiredState(state, false); } } @@ -106,6 +112,7 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); setOdometry(getOdometry()); + rotTarget = new Rotation2d(0); } /** @@ -166,24 +173,8 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - - // SmartDashboard.putNumber("LF CC Angle", modules[0].getAngle().getDegrees()); - // SmartDashboard.putNumber("RF CC Angle", modules[1].getAngle().getDegrees()); - // SmartDashboard.putNumber("LB CC Angle", modules[2].getAngle().getDegrees()); - // SmartDashboard.putNumber("RB CC Angle", modules[3].getAngle().getDegrees()); - - SmartDashboard.putNumber("LF Vel", modules[0].getDriveVel()); - SmartDashboard.putNumber("RF Vel", modules[1].getDriveVel()); - SmartDashboard.putNumber("LB Vel", modules[2].getDriveVel()); - SmartDashboard.putNumber("RB Vel", modules[3].getDriveVel()); SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); - - SmartDashboard.putNumber("LF Pos", modules[0].getDrivePos()); - SmartDashboard.putNumber("RF Pos", modules[1].getDrivePos()); - SmartDashboard.putNumber("LB Pos", modules[2].getDrivePos()); - SmartDashboard.putNumber("RB Pos", modules[3].getDrivePos()); - } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index efdae40..2616b71 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -126,7 +126,7 @@ public class SwerveModule extends SubsystemBase { * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module */ - public void setDesiredState(SwerveModuleState desiredState) { + public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = this.getAngle(); SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); @@ -139,7 +139,10 @@ public class SwerveModule extends SubsystemBase { // convert the CANCoder from its position reading to ticks double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + + if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + } double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12; diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java new file mode 100644 index 0000000..bad1605 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -0,0 +1,27 @@ +package frc4388.utility.controller; + +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; +import frc4388.robot.Constants.OIConstants; + +public class DeadbandedXboxController extends XboxController { + public DeadbandedXboxController(int port) { super(port); } + + @Override public double getLeftX() { return getLeft().getX(); } + @Override public double getLeftY() { return getLeft().getY(); } + @Override public double getRightX() { return getRight().getX(); } + @Override public double getRightY() { return getRight().getY(); } + + public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } + public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } + + public static Translation2d skewToDeadzonedCircle(double x, double y) { + Translation2d translation2d = new Translation2d(x, y); + double magnitude = translation2d.getNorm(); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + return translation2d; + } +} \ No newline at end of file From 9c7b35f8f9a9b1a9a0e3eb3f92ea6bdfa683cc5f Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Mon, 6 Feb 2023 19:12:52 -0700 Subject: [PATCH 3/6] hnuho --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4a03806..97f5db1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -72,8 +72,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - // new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) From f7005093bb5528484b23f7d843a633b9df3c2a1d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 6 Feb 2023 19:47:14 -0700 Subject: [PATCH 4/6] printed odometry to smartdashboard --- src/main/java/frc4388/robot/RobotContainer.java | 2 -- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 7 ++++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97f5db1..a3690d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -55,8 +55,6 @@ public class RobotContainer { , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - - } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 38e7415..a15f6ef 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -86,6 +86,7 @@ public class SwerveDrive extends SubsystemBase { // Convert field-relative speeds to robot-relative speeds. chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -172,9 +173,9 @@ public class SwerveDrive extends SubsystemBase { // This method will be called once per scheduler run updateOdometry(); - // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); + SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); + SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); } /** From aa42893b3bcdb419c604fa70288f753f1b8f8f99 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 7 Feb 2023 17:53:32 -0700 Subject: [PATCH 5/6] fixed gear ratios --- src/main/java/frc4388/robot/Constants.java | 10 +++++----- .../java/frc4388/robot/subsystems/SwerveDrive.java | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1b94ed1..efe227b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,11 +67,11 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value - public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value - public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value + public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; @@ -108,8 +108,8 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value // dimensions - public static final double WIDTH = 18.5; // TODO: find the actual value - public static final double HEIGHT = 18.5; // TODO: find the actual value + public static final double WIDTH = 18.5; + public static final double HEIGHT = 18.5; public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a15f6ef..a8a3c9f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -176,6 +176,7 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); } /** From f7ba8176e08f397f11bbfe5009140ecef6564a94 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 7 Feb 2023 18:54:09 -0700 Subject: [PATCH 6/6] drift fix --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a8a3c9f..17bdd5b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -79,9 +79,9 @@ public class SwerveDrive extends SubsystemBase { // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - if (rightStick.getNorm() < 0.1) { - rot = 0; - } + // if (rightStick.getNorm() < .1) { + // rot = 0; + // } // Convert field-relative speeds to robot-relative speeds. chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); @@ -176,7 +176,9 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); } /**