Elevator moves to commanded position

Team1559 · Feb 12, 2025 · d87069c · d87069c
1 parent 7e64250
commit d87069c
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 23 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -65,6 +65,7 @@
     "DEADBAND",
     "odometry",
     "setpoint",
-    "Teleop"
+    "Teleop",
+    "WPILOG"
   ]
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -53,8 +53,8 @@ public Robot() {
     public void robotInit() {
         drivetrain.setDefaultCommand(new TeleopDriveCommand(pilotController::getLeftY, pilotController::getLeftX,
                 pilotController::getRightX, 5.21, 1.925, drivetrain));
-        pilotController.leftBumper().onTrue(new InstantCommand(()-> elevator.setTargetPosition(1), elevator));
-        pilotController.rightBumper().onTrue(new InstantCommand(() -> elevator.setTargetPosition(0)));
+        pilotController.leftBumper().onTrue(new InstantCommand(()-> elevator.setTargetPosition(.5), elevator));
+        pilotController.rightBumper().onTrue(new InstantCommand(() -> elevator.setTargetPosition(-999)));
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/ElevatorIo2025.java b/src/main/java/frc/robot/subsystems/ElevatorIo2025.java
@@ -1,52 +1,53 @@
 package frc.robot.subsystems;
 
-import org.littletonrobotics.junction.Logger;
-
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkAbsoluteEncoder;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkLimitSwitch;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLimitSwitch;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
 
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DigitalInput;
 import frc.lib.subsystems.elevator.ElevatorIo;
 
 public class ElevatorIo2025 extends ElevatorIo {
     private static final double GEAR_RATIO = 25;
     private static final int GEAR_TEETH = 13;
     private static final double RACK_SPACING = Units.inchesToMeters(3D / 8D);
     private static final double MAX_HEIGHT = Units.inchesToMeters(55);
-    private static final double MAX_VELOCITY = 0.5;
+    private static final double MAX_VELOCITY = .5;//0.5;
     private static final double MAX_ACCEL = 0.5;
     private static final double GRAVITY_FEEDFORWARD = .0; // TODO: change this
+    private static final int NUM_STAGES = 2;
 
     private final SparkFlex motor;
     private final RelativeEncoder encoder;
     private final SparkClosedLoopController motorController;
-    // private final SparkLimitSwitch limitSwitch;
-    private final DigitalInput dioLimitSwitch;
+    private final SparkLimitSwitch limitSwitch;
+
+    private boolean lastLimitSwitchState = false;
+
+
 
     public ElevatorIo2025(String name, SparkFlex motor) {
         super(name);
         this.motor = motor;
         this.encoder = motor.getEncoder();
         this.motorController = motor.getClosedLoopController();
-        // this.limitSwitch = motor.getReverseLimitSwitch();
-        this.dioLimitSwitch = new DigitalInput(8);
+        this.limitSwitch = motor.getReverseLimitSwitch();
+
 
 
         SparkFlexConfig motorConfig = new SparkFlexConfig();
         motorConfig.idleMode(IdleMode.kBrake);
         motorConfig.closedLoop.maxMotion.maxVelocity(heightToMotorRotations(MAX_VELOCITY) * 60);
         motorConfig.closedLoop.maxMotion.maxAcceleration(heightToMotorRotations(MAX_ACCEL) * 60);
-        motorConfig.closedLoop.pid(1, 0, 0); // TODO: set these later
+        motorConfig.closedLoop.pid(.08
+        , 0, 0); // TODO: set these later
         motorConfig.inverted(false);
 
         motorConfig.softLimit.forwardSoftLimit(heightToMotorRotations(MAX_HEIGHT));
@@ -56,19 +57,20 @@ public ElevatorIo2025(String name, SparkFlex motor) {
 
     @Override
     protected void updateInputs(ElevatorInputs inputs) {
-        // inputs.lowerLimitSwitch = limitSwitch.isPressed();
-        inputs.lowerLimitSwitch = !dioLimitSwitch.get();
+        inputs.lowerLimitSwitch = limitSwitch.isPressed();
         inputs.currentPosition = motorRotationsToHeight(encoder.getPosition());
         inputs.motorCurrent = motor.getOutputCurrent();
     }
 
     @Override
     public void periodic() {
         super.periodic();
-        // if (limitSwitch.isPressed()) {
-        if(dioLimitSwitch.get()){
+        boolean isPressed = limitSwitch.isPressed();
+         if (isPressed && !lastLimitSwitchState) { //Rising edge
             encoder.setPosition(0);
         }
+
+        lastLimitSwitchState = isPressed;
     }
 
     @Override
@@ -79,10 +81,10 @@ public void setTargetPosition(double targetHeight) {
     }
 
     private double motorRotationsToHeight(double motorRotations) {
-        return motorRotations / GEAR_RATIO * GEAR_TEETH * RACK_SPACING;
+        return motorRotations / GEAR_RATIO * GEAR_TEETH * RACK_SPACING * NUM_STAGES;
     }
 
     private double heightToMotorRotations(double height) {
-        return height / RACK_SPACING / GEAR_TEETH * GEAR_RATIO;
+        return height / RACK_SPACING / GEAR_TEETH * GEAR_RATIO / NUM_STAGES;
     }
 }