uncommented apriltags

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

Hide file tree

Showing 3 changed files with 21 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@
 import frc.robot.commands.AutoCommands;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.commands.VisionCommands.ArmToShoot;
 import frc.robot.commands.VoltageCommandRamp;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.ArmIOReal;
@@ -326,6 +327,10 @@ private void configureButtonBindings() {
                 () -> -driveController.getLeftX() * Constants.DriveConstants.lowGearScaler,
                 FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d(),
                 () -> drive.getYaw()));
+    driveController.rightStick().toggleOnTrue((new ArmToShoot(arm, drive)));
+    // .repeatedly()
+    // .until(driveController.leftTrigger())
+    // .andThen(arm.runGoToPosCommand(40.0)));
 
     intake.setDefaultCommand(new InstantCommand(() -> intake.stop(), intake));
     shooter.setDefaultCommand(new InstantCommand(() -> shooter.stop(), shooter));
@@ -346,8 +351,8 @@ private void configureButtonBindings() {
                         new InstantCommand(() -> indexer.index(), indexer))),
                 new InstantCommand(() -> shooter.setShoot(), shooter)));
 
-    driveController.leftBumper().whileTrue(arm.runGoToPosCommand(40.0));
-    driveController.leftBumper().whileFalse(arm.runGoToPosCommand(20.0));
+    driveController.leftBumper().whileTrue(arm.runGoToPosCommand(60.0));
+    driveController.leftBumper().whileFalse(arm.runGoToPosCommand(40.0));
     // driveController.rightTrigger(0.9).whileTrue(intake.runPoopCommand());
   }
 

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);
+    arm.setTargetPos((-0.24 * Math.log(distance - 1.05) + 0.76) * 180 / Math.PI);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -147,8 +147,8 @@ public void periodic() {
       wheelAbsolutes[i] = modules[i].getPosition();
     }
 
-    // pose = kalman.getEstimatedPosition();
-    // updateVision(wheelAbsolutes);
+    pose = kalman.getEstimatedPosition();
+    updateVision(wheelAbsolutes);
 
     // Log measured states
     SwerveModuleState[] measuredStates = new SwerveModuleState[4];
@@ -321,6 +321,17 @@ public Twist2d getFieldVelocity() {
   public double getYawVelocity() {
     return gyroInputs.yawVelocityRadPerSec;
   }
+  /**
+   * gets the field velocity in the universal x and y direction
+   *
+   * @return a twist containing x y and theta
+   */
+  public Twist2d getFieldOrientedVelocity() {
+    return (new Twist2d(
+        fieldVelocity.dx * Math.cos(getYaw()) + fieldVelocity.dy * Math.sin(getYaw()),
+        fieldVelocity.dx * Math.sin(getYaw()) + fieldVelocity.dy * Math.cos(getYaw()),
+        fieldVelocity.dtheta));
+  }
 
   @AutoLogOutput
   public double getYaw() {