Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

commit

Browse Source
This commit is contained in:
Aarav
2023-03-11 14:03:36 -07:00
parent dfc6978186
commit 5521349510
7 changed files with 143 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -7
View File
@@ -82,19 +82,21 @@ public class SwerveDrive extends SubsystemBase {
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.1) {
rotTarget = gyro.getRotation2d();
rot = rightStick.getX();
} else {
rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
}
double rot = rightStick.getX();
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -235,8 +237,16 @@ public class SwerveDrive extends SubsystemBase {
* Shifts gear from high to low, or vice versa.
* @param shift true to shift to high, false to shift to low
*/
public void highSpeed(boolean shift) {
this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
// public void highSpeed(boolean shift) {
// this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
// }
public void toggleGear() {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
}