WIP prepare for shooter pivot.

Gongoliers · Feb 24, 2024 · 6291725 · 6291725
1 parent f1854c0
commit 6291725
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 16 deletions.
diff --git a/simgui.json b/simgui.json
@@ -41,15 +41,27 @@
     "transitory": {
       "Shuffleboard": {
         "Arm": {
+          "Derivatives": {
+            "open": true
+          },
           "Feedforward": {
             "open": true
           },
+          "Goal": {
+            "open": true
+          },
           "Position": {
             "open": true
           },
+          "Setpoint": {
+            "open": true
+          },
           "Velocities": {
             "open": true
           },
+          "Voltages": {
+            "open": true
+          },
           "open": true
         },
         "Climber": {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -71,18 +71,20 @@ private void configureBindings() {
     driverController.y().onTrue(odometry.tare());
     driverController.x().whileTrue(swerve.cross());
 
-    operatorController
-        .leftTrigger()
-        .whileTrue(
-            Commands.parallel(arm.to(ArmState.INTAKE), intake.out())
-                .andThen(Commands.parallel(intake.intake(), shooter.intake()))).onFalse(Commands.runOnce(() -> {
-                  intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE);
-                  arm.setGoal(ArmState.STOW);
-                }));
-
-    operatorController
-        .rightTrigger()
-        .whileTrue(Commands.parallel(arm.to(ArmState.SHOOT)).andThen(shooter.shoot())).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
+    // operatorController
+    //     .leftTrigger()
+    //     .whileTrue(
+    //         Commands.parallel(arm.to(ArmState.INTAKE), intake.out())
+    //             .andThen(Commands.parallel(intake.intake(), shooter.intake()))).onFalse(Commands.runOnce(() -> {
+    //               intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE);
+    //               arm.setGoal(ArmState.STOW);
+    //             }));
+
+    // operatorController
+    //     .rightTrigger()
+    //     .whileTrue(arm.to(ArmState.SHOOT).andThen(shooter.shoot())).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
+
+    operatorController.a().whileTrue(arm.driveWrist(operatorController::getLeftY));
   }
 
   /**

diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -8,10 +8,10 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
-import frc.robot.RobotConstants;
 import frc.robot.RobotMechanisms;
 import frc.robot.arm.WristMotorIO.WristMotorIOValues;
 import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
+import java.util.function.DoubleSupplier;
 
 /** Subsystem class for the arm subsystem. */
 public class Arm extends Subsystem {
@@ -67,7 +67,7 @@ public void periodic() {
     shoulderMotor.update(shoulderMotorValues);
     wristMotor.update(wristMotorValues);
 
-    setSetpoint(getSetpoint().nextSetpoint(goal));
+    setSetpoint(setpoint.nextSetpoint(goal));
 
     RobotMechanisms.getInstance().setArmState(getPosition());
   }
@@ -167,4 +167,8 @@ private void setSetpoint(ArmState setpoint) {
   public Command to(ArmState goal) {
     return runOnce(() -> setGoal(goal)).andThen(Commands.waitUntil(this::atGoal));
   }
+
+  public Command driveWrist(DoubleSupplier joystick) {
+    return run(() -> wristMotor.setVoltage(-joystick.getAsDouble() * 4));
+  }
 }
diff --git a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java
@@ -78,8 +78,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec
 
     @Override
     public void setVoltage(double volts) {
-        // TODO disabled for working on wrist 
-        // sparkMax.setVoltage(volts);
+        sparkMax.setVoltage(volts);
     }
 
     @Override