mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 08:38:02 -06:00
offsets (INCOMPLETE DO NOT TOUCH)
This commit is contained in:
aarav18
@@ -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
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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() {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user