better wrist alg and simple ff on aimbot alignment

src/main/java/frc/robot/RobotContainer.java

@@ -225,6 +225,13 @@ private void configureButtonBindings() {
             .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST)));
 
     controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
+    controller.rightTrigger().whileTrue(m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get()));
+    controller.leftTrigger().whileTrue(m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get()));
+    controller.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE));
+
+    // 96.240234375
+    // 60.029296875
+    // 2250
 
     m_driveSubsystem.setDefaultCommand(
         DriveCommands.joystickDrive(

src/main/java/frc/robot/commands/AimbotCommand.java

@@ -2,6 +2,7 @@
 
 import com.gos.lib.properties.pid.PidProperty;
 import com.gos.lib.properties.pid.WpiPidPropertyBuilder;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -36,6 +37,8 @@ public class AimbotCommand extends Command {
 
   private boolean m_runKicker;
 
+  private static final double TOLERENCE_DEGREES = 10.0;
+
   public AimbotCommand(ArmSubsystem armSubsystem,
                        DriveSubsystem driveSubsystem,
                        ShooterSubsystem shooterSubsystem,
@@ -49,6 +52,9 @@ public AimbotCommand(ArmSubsystem armSubsystem,
     m_smallController = new PIDController(0.0, 0.0, 0.0);
     m_fastController = new PIDController(0.0, 0.0, 0.0);
 
+    m_smallController.enableContinuousInput(-180, 180);
+    m_fastController.enableContinuousInput(-180, 180);
+
     m_smallProperty = new WpiPidPropertyBuilder("Drive/Aimbot Small", false, m_smallController)
             .addP(1.0)
             .addI(0.0)
@@ -82,18 +88,19 @@ public void execute() {
     FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration();
 
     // TODO make an actual equation for shot time based on distance
-    double shotTime = 0.5;
+    double distance = AimbotUtils.getDistanceFromSpeaker(m_driveSubsystem.getVisionPose());
+    double shotTime = 1.05;
 
     Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d();
     Translation2d movingTarget = new Translation2d();
 
     // loop over movement calcs to better adjust for acceleration
-    if (false) {
+    if (true) {
       for (int i = 0; i < 5; i++) {
         double virtualGoalX = target.getX()
-            - shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue());
+            + shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue());
         double virtualGoalY = target.getY()
-            - shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue());
+            + shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue());
 
         movingTarget = new Translation2d(virtualGoalX, virtualGoalY);
       }
@@ -103,46 +110,53 @@ public void execute() {
 
     // get our desired rotation and error from it
     double desiredRotationDegs =
-        AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(),
-            new Translation3d(movingTarget.getX(), movingTarget.getY(), 0.0))
+        AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose())
             .getDegrees();
     double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees());
 
     // if we're far from our setpoint, move faster
-    double omega;
-    if (error > 15.0) {
+    double omega;// = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);;
+    if (error > 5.0) {
       omega = m_fastController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);
     } else {
       omega = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);
     }
 
+    // add a feedforward component to compensate for horizontal movement
+    Translation2d linearFieldVelocity = new Translation2d(fieldRelativeSpeed.vx, fieldRelativeSpeed.vy);
+    Translation2d tangentalVelocity = linearFieldVelocity
+        .rotateBy(Rotation2d.fromDegrees(-desiredRotationDegs).unaryMinus());
+    double tangentalComponent = tangentalVelocity.getX();
+
     double x = -DriveCommands.setSensitivity(-m_driverController.getLeftY(), 0.25);
     double y = -DriveCommands.setSensitivity(-m_driverController.getLeftX(), 0.25);
 
+    x = MathUtil.applyDeadband(x, 0.1);
+    y = MathUtil.applyDeadband(y, 0.1);
 
     Rotation2d heading;
 
     // if red change heading goal
     if (DriverStation.getAlliance().isPresent()
             && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
-      heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180));
-    } else {
       heading = m_driveSubsystem.getRotation();
+    } else {
+      heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180));
     }
 
     // Convert to field relative speeds & send command
     m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(
             x * Constants.DriveConstants.MAX_LINEAR_SPEED,
             y * Constants.DriveConstants.MAX_LINEAR_SPEED,
-            omega * Constants.DriveConstants.MAX_ANGULAR_SPEED,
+            omega + tangentalComponent * 0.5,
             heading
     ));
 
     m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM);
-    m_shooterSubsystem.runShooterVelocity(m_runKicker);
+    m_shooterSubsystem.runShooterVelocity(m_runKicker).execute();
 
     // set shooter speeds and rumble controller
-    if (m_shooterSubsystem.atSpeed() && error < 10.0) {
+    if (m_shooterSubsystem.atSpeed() && error < 15.0) {
       m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0);
     } else {
       m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0);
@@ -152,7 +166,9 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_driveSubsystem.stopWithX();
-    m_shooterSubsystem.runShooterVelocity(false, 0.0, 0.0);
+    m_shooterSubsystem.setShooterPowerLeft(0.0);
+    m_shooterSubsystem.setShooterPowerRight(0.0);
+    m_shooterSubsystem.setKickerPower(0.0);
     m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW);
     m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0);
   }

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -152,8 +152,8 @@ public void handleState() {
         m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0;
       }
       case ANTI_DEFENSE -> {
-        m_desiredArmPoseDegs = 45.0;
-        m_desiredWristPoseDegs = 35.0;
+        m_desiredArmPoseDegs = 68.0;
+        m_desiredWristPoseDegs = 65.0;
       }
       case INTAKE -> {
         m_armVelocityMult = 1.0;

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -170,7 +170,7 @@ public void setupLogging() {
   }
 
   public boolean atSpeed() {
-    return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75
-        && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75;
+    return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 100
+        && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 100;
   }
 }

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -33,11 +33,11 @@ public class VisionSubsystem {
   private AprilTagFieldLayout m_aprilTagFieldLayout;
   private final String m_name;
 
-  private final double xyStdDevCoefficient = Units.inchesToMeters(4.0);
-  private final double thetaStdDevCoefficient = Units.degreesToRadians(14.0);
+  private final double xyStdDevCoefficient = Units.inchesToMeters(3.5);
+  private final double thetaStdDevCoefficient = Units.degreesToRadians(12.0);
 
-  private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(2.0);
-  private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(7.0);
+  private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(1.0);
+  private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(5.0);
 
   private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged();
 

src/main/java/lib/utils/AimbotUtils.java

@@ -47,7 +47,8 @@ public class AimbotUtils {
   public static double getWristAngle(double distance) {
 //    return m_angleLerpTable.get(distance);
 //    return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance);
-    return 48.903-(0.09001 * distance);
+//    return 48.903-(0.09001 * distance);
+    return 50.218 - (0.1108 * distance);
   }
 
   public static double getLeftSpeed(double distance) {