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] 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) {