From 56ad89a9747abe8d3b9d8f98a6cb987bbc43b366 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 21 Jan 2023 15:16:22 -0700 Subject: [PATCH] offsets (INCOMPLETE DO NOT TOUCH) --- src/main/java/frc4388/robot/Constants.java | 25 +++++--- .../java/frc4388/robot/RobotContainer.java | 32 +++++++--- src/main/java/frc4388/robot/RobotMap.java | 37 +++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 59 +++++++++++++++---- .../robot/subsystems/SwerveModule.java | 28 +++++---- .../java/frc4388/utility/RobotEncoder.java | 23 ++++++++ 6 files changed, 149 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc4388/utility/RobotEncoder.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d5774a1..3862e4f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,6 +23,9 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { + + public static final double ROTATION_SPEED = 2.0; + public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; public static final int LEFT_FRONT_STEER_ID = 3; @@ -85,15 +88,21 @@ public final class Constants { public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value - // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // TODO: find the actual value - // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // TODO: find the actual value - // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // TODO: find the actual value - // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // TODO: find the actual value + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values + + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work) + + // public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work) + // public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work) + // public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work) + // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work) - public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // TODO: find the actual value - public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // TODO: find the actual value - public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // TODO: find the actual value - public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // TODO: find the actual value } public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 03dcb0a..60379ea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -53,18 +53,26 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), - 0.3*getDriverController().getLeftYAxis(), - -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) - ); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(), + // 0.3*getDriverController().getLeftYAxis(), + // -0.3*getDriverController().getRightXAxis(), false), m_robotSwerveDrive) + // ); // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.runAllSteerMotors(0.2*getDriverController().getLeftYAxis()), m_robotSwerveDrive) - // ); - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.leftBack.angleMotor.set(0.2*getDriverController().getRightYAxis()), m_robotSwerveDrive) + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, + // -0.1, + // 0, false), m_robotSwerveDrive) // ); + + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + -0.3 * getDriverController().getLeftXAxis(), + 0.3 * getDriverController().getLeftYAxis(), + 0.3 * getDriverController().getRightXAxis(), + 0.3 * getDriverController().getRightYAxis(), + true), + m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); } /** @@ -85,6 +93,12 @@ public class RobotContainer { // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive)); + //New interupt button new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .onTrue(new InstantCommand()); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 176e881..040cf4b 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; +import frc4388.utility.RobotEncoder; import frc4388.utility.RobotGyro; /** @@ -48,18 +49,22 @@ public class RobotMap { public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + //public final RobotEncoder leftFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID, 268.900); public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + //public final RobotEncoder rightFrontEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID, 266.700); public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + //public final RobotEncoder leftBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID, 268.285); public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + //public final RobotEncoder rightBackEncoder = new RobotEncoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID, 86.965); void configureDriveMotors() { @@ -103,29 +108,29 @@ public class RobotMap { rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // config neutral deadband - // leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // config magnet offset - leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - leftBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - rightBackEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + // leftFrontEncoder.configMagnetOffset(90.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + // rightFrontEncoder.configMagnetOffset(0.0); //180.0); //270);//271.58203125);//SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + // leftBackEncoder.configMagnetOffset(23.99414); //0.0); //90.0);//92.98828125);//SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + // rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4b3333f..b066f9c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; 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; @@ -12,7 +13,9 @@ 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.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; @@ -55,26 +58,43 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - - for (SwerveModule m : this.modules) { - m.reset(); - } - - // this.gyro = gyro; } public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - double xSpeedMetersPerSecond = xSpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - double ySpeedMetersPerSecond = ySpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - + + Translation2d speed = new Translation2d(-xSpeed, ySpeed); + double mag = speed.getNorm(); + speed = speed.times(mag * speedAdjust); + + double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // SwerveModuleState[] states = kinematics.toSwerveModuleStates( // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) //); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); - SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + setModuleStates(states); + } + + public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { + // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; + Translation2d speed = new Translation2d(-leftX, leftY); + speed = speed.times(speed.getNorm() * speedAdjust); + // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + double xSpeedMetersPerSecond = -speed.getX(); + double ySpeedMetersPerSecond = speed.getY(); + // chassisSpeeds = fieldRelative + // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + + SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -83,6 +103,7 @@ 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)); for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; @@ -161,10 +182,26 @@ public class SwerveDrive extends SubsystemBase { this.rightBack.angleMotor.set(output); } + public void rotateCANCodersToAngle(double angle) { + for (SwerveModule module : this.modules) { + module.rotateToAngle(angle); + } + } + + public void resetCANCoders(double position) { + for (SwerveModule module : this.modules) { + module.reset(position); + } + } + @Override public void periodic() { // This method will be called once per scheduler run // updateOdometry(); + SmartDashboard.putNumber("LeftFront CC", this.modules[0].getAngle().getDegrees()); + SmartDashboard.putNumber("RightFront CC", this.modules[1].getAngle().getDegrees()); + SmartDashboard.putNumber("LeftBack CC", this.modules[2].getAngle().getDegrees()); + SmartDashboard.putNumber("RightBack CC", this.modules[3].getAngle().getDegrees()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 4988964..dff4429 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -19,19 +19,21 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.RobotEncoder; public class SwerveModule extends SubsystemBase { public WPI_TalonFX driveMotor; public WPI_TalonFX angleMotor; - private CANCoder canCoder; + // private CANCoder canCoder; + private CANCoder encoder; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, /*CANCoder canCoder*/CANCoder encoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; - this.canCoder = canCoder; + this.encoder = encoder; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); angleConfig.slot0.kP = swerveGains.kP; @@ -39,14 +41,16 @@ public class SwerveModule extends SubsystemBase { angleConfig.slot0.kD = swerveGains.kD; // use the CANcoder as the remote sensor for the primary TalonFX PID - angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID(); + angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleConfig); CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); canCoderConfig.magnetOffsetDegrees = offset; - canCoder.configAllSettings(canCoderConfig); + encoder.configAllSettings(canCoderConfig); + + // canCoderConfig.magnetOffsetDegrees = 270; } /** @@ -70,7 +74,7 @@ public class SwerveModule extends SubsystemBase { * @return the CANcoder of the SwerveModule */ public CANCoder getEncoder() { - return this.canCoder; + return this.encoder; } /** @@ -79,7 +83,7 @@ public class SwerveModule extends SubsystemBase { */ public Rotation2d getAngle() { // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); + return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); } public void stop() { @@ -126,15 +130,17 @@ public class SwerveModule extends SubsystemBase { double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); } - public void reset() { - canCoder.setPositionToAbsolute(); + public void reset(double position) { + // encoder.setPosition(position); + encoder.setPositionToAbsolute(); + // canCoder.setPosition(1024); } public double getCurrent() { diff --git a/src/main/java/frc4388/utility/RobotEncoder.java b/src/main/java/frc4388/utility/RobotEncoder.java new file mode 100644 index 0000000..2e397ac --- /dev/null +++ b/src/main/java/frc4388/utility/RobotEncoder.java @@ -0,0 +1,23 @@ +// 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.utility; + +import com.ctre.phoenix.sensors.CANCoder; + +/** Add your docs here. */ +public class RobotEncoder extends CANCoder { + private double offset; + + public RobotEncoder(int id, double offset) { + super(id); + + this.offset = offset; + } + + @Override + public double getAbsolutePosition() { + return super.getAbsolutePosition() - this.offset; + } +}