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

drift correction and recorded one note taxi auto (im geekin)

Browse Source
This commit is contained in:
Abhishrek05
2024-03-08 18:01:58 -07:00
parent ffcbbf39ad
commit e1b0864c28
3 changed files with 21 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+9 -2
View File
@@ -125,13 +125,20 @@ public class SwerveDrive extends SubsystemBase {
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
Translation2d rightStick = new Translation2d(rightX, rightY);
Translation2d rightStick = new Translation2d(-rightX, rightY);
if(fieldRelative) {
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians();
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
double min = tmp.getDegrees();
min = Math.max(Math.abs(min), 2);
if(tmp.getDegrees() < 0)
min*=-1;
tmp = new Rotation2d(min * Math.PI / 180);
rot = tmp.getRadians(); // x x - y/x
}
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);