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

drift fix (not tuned)

Browse Source
This commit is contained in:
66945
2023-03-15 09:57:01 -06:00
parent b5bc547898
commit f2bd759a2f
2 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1
View File
@@ -27,6 +27,7 @@ public final class Constants {
public static final double MAX_ROT_SPEED = 1.5; public static final double MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8; public static final double MIN_ROT_SPEED = 0.8;
public static double ROTATION_SPEED = MAX_ROT_SPEED; public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = MIN_ROT_SPEED;
public static final class IDs { public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2; public static final int LEFT_FRONT_WHEEL_ID = 2;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+3 -3
View File
@@ -58,9 +58,9 @@ public class SwerveDrive extends SubsystemBase {
double rot = 0; double rot = 0;
if (rightStick.getNorm() > 0.1) { if (rightStick.getNorm() > 0.1) {
rotTarget = gyro.getRotation2d(); rotTarget = gyro.getRotation2d();
rot = rightStick.getX(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
} else { } else {
rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); rot = rotTarget.minus(gyro.getRotation2d()).getRadians() * SwerveDriveConstants.ROT_CORRECTION_SPEED;
} }
// Use the left joystick to set speed. Apply a cubic curve and the set max speed. // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
@@ -68,7 +68,7 @@ public class SwerveDrive extends SubsystemBase {
Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds. // Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1));
} else { } else {
// Create robot-relative speeds. // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);