Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

add gear shifts

Browse Source
This commit is contained in:
C4llSiqn
2024-10-26 11:10:05 -06:00
parent e9dcabce62
commit eaf60aa475
3 changed files with 61 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+28 -49
View File
@@ -34,35 +34,37 @@ public final class Constants {
public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50; 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 SLOW_SPEED = 0.25;
public static final double FAST_SPEED = 0.5; public static final double FAST_SPEED = 0.5;
public static final double TURBO_SPEED = 1.0; public static final double TURBO_SPEED = 1.0;
public static final class DefaultSwerveRotOffsets { public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 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 ;//+ 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_LEFT_ROT_OFFSET = -0.227294921875 + 0.5;
public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 +0.5 ; public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5;
} }
public static final class IDs { public static final class IDs {
public static final int RIGHT_FRONT_WHEEL_ID = 2; public static final int RIGHT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_STEER_ID = 3; public static final int RIGHT_FRONT_STEER_ID = 3;
public static final int RIGHT_FRONT_ENCODER_ID = 10; public static final int RIGHT_FRONT_ENCODER_ID = 10;
public static final int LEFT_FRONT_WHEEL_ID = 4; public static final int LEFT_FRONT_WHEEL_ID = 4;
public static final int LEFT_FRONT_STEER_ID = 5; public static final int LEFT_FRONT_STEER_ID = 5;
public static final int LEFT_FRONT_ENCODER_ID = 11; public static final int LEFT_FRONT_ENCODER_ID = 11;
public static final int LEFT_BACK_WHEEL_ID = 6; public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7; public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12; public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_WHEEL_ID = 8; public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9; public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13; public static final int RIGHT_BACK_ENCODER_ID = 13;
} }
public static final class PIDConstants { public static final class PIDConstants {
@@ -85,8 +87,6 @@ public final class Constants {
} }
public static final class Conversions { 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_FAST = 6.22;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; 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 class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // 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; // TODO: find the actual value public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value 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_SPEED_FEET_PER_SECOND = 20.4;
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
// dimensions // dimensions
public static final double WIDTH = 18.5; public static final double WIDTH = 18.5;
@@ -127,23 +127,6 @@ public final class Constants {
} }
public static final class VisionConstants { 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 double APRIL_HEIGHT = -1.0; // TODO: find actual value
public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); 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 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 class DriveConstants {
public static final int DRIVE_PIGEON_ID = 14; public static final int DRIVE_PIGEON_ID = 14;
@@ -202,6 +180,7 @@ public final class Constants {
public static final class OIConstants { public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1; 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; public static final double LEFT_AXIS_DEADBAND = 0.1;
} }
src/main/java/frc4388/robot/RobotContainer.java
+5 -8
View File
@@ -69,7 +69,7 @@ public class RobotContainer {
/* Controllers */ /* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); 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_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(); private final Limelight limelight = new Limelight();
@@ -77,7 +77,6 @@ public class RobotContainer {
private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor);
/* Virtual Controllers */ /* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0); private final VirtualController m_virtualDriver = new VirtualController(0);
private final VirtualController m_virtualOperator = new VirtualController(1); private final VirtualController m_virtualOperator = new VirtualController(1);
@@ -123,9 +122,9 @@ public class RobotContainer {
private String lastAutoName = "four_note_taxi_kracken.auto"; private String lastAutoName = "four_note_taxi_kracken.auto";
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
lastAutoName, // () -> autoplaybackName.get(), // lastAutoName () -> autoplaybackName.get(), // lastAutoName
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, true); true, false);
private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
@@ -195,11 +194,10 @@ public class RobotContainer {
// ! /* Speed */ // ! /* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final 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) new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
@@ -207,7 +205,6 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
// ? /* Operator Buttons */ // ? /* Operator Buttons */
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+28 -22
View File
@@ -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 Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private RobotGyro gyro; 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 rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
@@ -52,7 +55,7 @@ public class SwerveDrive extends SubsystemBase {
this.rightBack = rightBack; this.rightBack = rightBack;
this.gyro = gyro; this.gyro = gyro;
reset_index();
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
} }
@@ -68,7 +71,6 @@ public class SwerveDrive extends SubsystemBase {
module.setDesiredState(state); module.setDesiredState(state);
} }
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
@@ -256,15 +258,29 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putNumber("RotTartget", rotTarget); SmartDashboard.putNumber("RotTartget", rotTarget);
} }
public void shiftDown() { private void reset_index() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
}
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} 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() { public void setToSlow() {
@@ -294,16 +310,6 @@ public class SwerveDrive extends SubsystemBase {
System.out.println("TURBO"); 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) { public void toggleGear(double angle) {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { 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; this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;