controls for armelevator

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

@@ -128,9 +128,10 @@ public static final class ArmElevatorConstants {
     public static final int kCurrentLimit = 30;
 
     public static final int initialExtension = 0; // TODO TUNE
-    public static final int maxExtension = 1000; // TODO TUNE
+    public static double minExtension = 0;
+    public static double maxExtension = 16; // Inches
 
-    public static final double armElevatorConversion = 1; // TODO TUNE
+    // public static final double armElevatorConversion = 1; 
 
     // ArmElevator PID constants
     public static final double kP = 0.1; // TODO TUNE
@@ -140,9 +141,6 @@ public static final class ArmElevatorConstants {
     // Arm Elevator Feedforward characterization constants
     public static final double ks = 0; // TODO TUNE
     public static final double kv = 0; // TODO TUNE
-
-    // public static final double ArmThreshold = 0.1; // TODO TUNE
-
   }
 
   public static final class Presets {

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

@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.ControlCommands.ArmCommands;
+import frc.robot.commands.ControlCommands.ArmElevatorCommands;
 import frc.robot.commands.ControlCommands.DriveCommands;
 import frc.robot.commands.ControlCommands.WristCommands;
 import frc.robot.commands.FeedForwardCharacterization;
@@ -220,6 +221,9 @@ private void configureButtonBindings() {
     sWrist.setDefaultCommand(
         WristCommands.joystickDrive(sWrist, () -> operatorController.getLeftY()));
 
+    sArmElevator.setDefaultCommand(
+        ArmElevatorCommands.triggerDrive(sArmElevator, () -> operatorController.getLeftTriggerAxis(), () -> operatorController.getRightTriggerAxis()));
+
     operatorController
         .a()
         .whileTrue(

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2024 FRC 6328, FRC 5829
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -0,0 +1,47 @@
+// Copyright 2021-2024 FRC 6328, FRC 5829
+// 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.armElevator.ArmElevator;
+import java.util.function.DoubleSupplier;
+
+public class ArmElevatorCommands {
+  private static final double DEADBAND = 0.3;
+  private static final double MAXINCHESPERSECOND = 3;
+
+  private ArmElevatorCommands() {}
+
+  /** Wrist command using one axis of a joystick (controlling wrist velocity). */
+  public static Command triggerDrive(
+    ArmElevator armElevator, 
+    DoubleSupplier leftTriggerSupplier,
+    DoubleSupplier rightTriggerSupplier) {
+    return Commands.run(
+        () -> {
+          // Apply deadband (i.e. min DEADBAND max 1.0)
+          double stickMagnitude = MathUtil.applyDeadband(leftTriggerSupplier.getAsDouble(), DEADBAND) 
+              + -MathUtil.applyDeadband(rightTriggerSupplier.getAsDouble(), DEADBAND);
+
+          // Calcaulate new rotational velocity
+          double rotationalVelocity = stickMagnitude * MAXINCHESPERSECOND;
+
+          // Send command to wrist subsystem to run wrist
+          armElevator.runTargetVelocity(rotationalVelocity);
+        },
+        armElevator);
+  }
+}

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2024 FRC 6328, FRC 5829
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -22,6 +22,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.ArmConstants;
+import frc.robot.commands.ControlCommands.ArmCommands;
 
 /**
  * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
@@ -42,7 +43,7 @@ public class ArmIOSparkMax implements ArmIO {
 
   private final SparkPIDController pid = leftMotor.getPIDController();
 
-  private double targetAngle = 0; // Radians, just a default value.
+  private double targetAngle = 0.0; // Radians, just a default value.
 
   public ArmIOSparkMax() {
     leftMotor.restoreFactoryDefaults();
@@ -118,52 +119,4 @@ public void configurePID(double kP, double kI, double kD) {
     pid.setD(kD, 0);
     pid.setFF(0, 0);
   }
-
-  // TODO: The following commented code is from the ArmSubsystem - see if it needs to be integrated
-  // here
-  /*
-  public void setRotation(double value) {
-    armHeight = value;
-  }
-
-  public void setDegrees(double degrees) {
-    double encoderunits =
-        Convert.angleToEncoderPos(degrees, kArmGearRatio, Encoder.RevRelativeEncoder);
-    armHeight = encoderunits;
-  }
-
-  public double getAngle() {
-    final double rawRevs = mRightArmEncoder.getPosition();
-    final double theta =
-        Convert.encoderPosToAngle(rawRevs, kArmGearRatio, Encoder.RevRelativeEncoder);
-    return ArmConstants.startingAngle - theta;
-  }
-
-  public void resetEncoderValue() {
-    armHeight = 0;
-    mRightArmEncoder.setPosition(armHeight);
-  }
-
-  public void drive(double pct) {
-
-    // MathUtil.clamp(armHeight, ArmConstants.minimumHeight, getMaximumRotation());
-    armHeight += pct * ArmConstants.armConversion;
-  }
-
-  public boolean isFinished() {
-    return Math.abs(mRightArmEncoder.getPosition() - armHeight) < Presets.ArmThreshold;
-  }
-
-  @Override
-  public void periodic() {
-    mRightArmPIDController.setReference(armHeight, CANSparkMax.ControlType.kPosition, 0);
-    // SmartDashboard.putNumber(
-    //         "Arm Error",
-    //         Convert.encoderPosToAngle(
-    //                 mRightArmEncoder.getPosition() - armHeight, kArmGearRatio,
-    // Encoder.RevRelativeEncoder));
-    // SmartDashboard.putNumber("Arm angle", -(this.getAngle() - Arm.startingAngle));
-  }
-   */
-
 }

src/main/java/frc/robot/subsystems/armElevator/ArmElevator.java

@@ -14,7 +14,6 @@
 package frc.robot.subsystems.armElevator;
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmElevatorConstants;
 import frc.robot.Constants.EnvironmentalConstants;
@@ -59,13 +58,19 @@ public void runVolts(double volts) {
     io.setVoltage(volts);
   }
 
-  /** Run closed loop at the specified velocity. */
-  public void runVelocity(double velocityRPM) {
-    var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
-    io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec));
+  // /** Run closed loop at the specified velocity. */
+  // public void runVelocity(double velocityRPM) {
+  //   var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
+  //   io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec));
 
-    // Log ArmElevator setpoint
-    Logger.recordOutput("ArmElevator/SetpointRPM", velocityRPM);
+  //   // Log ArmElevator setpoint
+  //   Logger.recordOutput("ArmElevator/SetpointRPM", velocityRPM);
+  // }
+
+  public void runTargetVelocity(double velocityInchesPerSecond) {
+    io.setTargetDistance(inputs.targetDistance 
+      + 0.02 // TODO cycle time needed
+      * velocityInchesPerSecond);
   }
 
   /** Stops the ArmElevator. */
@@ -79,12 +84,12 @@ public double getTargetDistance() {
   }
 
   // get the current position of the elevator
-  @AutoLogOutput(key = "ArmElevator/PositionMeters")
+  @AutoLogOutput(key = "ArmElevator/PositionInches")
   public double getPositionMeters() {
-    return inputs.positionMeters;
+    return inputs.positionInches;
   }
 
-  public void setTargetDistance(double distanceMeters) {
-    io.setTargetDistance(distanceMeters);
-  }
+  // public void setTargetDistance(double distanceInches) {
+  //   io.setTargetDistance(distanceInches);
+  // }
 }

src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIO.java

@@ -18,7 +18,7 @@
 public interface ArmElevatorIO {
   @AutoLog
   public static class ArmElevatorIOInputs {
-    public double positionMeters = 0.0;
+    public double positionInches = 0.0;
     public double velocityMetersPerSec = 0.0;
     public double targetDistance = 0.0;
     public double[] currentAmps = new double[] {};
@@ -30,14 +30,12 @@ public default void updateInputs(ArmElevatorIOInputs inputs) {}
   /** Run open loop at the specified voltage. */
   public default void setVoltage(double volts) {}
 
-  /** Run closed loop at the specified velocity. */
-  public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
-
   /** Stop in open loop. */
   public default void stop() {}
 
   /** Set velocity PID constants. */
   public default void configurePID(double kP, double kI, double kD) {}
 
+  /** In inches */
   public default void setTargetDistance(double distance) {}
 }

src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSim.java

@@ -41,7 +41,7 @@ public void updateInputs(ArmElevatorIOInputs inputs) {
     sim.update(0.02);
 
     inputs.targetDistance = 0.0;
-    inputs.positionMeters = sim.getPositionMeters();
+    inputs.positionInches = sim.getPositionMeters();
     inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()};
   }
 
@@ -52,12 +52,12 @@ public void setVoltage(double volts) {
     sim.setInputVoltage(volts);
   }
 
-  @Override
-  public void setVelocity(double velocityRadPerSec, double ffVolts) {
-    closedLoop = true;
-    pid.setSetpoint(velocityRadPerSec);
-    this.ffVolts = ffVolts;
-  }
+  // @Override
+  // public void setVelocity(double velocityRadPerSec, double ffVolts) {
+  //   closedLoop = true;
+  //   pid.setSetpoint(velocityRadPerSec);
+  //   this.ffVolts = ffVolts;
+  // }
 
   @Override
   public void stop() {

src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java

@@ -18,6 +18,8 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.SparkPIDController.ArbFFUnits;
+
+import edu.wpi.first.math.MathUtil;
 import frc.robot.Constants.ArmElevatorConstants;
 
 /**
@@ -42,9 +44,9 @@ public ArmElevatorIOSparkMax() {
 
     motor.setCANTimeout(250);
 
-    // motor.setInverted(false);
+    motor.setInverted(true);
 
-    motor.enableVoltageCompensation(12.0);
+    // motor.enableVoltageCompensation(12.0);
 
     motor.setSmartCurrentLimit(ArmElevatorConstants.kCurrentLimit);
 
@@ -55,7 +57,7 @@ public ArmElevatorIOSparkMax() {
 
   @Override
   public void updateInputs(ArmElevatorIOInputs inputs) {
-    inputs.positionMeters = encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE;
+    inputs.positionInches = encoder.getPosition() / GEAR_RATIO * PULLEY_CIRCUMFERENCE;
     inputs.velocityMetersPerSec = encoder.getVelocity() / GEAR_RATIO * PULLEY_CIRCUMFERENCE;
     inputs.targetDistance = targetDistance;
     inputs.currentAmps = new double[] {motor.getOutputCurrent()};
@@ -67,15 +69,15 @@ public void setVoltage(double volts) {
     motor.setVoltage(volts);
   }
 
-  @Override
-  public void setVelocity(double velocityRadPerSec, double ffVolts) {
+  // @Override
+  // public void setVelocity(double velocityRadPerSec, double ffVolts) {
     // pid.setReference(
     //     Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO,
     //     ControlType.kVelocity,
     //     0,
     //     ffVolts,
     //     ArbFFUnits.kVoltage);
-  }
+  // }
 
   @Override
   public void stop() {
@@ -91,16 +93,13 @@ public void configurePID(double kP, double kI, double kD) {
   }
 
   @Override
-  public void setTargetDistance(double distanceMeters) {
-    targetDistance = distanceMeters;
+  public void setTargetDistance(double distanceInches) {
+    targetDistance = MathUtil.clamp(distanceInches, ArmElevatorConstants.minExtension, ArmElevatorConstants.maxExtension);
     pid.setReference(
-        distanceMeters / PULLEY_CIRCUMFERENCE * GEAR_RATIO,
+        targetDistance / PULLEY_CIRCUMFERENCE * GEAR_RATIO,
         ControlType.kPosition,
         0,
         0,
         ArbFFUnits.kVoltage);
-
-    // targetDistance = MathUtil.clamp(targetDistance, ArmConstants.minimumDistance,
-    // ArmConstants.maximumDistance);
   }
 }