Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

swerve shtuff

Browse Source
This commit is contained in:
aarav18
2023-01-20 21:04:58 -07:00
parent 6b631e914d
commit bf4088d9db
6 changed files with 150 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+30 -22
View File
@@ -24,20 +24,21 @@ import frc4388.utility.LEDPatterns;
public final class Constants { public final class Constants {
public static final class SwerveDriveConstants { public static final class SwerveDriveConstants {
public static final class IDs { public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID public static final int LEFT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID public static final int LEFT_FRONT_STEER_ID = 3;
public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find the actual ID public static final int LEFT_FRONT_ENCODER_ID = 10;
public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find the actual ID
public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find the actual ID public static final int RIGHT_FRONT_WHEEL_ID = 4;
public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find the actual ID public static final int RIGHT_FRONT_STEER_ID = 5;
public static final int LEFT_BACK_STEER_ID = -1; // TODO: find the actual ID public static final int RIGHT_FRONT_ENCODER_ID = 11;
public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find the actual ID public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13;
} }
public static final class PIDConstants { public static final class PIDConstants {
@@ -63,7 +64,9 @@ 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_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 = 2.0; // TODO: find the actual value
public static final double MOTOR_REV_PER_WHEEL_REV = -1; // 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 TICKS_PER_MOTOR_REV = 2048; 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 = 4.0; // TODO: the actual value
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
@@ -82,17 +85,22 @@ public final class Constants {
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value 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 NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
public static final double LEFT_FRONT_ENCODER_OFFSET = -1.0; // 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 = -1.0; // 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 = -1.0; // 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 = -1.0; // 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.; // 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 = -1; // TODO: find the actual value public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
// dimensions // dimensions
public static final double WIDTH = -1; // TODO: find the actual value public static final double WIDTH = 18.5; // TODO: find the actual value
public static final double HEIGHT = -1; // TODO: find the actual value public static final double HEIGHT = 18.5; // TODO: find the actual value
public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d;
@@ -102,11 +110,11 @@ public final class Constants {
} }
public static final class GyroConstants { public static final class GyroConstants {
public static final int ID = -1; // TODO: find the actual ID public static final int ID = 14; // TODO: find the actual ID
} }
public static final class LEDConstants { public static final class LEDConstants {
public static final int LED_SPARK_ID = 0; // public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
} }
src/main/java/frc4388/robot/Robot.java
+6 -6
View File
@@ -60,7 +60,7 @@ public class Robot extends TimedRobot {
} }
} }
private MicroBot bot = new MicroBot(); // private MicroBot bot = new MicroBot();
/** /**
* This function is run when the robot is first started up and should be * This function is run when the robot is first started up and should be
@@ -72,7 +72,7 @@ public class Robot extends TimedRobot {
// autonomous chooser on the dashboard. // autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer(); m_robotContainer = new RobotContainer();
bot.setDefaultCommand(new AutoBalance(bot)); // bot.setDefaultCommand(new AutoBalance(bot));
} }
/** /**
@@ -146,7 +146,7 @@ public class Robot extends TimedRobot {
} }
m_robotTime.startMatchTime(); m_robotTime.startMatchTime();
m_robotContainer.gyroRef.reset(); // m_robotContainer.gyroRef.reset();
} }
/** /**
@@ -154,9 +154,9 @@ public class Robot extends TimedRobot {
*/ */
@Override @Override
public void teleopPeriodic() { public void teleopPeriodic() {
SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
} }
/** /**
src/main/java/frc4388/robot/RobotContainer.java
+19 -12
View File
@@ -32,15 +32,15 @@ public class RobotContainer {
private final RobotMap m_robotMap = new RobotMap(); private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); private 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); // private final LED m_robotLED = new LED(m_robotMap.LEDController);
/* Controllers */ /* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_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 XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
public RobotGyro gyroRef = m_robotMap.gyro; // public RobotGyro gyroRef = m_robotMap.gyro;
/** /**
* The container for the robot. Contains subsystems, OI devices, and commands. * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -51,13 +51,20 @@ public class RobotContainer {
/* Default Commands */ /* Default Commands */
// drives the robot with a two-axis input from the driver controller // drives the robot with a two-axis input from the driver controller
// continually sends updates to the Blinkin LED controller to keep the lights on // continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
m_robotSwerveDrive.setDefaultCommand( m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-0.3*getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis(), 0.3*getDriverController().getLeftYAxis(),
-getDriverController().getRightXAxis(), false), m_robotSwerveDrive) -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)
// );
} }
/** /**
@@ -69,14 +76,14 @@ public class RobotContainer {
private void configureButtonBindings() { private void configureButtonBindings() {
/* Driver Buttons */ /* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> gyroRef.reset())); // .onTrue(new InstantCommand(() -> gyroRef.reset()));
/* Operator Buttons */ /* Operator Buttons */
// activates "Lit Mode" // activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED)) // .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
.whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED)); // .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
//New interupt button //New interupt button
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
src/main/java/frc4388/robot/RobotMap.java
+11 -11
View File
@@ -22,8 +22,8 @@ import frc4388.utility.RobotGyro;
* testing and modularization. * testing and modularization.
*/ */
public class RobotMap { public class RobotMap {
private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
public RobotGyro gyro = new RobotGyro(m_pigeon2); // public RobotGyro gyro = new RobotGyro(m_pigeon2);
public SwerveModule leftFront; public SwerveModule leftFront;
@@ -38,7 +38,7 @@ public class RobotMap {
} }
/* LED Subsystem */ /* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() { void configureLEDMotorControllers() {
@@ -103,17 +103,17 @@ public class RobotMap {
rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config neutral deadband // config neutral deadband
leftFrontSteer.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); // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.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); // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.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); // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.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); // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// config magnet offset // config magnet offset
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET); leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+81 -61
View File
@@ -18,14 +18,14 @@ import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase { public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront; public SwerveModule leftFront;
private SwerveModule rightFront; public SwerveModule rightFront;
private SwerveModule leftBack; public SwerveModule leftBack;
private SwerveModule rightBack; public SwerveModule rightBack;
private SwerveModule[] modules; private SwerveModule[] modules;
private RobotGyro gyro; // private RobotGyro gyro;
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
@@ -34,21 +34,21 @@ public class SwerveDrive extends SubsystemBase {
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private SwerveDriveOdometry odometry = new SwerveDriveOdometry( // private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
kinematics, // kinematics,
gyro.getRotation2d(), // gyro.getRotation2d(),
new SwerveModulePosition[] { // new SwerveModulePosition[] {
leftFront.getPosition(), // leftFront.getPosition(),
rightFront.getPosition(), // rightFront.getPosition(),
leftBack.getPosition(), // leftBack.getPosition(),
rightBack.getPosition() // rightBack.getPosition()
} // }
); // );
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
/** Creates a new SwerveDrive. */ /** 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.leftFront = leftFront;
this.rightFront = rightFront; this.rightFront = rightFront;
this.leftBack = leftBack; this.leftBack = leftBack;
@@ -56,17 +56,23 @@ public class SwerveDrive extends SubsystemBase {
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
this.gyro = gyro; for (SwerveModule m : this.modules) {
m.reset();
}
// this.gyro = gyro;
} }
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; double xSpeedMetersPerSecond = xSpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; double ySpeedMetersPerSecond = ySpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveModuleState[] states = kinematics.toSwerveModuleStates( // SwerveModuleState[] states = kinematics.toSwerveModuleStates(
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
); //);
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
setModuleStates(states); setModuleStates(states);
@@ -87,64 +93,78 @@ public class SwerveDrive extends SubsystemBase {
/** /**
* Updates the odometry of the SwerveDrive. * Updates the odometry of the SwerveDrive.
*/ */
public void updateOdometry() { // public void updateOdometry() {
odometry.update( // odometry.update(
gyro.getRotation2d(), // gyro.getRotation2d(),
new SwerveModulePosition[] { // new SwerveModulePosition[] {
leftFront.getPosition(), // leftFront.getPosition(),
rightFront.getPosition(), // rightFront.getPosition(),
leftBack.getPosition(), // leftBack.getPosition(),
rightBack.getPosition() // rightBack.getPosition()
} // }
); // );
} // }
/** /**
* Gets the odometry of the SwerveDrive. * Gets the odometry of the SwerveDrive.
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
*/ */
public Pose2d getOdometry() { // public Pose2d getOdometry() {
return odometry.getPoseMeters(); // return odometry.getPoseMeters();
} // }
/** /**
* Sets the odometry of the SwerveDrive. * Sets the odometry of the SwerveDrive.
* @param pose Pose to set the odometry to. * @param pose Pose to set the odometry to.
*/ */
public void setOdometry(Pose2d pose) { // public void setOdometry(Pose2d pose) {
odometry.resetPosition( // odometry.resetPosition(
gyro.getRotation2d(), // gyro.getRotation2d(),
new SwerveModulePosition[] { // new SwerveModulePosition[] {
leftFront.getPosition(), // leftFront.getPosition(),
rightFront.getPosition(), // rightFront.getPosition(),
leftBack.getPosition(), // leftBack.getPosition(),
rightBack.getPosition() // rightBack.getPosition()
}, // },
pose // pose
); // );
} // }
/** /**
* Resets the odometry of the SwerveDrive to 0. * 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 gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
*/ */
public void resetOdometry() { // public void resetOdometry() {
odometry.resetPosition( // odometry.resetPosition(
gyro.getRotation2d(), // gyro.getRotation2d(),
new SwerveModulePosition[] { // new SwerveModulePosition[] {
leftFront.getPosition(), // leftFront.getPosition(),
rightFront.getPosition(), // rightFront.getPosition(),
leftBack.getPosition(), // leftBack.getPosition(),
rightBack.getPosition() // rightBack.getPosition()
}, // },
new Pose2d() // new Pose2d()
); // );
// }
public void runAllDriveMotors(double output) {
this.leftFront.driveMotor.set(output);
this.rightFront.driveMotor.set(output);
this.leftBack.driveMotor.set(output);
this.rightBack.driveMotor.set(output);
}
public void runAllSteerMotors(double output) {
this.leftFront.angleMotor.set(output);
this.rightFront.angleMotor.set(output);
this.leftBack.angleMotor.set(output);
this.rightBack.angleMotor.set(output);
} }
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run
updateOdometry(); // updateOdometry();
} }
/** /**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+2 -2
View File
@@ -21,8 +21,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains; import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase { public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor; public WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor; public WPI_TalonFX angleMotor;
private CANCoder canCoder; private CANCoder canCoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;