Merge branch 'development' of https://github.com/awtybots/FRC-2024 in…

…to development

src/main/java/frc/robot/Constants.java

@@ -154,10 +154,10 @@ public static final class WristConstants {
 
     public static final int kCurrentLimit = 30;
 
-    public static final int initialExtension = 0; // TODO TUNE
-    public static final int maxExtension = 1; // TODO TUNE
+    public static final int initialAngle = 0; // TODO TUNE radians
 
-    public static final double armElevatorConversion = 1; // TODO TUNE
+    public static final int minAngle = -1; // TODO TUNE radians
+    public static final int maxAngle = 1; // TODO TUNE radians
 
     // ArmElevator PID constants
     public static final double kP = 0.1; // TODO TUNE

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

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.ControlCommands.ArmCommands;
 import frc.robot.commands.ControlCommands.DriveCommands;
+import frc.robot.commands.ControlCommands.WristCommands;
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.ArmIO;
@@ -214,6 +215,9 @@ private void configureButtonBindings() {
 
     sArm.setDefaultCommand(ArmCommands.joystickDrive(sArm, () -> -operatorController.getRightY()));
 
+    sWrist.setDefaultCommand(
+        WristCommands.joystickDrive(sWrist, () -> operatorController.getLeftY()));
+
     operatorController
         .a()
         .whileTrue(

src/main/java/frc/robot/commands/ControlCommands/ArmCommands.java

@@ -34,8 +34,8 @@ public static Command joystickDrive(Arm arm, DoubleSupplier ySupplier) {
           double stickMagnitude = MathUtil.applyDeadband(ySupplier.getAsDouble(), DEADBAND);
 
           // Square values, so that it's easier to control at lower speeds
-          double sign = Math.copySign(1, stickMagnitude);
-          stickMagnitude = stickMagnitude; // * stickMagnitude * sign;
+          // double sign = Math.copySign(1, stickMagnitude);
+          // stickMagnitude = stickMagnitude * stickMagnitude * sign;
 
           // Calcaulate new rotational velocity
           double rotationalVelocity = stickMagnitude * MAXRPM;

src/main/java/frc/robot/commands/ControlCommands/WristCommands.java

@@ -0,0 +1,44 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.commands.ControlCommands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.wrist.Wrist;
+import java.util.function.DoubleSupplier;
+
+public class WristCommands {
+  private static final double DEADBAND = 0.3;
+  // idk what a realistic one is so this is roughly 90 degrees per 2 seconds
+  private static final double MAXRADIANSPERSECOND = 1.6;
+
+  private WristCommands() {}
+
+  /** Wrist command using one axis of a joystick (controlling wrist velocity). */
+  public static Command joystickDrive(Wrist wrist, DoubleSupplier ySupplier) {
+    return Commands.run(
+        () -> {
+          // Apply deadband (i.e. min DEADBAND max 1.0)
+          double stickMagnitude = MathUtil.applyDeadband(ySupplier.getAsDouble(), DEADBAND);
+
+          // Calcaulate new rotational velocity
+          double rotationalVelocity = stickMagnitude * MAXRADIANSPERSECOND;
+
+          // Send command to wrist subsystem to run wrist
+          wrist.runTargetVelocity(rotationalVelocity);
+        },
+        wrist);
+  }
+}

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

@@ -74,9 +74,9 @@ public void runVelocity(double velocityRPM) {
   public void runTargetVelocity(double targetVelocity) {
     io.setTargetAngle(
         inputs.targetPositionRad
-            + 0.02
-                * ArmConstants.armConversion
-                * Units.rotationsToRadians(targetVelocity)); // TODO Constant needed
+            + 0.02 // TODO correct cycle time here needed
+                // * ArmConstants.armConversion
+                * Units.rotationsToRadians(targetVelocity));
   }
 
   /**

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

@@ -34,6 +34,7 @@ public default void setVoltage(double volts) {}
   /** Run closed loop at the specified velocity. */
   public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
 
+  /** Run loop for the specified target angle. */
   public default void setTargetAngle(double angle) {}
 
   /** Stop in open loop. */

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

@@ -41,7 +41,6 @@ public class ArmIOSparkMax implements ArmIO {
       leftMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
 
   private final SparkPIDController pid = leftMotor.getPIDController();
-  private final SparkPIDController mLeftArmPIDController;
 
   private double targetAngle = 0; // Radians, just a default value.
 
@@ -52,16 +51,14 @@ public ArmIOSparkMax() {
     leftMotor.setCANTimeout(250);
     rightMotor.setCANTimeout(250);
 
-    leftRelativeEncoder.setPosition(0.0);
+    // leftRelativeEncoder.setPosition(0.0);
 
     // left.setInverted(false);
     // right.setInverted(false);
 
     leftMotor.enableVoltageCompensation(12.0);
     rightMotor.enableVoltageCompensation(12.0);
 
-    mLeftArmPIDController = leftMotor.getPIDController();
-
     leftMotor.setSmartCurrentLimit(ArmConstants.kCurrentLimit);
     rightMotor.setSmartCurrentLimit(ArmConstants.kCurrentLimit);
 
@@ -79,16 +76,6 @@ public void updateInputs(ArmIOInputs inputs) {
     inputs.appliedVolts = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage();
     inputs.currentAmps = new double[] {leftMotor.getOutputCurrent()};
     inputs.targetPositionRad = targetAngle;
-
-    // FOR TESTING DELETE
-    // pid.setReference(
-    //     Units.radiansPerSecondToRotationsPerMinute(targetAngle) * GEAR_RATIO,
-    //     ControlType.kPosition,
-    //     0,
-    //     0,
-    //     ArbFFUnits.kVoltage);
-
-    // pid.setReference(5, ControlType.kVoltage, 0, 0, ArbFFUnits.kVoltage);
   }
 
   @Override
@@ -109,15 +96,13 @@ public void setVelocity(double velocityRadPerSec, double ffVolts) {
 
   @Override
   public void setTargetAngle(double angle) {
-    targetAngle = angle;
+    targetAngle = MathUtil.clamp(angle, ArmConstants.minimumAngle, ArmConstants.maximumAngle);
     pid.setReference(
-        Units.radiansToRotations(angle) * GEAR_RATIO,
+        Units.radiansToRotations(targetAngle) * GEAR_RATIO,
         ControlType.kPosition,
         0,
         0,
         ArbFFUnits.kVoltage);
-
-    targetAngle = MathUtil.clamp(targetAngle, ArmConstants.minimumAngle, ArmConstants.maximumAngle);
   }
 
   @Override

src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java

@@ -26,7 +26,7 @@
  * "CANSparkFlex".
  */
 public class ClimberIOSparkMax implements ClimberIO {
-  private static final double GEAR_RATIO = 50.0; // May be reciprocal 50:1 or 40:1
+  private static final double GEAR_RATIO = 50.0; // 49:1 in reality but too late
 
   private final CANSparkMax leftMotor =
       new CANSparkMax(ClimberConstants.kLeftClimberMotorId, MotorType.kBrushless);

src/main/java/frc/robot/subsystems/wrist/Wrist.java

@@ -68,6 +68,13 @@ public void runVelocity(double velocityRPM) {
     Logger.recordOutput("Wrist/SetpointRPM", velocityRPM);
   }
 
+  public void runTargetVelocity(double targetVelocity) {
+    io.setTargetAngle(
+        inputs.targetPositionRad
+            + 0.02 // TODO correct cycle time here needed
+                * targetVelocity);
+  }
+
   /** Stops the Wrist. */
   public void stop() {
     io.stop();

src/main/java/frc/robot/subsystems/wrist/WristIO.java

@@ -13,6 +13,7 @@
 
 package frc.robot.subsystems.wrist;
 
+import frc.robot.Constants.WristConstants;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface WristIO {
@@ -22,6 +23,7 @@ public static class WristIOInputs {
     public double velocityRadPerSec = 0.0;
     public double appliedVolts = 0.0;
     public double[] currentAmps = new double[] {};
+    public double targetPositionRad = WristConstants.initialAngle;
   }
 
   /** Updates the set of loggable inputs. */
@@ -33,6 +35,9 @@ public default void setVoltage(double volts) {}
   /** Run closed loop at the specified velocity. */
   public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
 
+  /** Run closed loop for the specified target angle. */
+  public default void setTargetAngle(double angle) {}
+
   /** Stop in open loop. */
   public default void stop() {}