Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

update gearshifting logic

Browse Source
This commit is contained in:
Michael Mikovsky
2024-08-27 17:45:51 -06:00
parent f77712aa94
commit 721f160f37
3 changed files with 29 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+15 -14
View File
@@ -35,19 +35,20 @@ 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 SLOW_SPEED = 0.5; public static final double SLOW_SPEED = 0.25;
public static final double FAST_SPEED = 1.0; public static final double FAST_SPEED = 0.5;
public static final double TURBO_SPEED = 4.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.130126953125 + 0.25 + 0.25; public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 0.5;
public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 +0.5 ;//+ 0.5;
public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25; public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875+0.5 ;
public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25; 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;
@@ -59,7 +60,7 @@ public final class Constants {
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;
} }
@@ -84,15 +85,15 @@ public final class Constants {
} }
public static final class Conversions { public static final class Conversions {
public static final int CANCODER_TICKS_PER_ROTATION = 4096; public static final int CANCODER_TICKS_PER_ROTATION = 1;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.3;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8; public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 2048; public static final double TICKS_PER_MOTOR_REV = 0.5;
public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double WHEEL_DIAMETER_INCHES = 3.9;
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;
@@ -111,7 +112,7 @@ public final class Constants {
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 MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value 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_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
// dimensions // dimensions
src/main/java/frc4388/robot/RobotContainer.java
+8 -8
View File
@@ -251,13 +251,13 @@ public class RobotContainer {
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
// * /* D-Pad Stuff */ // * /* D-Pad Stuff */
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
@@ -308,11 +308,11 @@ 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.shiftUp())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()));
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); // .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.shiftDown())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -5
View File
@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
import frc4388.utility.RobotGyro; import frc4388.utility.RobotGyro;
import frc4388.utility.RobotUnits; import frc4388.utility.RobotUnits;
@@ -70,7 +71,7 @@ public class SwerveDrive extends SubsystemBase {
boolean stopped = false; 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) * 1.0; double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
if (fieldRelative) { if (fieldRelative) {
@@ -102,7 +103,7 @@ public class SwerveDrive extends SubsystemBase {
// Convert field-relative speeds to robot-relative speeds. // Convert field-relative speeds to robot-relative speeds.
// chassisSpeeds = chassisSpeeds. // chassisSpeeds = chassisSpeeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds. } else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
} }
@@ -267,7 +268,7 @@ public class SwerveDrive extends SubsystemBase {
} }
public void setToSlow() { public void setToSlow() {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW"); System.out.println("SLOW");
System.out.println("SLOW"); System.out.println("SLOW");
System.out.println("SLOW"); System.out.println("SLOW");
@@ -276,7 +277,7 @@ public class SwerveDrive extends SubsystemBase {
} }
public void setToFast() { public void setToFast() {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED; this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST"); System.out.println("FAST");
System.out.println("FAST"); System.out.println("FAST");
System.out.println("FAST"); System.out.println("FAST");
@@ -285,7 +286,7 @@ public class SwerveDrive extends SubsystemBase {
} }
public void setToTurbo() { public void setToTurbo() {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO"); System.out.println("TURBO");
System.out.println("TURBO"); System.out.println("TURBO");
System.out.println("TURBO"); System.out.println("TURBO");