autoaim slowly getting there, but the function isn't right yet

NicholsSchool · Oct 25, 2024 · 8750fce · 8750fce
1 parent af67d66
commit 8750fce
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -250,6 +250,7 @@ public void setStartingPose() {
           startPositionIndex.get() == 0
               ? AllianceFlipUtil.apply(startPosition0)
               : AllianceFlipUtil.apply(startPosition1));
+      arm.setTargetPos(40);
     }
   }
 
@@ -351,7 +352,7 @@ private void configureButtonBindings() {
                         new InstantCommand(() -> indexer.index(), indexer))),
                 new InstantCommand(() -> shooter.setShoot(), shooter)));
 
-    driveController.leftBumper().whileTrue(arm.runGoToPosCommand(60.0));
+    driveController.leftBumper().whileTrue(arm.runGoToPosCommand(50.0));
     driveController.leftBumper().whileFalse(arm.runGoToPosCommand(40.0));
     // driveController.rightTrigger(0.9).whileTrue(intake.runPoopCommand());
   }

diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -157,9 +157,8 @@ public static Command joystickDriveFacingPoint(
           Pose2d robotPose = drive.getPose();
           double desiredAngle =
               Math.atan2(
-                      facingPose.getY() - robotPose.getY(),
-                      AllianceFlipUtil.apply(facingPose.getX()) - robotPose.getX())
-                  + Math.PI;
+                  facingPose.getY() - robotPose.getY(),
+                  AllianceFlipUtil.apply(facingPose.getX()) - robotPose.getX());
 
           // Square values
           linearMagnitude = linearMagnitude * linearMagnitude;

diff --git a/src/main/java/frc/robot/commands/VisionCommands/ArmToShoot.java b/src/main/java/frc/robot/commands/VisionCommands/ArmToShoot.java
@@ -35,6 +35,6 @@ public void execute() {
                     AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.getX()),
                     FieldConstants.Speaker.centerSpeakerOpening.getY()),
                 new Rotation2d()));
-    arm.setTargetPos((-0.24 * Math.log(distance - 1.05) + 0.76) * 180 / Math.PI);
+    arm.setTargetPos((-0.33 * Math.log(119.0 * (distance - 0.67)) + 2) * 180 / Math.PI);
   }
 }