Sensorless homing

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

@@ -5,14 +5,14 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc.robot.commands.ElevatorCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.ElevatorToPosition;
 import frc.robot.commands.TankDriveCommand;
 import frc.robot.subsystems.drive.Drivetrain;
 import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.elevator.ElevatorConstants;
 
 public class RobotContainer {
   /**
@@ -36,28 +36,24 @@ public class RobotContainer {
    * Up[-1,1]Down
    */
   private CommandJoystick driverController;
-  private CommandJoystick opController;
+  private CommandXboxController opController;
 
 
   private final Drivetrain drive = new Drivetrain();
 
-  private final Elevator elevate = new Elevator();
+  private final Elevator elevator = new Elevator();
 
 
   public RobotContainer() {
 
     driverController = new CommandJoystick(0);
-    opController = new CommandJoystick(1);
+    opController = new CommandXboxController(1);
 
     drive.setDefaultCommand(new TankDriveCommand(drive, ()->driverController.getRawAxis(1), ()->driverController.getRawAxis(5)));
-    new JoystickButton(opController.getHID(), 6).onTrue((new ElevatorCommand(elevate, true).withInterruptBehavior(InterruptionBehavior.kCancelSelf)));
-    new JoystickButton(opController.getHID(), 5).onTrue((new ElevatorCommand(elevate, false).withInterruptBehavior(InterruptionBehavior.kCancelSelf)));
 
-    configureBindings();
-  }
-
-  private void configureBindings() {
-
+    opController.povUp().onTrue(new ElevatorToPosition(elevator, ElevatorConstants.topHeight));
+    opController.povLeft().onTrue(new ElevatorToPosition(elevator, ElevatorConstants.middleHeight));
+    opController.povDown().onTrue(new ElevatorToPosition(elevator, ElevatorConstants.bottomHeight));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/ElevatorToPosition.java

@@ -0,0 +1,28 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.elevator.Elevator;
+
+public class ElevatorToPosition extends Command {
+
+    private final Elevator elevator;
+    private final double height;
+
+    public ElevatorToPosition(Elevator elevator, double height) {
+        this.elevator = elevator;
+        this.height = height;
+        addRequirements(elevator);
+    }
+
+    @Override
+    public void execute() {
+        elevator.setPosition(height);
+    }
+
+    @Override
+    public boolean isFinished() {
+        //if we hit a hard stop, cancel the command
+        return elevator.hasHitHardStop();
+    }
+
+}

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -1,81 +1,59 @@
 package frc.robot.subsystems.elevator;
 
-import com.revrobotics.spark.ClosedLoopSlot;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import com.revrobotics.spark.SparkMax;
-
 import frc.robot.util.SparkUtil;
 
 public class Elevator extends SubsystemBase{
     private final SparkMax heightMotor;
-    private final SparkMax armMotor;
-    private final SparkClosedLoopController closedLoopController;
-    private final SparkClosedLoopController armLoopController;
+
+    private final SparkClosedLoopController heightPID;
+
+
     public Elevator(){
         heightMotor = new SparkMax(ElevatorConstants.heightMotorCANID, MotorType.kBrushless);
-        armMotor = new SparkMax(ElevatorConstants.armMotorCANID, MotorType.kBrushless);
-
-        closedLoopController = heightMotor.getClosedLoopController();
-        armLoopController = armMotor.getClosedLoopController();
+        heightPID = heightMotor.getClosedLoopController();
 
         SparkMaxConfig motorConfig = new SparkMaxConfig();
         motorConfig.smartCurrentLimit(50).idleMode(IdleMode.kBrake).inverted(false);
         motorConfig.encoder.positionConversionFactor(ElevatorConstants.positionConversionFactor);
-        /*
-         * Configure the closed loop controller. We want to make sure we set the
-         * feedback sensor as the primary encoder.
-         */
         motorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
-            // Set PID values for height control. We don't need to pass a closed loop
-            // slot, as it will default to slot 0.
             .p(0.1)
             .i(0)
             .d(0)
             .outputRange(-1, 1);
 
 
-        SparkUtil.tryUntilOk(5, 
-        () -> heightMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
-
-        motorConfig.encoder.positionConversionFactor(ElevatorConstants.armPositionConversionFactor);
-        motorConfig.closedLoop
-            // Set PID values for arm control in slot 1
-            .p(0.1, ClosedLoopSlot.kSlot0)
-            .i(0, ClosedLoopSlot.kSlot0)
-            .d(0, ClosedLoopSlot.kSlot0)
-            .outputRange(-1, 1, ClosedLoopSlot.kSlot0);
-        SparkUtil.tryUntilOk(5, 
-        () -> armMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
-
-
+        SparkUtil.tryUntilOk(
+            5, 
+            () -> heightMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
+        );        
     }
 
-    /**
-     * Set the motion of the elevator. Positive is up.
-     * @param speed
-     */
-    public void run(double speed){
-        heightMotor.set(speed);
+    public boolean hasHitHardStop() {
+        return 
+            heightMotor.getOutputCurrent() > ElevatorConstants.autoStopCurrentThreshold
+            && 
+            heightMotor.getEncoder().getVelocity() < ElevatorConstants.autoStopVelocityThreshold.in(MetersPerSecond);
     }
 
     /**
-     * Sets the elevator and accompanying arm's setpoints. I have never used Rev's built-in PID, so I can only hope this works.
-     * @param heightPosition Elevator's setpoint
-     * @param armAngle Arm's setpoint
+     * Sets the elevators setpoint.
+     * @param height Position to move towards
      */
-    public void setPose(double heightPosition, double armAngle){
-        closedLoopController.setReference(heightPosition, ControlType.kPosition, ClosedLoopSlot.kSlot0);
-        armLoopController.setReference(armAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0);
+    public void setPosition(double height){
+        heightPID.setReference(height, ControlType.kPosition);
     }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -1,9 +1,17 @@
 package frc.robot.subsystems.elevator;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
+import edu.wpi.first.units.measure.LinearVelocity;
+
 public class ElevatorConstants {
     public static final int heightMotorCANID = 6;
-    public static final int armMotorCANID = 7;
     public static final double positionConversionFactor = 1; //TODO: measure this
-    public static final double armPositionConversionFactor = 1; //TODO: measure this
+
     public static final double autoStopCurrentThreshold = 15;
+    public static final LinearVelocity autoStopVelocityThreshold = MetersPerSecond.of(0.1); //TODO measure
+
+    public static final double topHeight = 0;
+    public static final double middleHeight = 0;
+    public static final double bottomHeight = 0;
 }

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

@@ -0,0 +1,25 @@
+package frc.robot.subsystems.wrist;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class MoveWrist extends Command {
+
+    private final Wrist wrist;
+    private final boolean direction;
+
+    public MoveWrist(Wrist wrist, boolean moveOut) {
+        this.wrist = wrist;
+        direction = moveOut;
+    }
+
+    @Override
+    public void execute() {
+        wrist.rotate(WristConstants.wristSpeed * (direction ? 1 : -1));
+    }
+
+    @Override
+    public boolean isFinished() {
+        return wrist.hasHitHardStop();
+    }
+
+}

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

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.wrist;
+
+import static edu.wpi.first.units.Units.RPM;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.SparkUtil;
+
+public class Wrist extends SubsystemBase {
+
+    private final SparkMax motor;
+
+    public Wrist() {
+        motor = new SparkMax(WristConstants.motorCANId, MotorType.kBrushless);
+
+        SparkMaxConfig config = new SparkMaxConfig();
+        config.smartCurrentLimit(0).idleMode(IdleMode.kBrake);
+        config.encoder.velocityConversionFactor(0);
+
+        SparkUtil.tryUntilOk(
+            5, 
+            () -> motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
+        );
+    }
+
+    public boolean hasHitHardStop() {
+        return 
+            motor.getOutputCurrent() > WristConstants.currentAutoStopThreshold 
+            &&
+            motor.getEncoder().getVelocity() < WristConstants.autoStopVelocityThreshold.in(RPM);
+    }
+
+    /**
+     * Rotate the wrist. Positive is away from the robot. 
+     * @param speed
+     */
+    public void rotate(double speed) {
+        motor.set(speed);
+    }
+}

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

@@ -0,0 +1,13 @@
+package frc.robot.subsystems.wrist;
+
+import static edu.wpi.first.units.Units.RPM;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+
+public class WristConstants {
+    public static final int motorCANId = 7;
+    public static final double currentAutoStopThreshold = 15;
+    public static final AngularVelocity autoStopVelocityThreshold = RPM.of(10); //TODO measure
+
+    public static final double wristSpeed = 0.75;
+}