Skip to content

Commit

Permalink
Fixed teleop drive
Browse files Browse the repository at this point in the history
  • Loading branch information
@kidskoding
kidskoding committed Feb 3, 2024
1 parent 271d0c5 commit d84e268
Show file tree
Hide file tree
Showing 4 changed files with 26 additions and 26 deletions.
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,10 @@ public RobotContainer() {
drive =
new Drive(
new GyroIOPigeon2(),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
new ModuleIOSparkMax(3));
new ModuleIOSparkMax(3),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(2));
// flywheel = new Flywheel(new FlywheelIOSparkMax());
// drive = new Drive(
// new GyroIOPigeon2(),
Expand Down Expand Up @@ -132,9 +132,9 @@ private void configureButtonBindings() {
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -applyDeadband(controller.getLeftY()),
() -> -applyDeadband(controller.getLeftX()),
() -> -applyDeadband(controller.getRightX())));
() -> applyDeadband(controller.getLeftY()),
() -> applyDeadband(controller.getLeftX()),
() -> applyDeadband(controller.getRightX())));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ public static Command joystickDrive(
MathUtil.applyDeadband(
Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
Rotation2d linearDirection =
new Rotation2d(-xSupplier.getAsDouble(), ySupplier.getAsDouble());
new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);

// Square values
Expand All @@ -62,7 +62,7 @@ public static Command joystickDrive(
// Convert to field relative speeds & send command
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;
&& DriverStation.getAlliance().get() == Alliance.Blue;
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@

public class Drive extends SubsystemBase {
private static final double MAX_LINEAR_SPEED = Units.feetToMeters(7);
private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
private static final double TRACK_WIDTH_X = Units.inchesToMeters(21.0);
private static final double TRACK_WIDTH_Y = Units.inchesToMeters(21.0);
private static final double DRIVE_BASE_RADIUS =
Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
Expand Down Expand Up @@ -151,7 +151,7 @@ public void periodic() {
// Update gyro angle
if (gyroInputs.connected) {
// Use the real gyro angle
rawGyroRotation = gyroInputs.yawPosition;
rawGyroRotation = Rotation2d.fromRadians(Math.PI * 2).minus(gyroInputs.yawPosition);
} else {
// Use the angle delta from the kinematics and module deltas
Twist2d twist = kinematics.toTwist2d(moduleDeltas);
Expand Down Expand Up @@ -271,10 +271,10 @@ public double getMaxAngularSpeedRadPerSec() {
/** Returns an array of module translations. */
public static Translation2d[] getModuleTranslations() {
return new Translation2d[] {
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
};
}
}
24 changes: 12 additions & 12 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,32 +46,32 @@ public class ModuleIOSparkMax implements ModuleIO {
private final RelativeEncoder turnRelativeEncoder;
private final SparkAbsoluteEncoder turnAbsoluteEncoder;

private final boolean isTurnMotorInverted = false;
private final boolean isTurnMotorInverted = true;
private final Rotation2d absoluteEncoderOffset;

public ModuleIOSparkMax(int index) {
switch (index) {
case 0:
driveSparkMax = new CANSparkMax(21, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(11, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(20, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(10, MotorType.kBrushless);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
absoluteEncoderOffset = new Rotation2d(0); // MUST BE CALIBRATED
absoluteEncoderOffset = new Rotation2d(Math.PI); // MUST BE CALIBRATED
break;
case 1:
driveSparkMax = new CANSparkMax(20, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(10, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(21, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(11, MotorType.kBrushless);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
absoluteEncoderOffset = new Rotation2d(0); // MUST BE CALIBRATED
absoluteEncoderOffset = new Rotation2d(Math.PI); // MUST BE CALIBRATED
break;
case 2:
driveSparkMax = new CANSparkMax(23, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(13, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(22, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(12, MotorType.kBrushless);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
absoluteEncoderOffset = new Rotation2d(Math.PI); // MUST BE CALIBRATED
break;
case 3:
driveSparkMax = new CANSparkMax(22, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(12, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(23, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(13, MotorType.kBrushless);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
absoluteEncoderOffset = new Rotation2d(0); // MUST BE CALIBRATED
break;
Expand Down Expand Up @@ -119,7 +119,7 @@ public void updateInputs(ModuleIOInputs inputs) {
inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()};

inputs.turnAbsolutePosition =
new Rotation2d(turnAbsoluteEncoder.getPosition() * 2.0 * Math.PI)
new Rotation2d((1 - turnAbsoluteEncoder.getPosition()) * 2.0 * Math.PI)
.minus(absoluteEncoderOffset);
inputs.turnPosition =
Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO);
Expand Down

0 comments on commit d84e268

Please sign in to comment.