fix broken aim on red

Author: AkeBoss-tech

sim_logs/.Log_24-03-22_11-14-32.wpilog.icloud

@@ -36,6 +36,7 @@
       "/SmartDashboard/NoteTwo": "String Chooser",
       "/SmartDashboard/NoteTwoShot": "String Chooser",
       "/SmartDashboard/Pathfind to Pickup Pos": "Command",
+      "/SmartDashboard/Pivot Sysid": "Command",
       "/SmartDashboard/PivotArm": "Subsystem",
       "/SmartDashboard/Start": "String Chooser",
       "/SmartDashboard/Sysid Dynamic Drive Backward": "Command",
@@ -78,6 +79,11 @@
         "window": {
           "visible": true
         }
+      },
+      "/SmartDashboard/Pivot Sysid": {
+        "window": {
+          "visible": true
+        }
       }
     }
   },

sim_logs/.Log_24-03-22_11-16-19.wpilog.icloud

@@ -574,6 +574,8 @@ public void disabledPeriodic() {
 
     setPivotPose3d();
     field.setRobotPose(drive.getPose());
+    Logger.recordOutput("DriveAimed", DriveCommands.pointedAtSpeaker(drive));
+    Logger.recordOutput("PivotAimed", pivot.atSetpoint());
   }
 
   public Command intakeUntilIntaked(){

sim_logs/.Log_24-03-22_11-18-43.wpilog.icloud

@@ -85,8 +85,7 @@ public static Command joystickDrive(
                             .getTranslation();
 
                     // Convert to field relative speeds & send command
-                    boolean isFlipped = DriverStation.getAlliance().isPresent()
-                            && DriverStation.getAlliance().get() == Alliance.Red;
+                    boolean isFlipped = getIsFlipped();
                     drive.runVelocity(
                             ChassisSpeeds.fromFieldRelativeSpeeds(
                                     linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
@@ -138,6 +137,11 @@ public static Command joystickDriveRobotRelative(
                 drive);
     }
 
+    private static boolean getIsFlipped() {
+        return DriverStation.getAlliance().isPresent()
+                        && DriverStation.getAlliance().get() == Alliance.Red;
+    }
+
     /**
      * Drive robot while pointing at a specific point on the field.
      */
@@ -153,6 +157,8 @@ public static Command joystickSpeakerPoint(
                                                                                               // should use field
                                                                                               // constants
 
+
+
 
                     // Apply deadband
                     double linearMagnitude = MathUtil.applyDeadband(
@@ -161,9 +167,12 @@ public static Command joystickSpeakerPoint(
                     Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble() * slowMode,
                             ySupplier.getAsDouble() * slowMode);
                     Transform2d targetTransform = drive.getPose().minus(speakerPose);
-                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY());
+                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY())
+                        .plus(new Rotation2d(getIsFlipped() ? Math.PI : 0));
                     // Rotation2d deltaDirection = drive.getRotation().minus(targetDirection);
 
+                    Logger.recordOutput("AimAngle", targetDirection);
+
                     double omega = angleController.calculate(drive.getRotation().getRadians(),
                             targetDirection.getRadians());
 
@@ -210,7 +219,7 @@ public static Command joystickPasserPoint(
                     Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble() * slowMode,
                             ySupplier.getAsDouble() * slowMode);
                     Transform2d targetTransform = drive.getPose().minus(passerPosition);
-                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY());
+                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY()).plus(new Rotation2d(getIsFlipped() ? Math.PI : 0));;
                     // Rotation2d deltaDirection = drive.getRotation().minus(targetDirection);
 
                     double omega = angleController.calculate(drive.getRotation().getRadians(),
@@ -283,13 +292,13 @@ public static Command turnSpeakerAngle(Drive drive) {
         return new FunctionalCommand(
                 () -> {
                     Transform2d targetTransform = drive.getPose().minus(speakerPose);
-                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY());
+                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY()).plus(new Rotation2d(getIsFlipped() ? Math.PI : 0));;
                     angleController.setSetpoint(targetDirection.getRadians());
                 },
                 () -> {
                     // defines distance from speaker
                     Transform2d targetTransform = drive.getPose().minus(speakerPose);
-                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY());
+                    Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY()).plus(new Rotation2d(getIsFlipped() ? Math.PI : 0));;
                     double omega = angleController.calculate(drive.getRotation().getRadians(),
                             targetDirection.getRadians());
                     omega = Math.copySign(omega * omega, omega); // no idea why squared
@@ -322,7 +331,7 @@ public static void toggleSlowMode() {
     public static boolean pointedAtSpeaker(Drive drive) {
         Pose2d speakerPose = FieldConstants.speakerPosition();
         Transform2d targetTransform = drive.getPose().minus(speakerPose);
-        Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY());
+        Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY()).plus(new Rotation2d(getIsFlipped() ? Math.PI : 0));;
 
         // Convert to robot relative speeds and send command
         if (Math.abs(drive.getRotation().getDegrees() - targetDirection.getDegrees()) < 1) {

sim_logs/Log_177dd034386b90b9.wpilog

@@ -3,6 +3,9 @@
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import java.util.function.DoubleSupplier;
 
@@ -12,12 +15,18 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
@@ -35,6 +44,13 @@ public class PivotArm extends SubsystemBase {
     private LoggedDashboardNumber logkV;
     private LoggedDashboardNumber logkA;
 
+    // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+    private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+    // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+    private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
+    // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+    private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
+
 
     private double setpoint = 0;
 
@@ -61,8 +77,18 @@ public PivotArm(PivotArmIO io) {
         logkA = new LoggedDashboardNumber("PivotArm/kG", io.getkA());
 
         SysId = new SysIdRoutine(
-            new SysIdRoutine.Config(Volts.per(Second).of(PivotArmConstants.RAMP_RATE), Volts.of(PivotArmConstants.STEP_VOLTAGE), Seconds.of(6)),
-            new SysIdRoutine.Mechanism(v -> io.setVoltage(v.in(Volts)), null, this));
+            new SysIdRoutine.Config(Volts.per(Second).of(PivotArmConstants.RAMP_RATE), Volts.of(PivotArmConstants.STEP_VOLTAGE), null),
+            new SysIdRoutine.Mechanism(v -> io.setVoltage(v.in(Volts)), 
+                (sysidLog) -> {
+                    sysidLog.motor("pivot")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(inputs.appliedVolts, Volts))
+                    .angularPosition(m_angle.mut_replace(inputs.angleRads, Rotations))
+                    .angularVelocity(
+                        m_velocity.mut_replace(inputs.angVelocityRadsPerSec, RotationsPerSecond));
+                        
+                }, 
+                this));
 
     }
 
@@ -251,38 +277,43 @@ public Command bringDownCommand() {
             () -> {}, 
             () -> {
                 move(-1);
+                setpoint = 0;
             }, 
             (interrupted) -> {
                 move(0);
             }, 
             () -> {
-                return io.getAngle() < 0.15;
+                return io.getAngle() < 0.1;
             }, 
             this);
     }
 
     public Command quasistaticForward() {
         return SysId
             .quasistatic(Direction.kForward)
-            .until(() -> getAngle().getRadians() > PivotArmConstants.PIVOT_ARM_MAX_ANGLE);
+            .until(() -> getAngle().getRadians() > PivotArmConstants.PIVOT_ARM_MAX_ANGLE)
+                .alongWith(new InstantCommand(() -> Logger.recordOutput("PivotArm/sysid-test-state-", "quasistatic-forward")));
       }
 
     public Command quasistaticBack() {
         return SysId
             .quasistatic(Direction.kReverse)
-            .until(() -> getAngle().getRadians() < PivotArmConstants.PIVOT_ARM_MIN_ANGLE);
+            .until(() -> getAngle().getRadians() < PivotArmConstants.PIVOT_ARM_MIN_ANGLE)
+                .alongWith(new InstantCommand(() -> Logger.recordOutput("PivotArm/sysid-test-state-", "quasistatic-reverse")));
     }
 
     public Command dynamicForward() {
         return SysId
             .dynamic(Direction.kForward)
-            .until(() -> getAngle().getRadians() > PivotArmConstants.PIVOT_ARM_MAX_ANGLE);
+            .until(() -> getAngle().getRadians() > PivotArmConstants.PIVOT_ARM_MAX_ANGLE)
+                .alongWith(new InstantCommand(() -> Logger.recordOutput("PivotArm/sysid-test-state-", "dynamic-forward")));
     }
 
     public Command dynamicBack() {
         return SysId
             .dynamic(Direction.kReverse)
-            .until(() -> getAngle().getRadians() < PivotArmConstants.PIVOT_ARM_MIN_ANGLE);
+            .until(() -> getAngle().getRadians() < PivotArmConstants.PIVOT_ARM_MIN_ANGLE)
+                .alongWith(new InstantCommand(() -> Logger.recordOutput("PivotArm/sysid-test-state-", "dynamic-reverse")));
     }
 
 }

sim_logs/Log_24-03-22_11-06-26.wpilog

@@ -91,18 +91,26 @@ public static void configureControls() {
                 break;
             case PROGRAMMERS:
             default:
-                DRIVE_FORWARD = () -> -driver.getLeftY();
-                DRIVE_STRAFE = () -> -driver.getLeftX();
-                DRIVE_ROTATE = driver::getRightX;
-                DRIVE_SLOW = driver.x();
-                DRIVE_SPEAKER_AIM = driver.b();
-
-                DRIVE_STOP = driver.rightBumper();
+                // Driver controls
+                DRIVE_FORWARD = () -> (-driver.getLeftY());
+                DRIVE_STRAFE = ()->(-driver.getLeftX());
+                DRIVE_ROTATE = () -> (-driver.getRightX());
 
-                LOCK_PASS = driver.a();
+                // Driver Settings
+                DRIVE_SLOW = driver.start();
+                DRIVE_STOP = driver.x();
 
-                DRIVE_AMP = driver.leftBumper();
-                break;
+                // Driver Modes
+                DRIVE_ROBOT_RELATIVE = driver.y();
+                DRIVE_SPEAKER_AIM = driver.b(); // uses vision
+
+                // Driver Angle Locks
+                LOCK_BACK = driver.getDPad(DPad.DOWN);
+                LOCK_PICKUP = driver.getDPad(DPad.RIGHT);
+                LOCK_ON_AMP = driver.rightBumper();
+                LOCK_PASS = driver.getDPad(DPad.LEFT); // uses vision
+
+                DRIVE_AMP = EMPTY_TRIGGER; // uses vision
         }
 
         switch (Constants.operator) {