Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-08 16:28:00 -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 d0b94e7e94
commit 3697360963
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 static final class SwerveDriveConstants {
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID
public static final int RIGHT_FRONT_WHEEL_ID = -1; // TODO: find the actual ID
public static final int LEFT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
public static final int RIGHT_BACK_STEER_ID = -1; // TODO: find the actual ID
public static final int LEFT_FRONT_WHEEL_ID = 2;
public static final int LEFT_FRONT_STEER_ID = 3;
public static final int LEFT_FRONT_ENCODER_ID = 10;
public static final int LEFT_FRONT_STEER_ID = -1; // TODO: find the actual ID
public static final int RIGHT_FRONT_STEER_ID = -1; // TODO: find the actual ID
public static final int LEFT_BACK_STEER_ID = -1; // TODO: find the actual ID
public static final int RIGHT_BACK_WHEEL_ID = -1; // TODO: find the actual ID
public static final int RIGHT_FRONT_WHEEL_ID = 4;
public static final int RIGHT_FRONT_STEER_ID = 5;
public static final int RIGHT_FRONT_ENCODER_ID = 11;
public static final int LEFT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID
public static final int RIGHT_FRONT_ENCODER_ID = -1; // TODO: find the actual ID
public static final int LEFT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
public static final int RIGHT_BACK_ENCODER_ID = -1; // TODO: find the actual ID
public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12;
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 {
@@ -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_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 WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
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 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 RIGHT_FRONT_ENCODER_OFFSET = -1.0; // TODO: find the actual value
public static final double LEFT_BACK_ENCODER_OFFSET = -1.0; // TODO: find the actual value
public static final double RIGHT_BACK_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 = (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.; // 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
public static final double WIDTH = -1; // TODO: find the actual value
public static final double HEIGHT = -1; // TODO: find the actual value
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 HALF_WIDTH = WIDTH / 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 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 int LED_SPARK_ID = 0;
// public static final int LED_SPARK_ID = 0;
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
@@ -72,7 +72,7 @@ public class Robot extends TimedRobot {
// autonomous chooser on the dashboard.
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_robotContainer.gyroRef.reset();
// m_robotContainer.gyroRef.reset();
}
/**
@@ -154,9 +154,9 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
// SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
// SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
// 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();
/* 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 LED m_robotLED = new LED(m_robotMap.LEDController);
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);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_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.
@@ -51,13 +51,20 @@ public class RobotContainer {
/* Default Commands */
// 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
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis(),
-getDriverController().getRightXAxis(), false), m_robotSwerveDrive)
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)
// );
}
/**
@@ -69,14 +76,14 @@ public class RobotContainer {
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> gyroRef.reset()));
// new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> gyroRef.reset()));
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
.whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
// new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
// .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
// .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
//New interupt 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.
*/
public class RobotMap {
private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
public RobotGyro gyro = new RobotGyro(m_pigeon2);
// private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
public SwerveModule leftFront;
@@ -38,7 +38,7 @@ public class RobotMap {
}
/* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() {
@@ -103,17 +103,17 @@ 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);
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 {
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
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 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 SwerveDriveOdometry odometry = new SwerveDriveOdometry(
kinematics,
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
}
);
// private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
// kinematics,
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
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;
@@ -56,17 +56,23 @@ public class SwerveDrive extends SubsystemBase {
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) {
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
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;
SwerveModuleState[] states = kinematics.toSwerveModuleStates(
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
);
// 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));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
setModuleStates(states);
@@ -87,64 +93,78 @@ public class SwerveDrive extends SubsystemBase {
/**
* 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.
*/
public void resetOdometry() {
odometry.resetPosition(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
},
new Pose2d()
);
// public void resetOdometry() {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// 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
public void periodic() {
// This method will be called once per scheduler run
updateOdometry();
// updateOdometry();
}
/**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+3 -3
View File
@@ -21,8 +21,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
public WPI_TalonFX driveMotor;
public WPI_TalonFX angleMotor;
private CANCoder canCoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -78,7 +78,7 @@ public class SwerveModule extends SubsystemBase {
* @return the angle of a SwerveModule as a Rotation2d
*/
public Rotation2d getAngle() {
// Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
// Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}