mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 00:38:02 -06:00
add gear shifts
This commit is contained in:
C4llSiqn
@@ -34,35 +34,37 @@ public final class Constants {
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
|
||||
public static final double[] GEARS = {0.25, 0.5, 1.0};
|
||||
|
||||
public static final double SLOW_SPEED = 0.25;
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
public static final class DefaultSwerveRotOffsets {
|
||||
public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 0.5;
|
||||
public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 +0.5 ;//+ 0.5;
|
||||
public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875+0.5 ;
|
||||
public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 +0.5 ;
|
||||
public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 + 0.5;
|
||||
public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5;
|
||||
public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5;
|
||||
public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5;
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
|
||||
public static final int RIGHT_FRONT_WHEEL_ID = 2;
|
||||
public static final int RIGHT_FRONT_STEER_ID = 3;
|
||||
public static final int RIGHT_FRONT_ENCODER_ID = 10;
|
||||
|
||||
public static final int LEFT_FRONT_WHEEL_ID = 4;
|
||||
public static final int LEFT_FRONT_STEER_ID = 5;
|
||||
public static final int LEFT_FRONT_ENCODER_ID = 11;
|
||||
|
||||
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 int RIGHT_FRONT_WHEEL_ID = 2;
|
||||
public static final int RIGHT_FRONT_STEER_ID = 3;
|
||||
public static final int RIGHT_FRONT_ENCODER_ID = 10;
|
||||
|
||||
public static final int LEFT_FRONT_WHEEL_ID = 4;
|
||||
public static final int LEFT_FRONT_STEER_ID = 5;
|
||||
public static final int LEFT_FRONT_ENCODER_ID = 11;
|
||||
|
||||
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 {
|
||||
@@ -85,8 +87,6 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class Conversions {
|
||||
public static final int CANCODER_TICKS_PER_ROTATION = 1;
|
||||
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5;
|
||||
|
||||
@@ -107,13 +107,13 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_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 OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; // TODO: find the actual value
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
@@ -127,23 +127,6 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
// public static final String NAME = "photonCamera";
|
||||
|
||||
// public static final int LIME_HIXELS = 640;
|
||||
// public static final int LIME_VIXELS = 480;
|
||||
|
||||
// public static final double H_FOV = 59.6;
|
||||
// public static final double V_FOV = 45.7;
|
||||
|
||||
// public static final double LIME_HEIGHT = 6.0;
|
||||
// public static final double LIME_ANGLE = 55.0;
|
||||
|
||||
// // public static final double HIGH_TARGET_HEIGHT = 46.0;
|
||||
// public static final double HIGH_TAPE_HEIGHT = 44.0;
|
||||
|
||||
// // public static final double MID_TARGET_HEIGHT = 34.0;
|
||||
// public static final double MID_TAPE_HEIGHT = 24.0;
|
||||
|
||||
// public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
|
||||
|
||||
public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609);
|
||||
@@ -153,11 +136,6 @@ public final class Constants {
|
||||
public static final double targetPosDistance = 1.5;
|
||||
|
||||
}
|
||||
|
||||
public static final class AutoAlignConstants {
|
||||
public static final double MoveSpeed = 0.0;
|
||||
public static final double RotSpeed = 0.0;
|
||||
}
|
||||
|
||||
public static final class DriveConstants {
|
||||
public static final int DRIVE_PIGEON_ID = 14;
|
||||
@@ -202,6 +180,7 @@ public final class Constants {
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final int XBOX_PROGRAMMER_ID = 2;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
|
||||
}
|
||||
|
||||
@@ -69,7 +69,7 @@ public class RobotContainer {
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
|
||||
|
||||
private final Limelight limelight = new Limelight();
|
||||
|
||||
@@ -77,7 +77,6 @@ public class RobotContainer {
|
||||
|
||||
private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor);
|
||||
|
||||
|
||||
/* Virtual Controllers */
|
||||
private final VirtualController m_virtualDriver = new VirtualController(0);
|
||||
private final VirtualController m_virtualOperator = new VirtualController(1);
|
||||
@@ -123,9 +122,9 @@ public class RobotContainer {
|
||||
private String lastAutoName = "four_note_taxi_kracken.auto";
|
||||
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
|
||||
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
lastAutoName, // () -> autoplaybackName.get(), // lastAutoName
|
||||
() -> autoplaybackName.get(), // lastAutoName
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, true);
|
||||
true, false);
|
||||
|
||||
private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto",
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
@@ -195,11 +194,10 @@ public class RobotContainer {
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
|
||||
@@ -207,7 +205,6 @@ public class RobotContainer {
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
|
||||
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
|
||||
|
||||
@@ -33,10 +33,13 @@ public class SwerveDrive extends SubsystemBase {
|
||||
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
||||
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
private int gear_index;
|
||||
private boolean stopped = false;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
|
||||
@@ -52,7 +55,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.gyro = gyro;
|
||||
|
||||
reset_index();
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
@@ -68,7 +71,6 @@ public class SwerveDrive extends SubsystemBase {
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
|
||||
boolean stopped = false;
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
|
||||
@@ -256,15 +258,29 @@ public class SwerveDrive extends SubsystemBase {
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
|
||||
|
||||
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
private void reset_index() {
|
||||
gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
|
||||
}
|
||||
|
||||
}
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1) i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
@@ -294,16 +310,6 @@ public class SwerveDrive extends SubsystemBase {
|
||||
System.out.println("TURBO");
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
|
||||
} else {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void toggleGear(double angle) {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
|
||||
Reference in New Issue
Block a user