Untested changes to automate climb

frc1444 · Mar 12, 2020 · ffc6a5a · ffc6a5a
1 parent c26d680
commit ffc6a5a
Show file tree

Hide file tree

Showing 7 changed files with 90 additions and 33 deletions.
diff --git a/core/src/main/java/com/first1444/frc/robot2020/actions/OperatorAction.java b/core/src/main/java/com/first1444/frc/robot2020/actions/OperatorAction.java
@@ -149,9 +149,12 @@ protected void onUpdate() {
                 }
                 wantsToClimb = true;
             }
-//            if(input.getClimbStarting().isDown()){
-//                robot.getClimber().startingPosition(3.0);
-//            }
+            if(input.getClimbClimbingPosition().isDown()){
+                if(clearToClimb){
+                    robot.getClimber().climbingPosition();
+                }
+                wantsToClimb = true;
+            }
             if (wantsToClimb && (!clearToClimb || autoDown)) { // if it's not clear to climb or auto targeting is enabled
                 input.getDriverRumble().rumbleTimeout(500, 1.0);
             }

diff --git a/core/src/main/java/com/first1444/frc/robot2020/input/RobotInput.kt b/core/src/main/java/com/first1444/frc/robot2020/input/RobotInput.kt
@@ -87,10 +87,9 @@ class RobotInput(
     // region Climb Controls
     val climbStored: InputPart
         get() = attackJoystick.leftLower
-
     /** If pressed, should make the climber go to the starting position */
-    val climbStarting: InputPart
-        get() = attackJoystick.rightUpper
+    val climbClimbingPosition: InputPart
+        get() = attackJoystick.thumbLower
 
     /** An input part that makes the climber move up and down manually. */
     val climbRawControl: InputPart

diff --git a/core/src/main/java/com/first1444/frc/robot2020/subsystems/Climber.java b/core/src/main/java/com/first1444/frc/robot2020/subsystems/Climber.java
@@ -7,7 +7,11 @@ public interface Climber extends Runnable {
     void setRawSpeed(double speed);
 
     void storedPosition(double timeoutSeconds);
-    void startingPosition(double timeoutSeconds);
+
+    /**
+     * Must be called repeatedly
+     */
+    void climbingPosition();
 
     boolean isStored();
 

diff --git a/core/src/main/java/com/first1444/frc/robot2020/subsystems/implementations/BaseClimber.java b/core/src/main/java/com/first1444/frc/robot2020/subsystems/implementations/BaseClimber.java
@@ -7,19 +7,24 @@
 
 public abstract class BaseClimber implements Climber {
 
+    private enum Mode {
+        MANUAL,
+        STORED_POSITION,
+        CLIMBING_POSITION
+    }
+
     protected final Clock clock;
 
     private double speed = 0.0;
-    private boolean startingPosition = false;
-    private boolean storedPosition = false;
+    private Mode mode = Mode.MANUAL;
     private double timeoutTime = 0.0;
 
     protected BaseClimber(Clock clock) {
         this.clock = clock;
     }
 
     protected abstract void useSpeed(double speed);
-    protected abstract void goToStartingPosition();
+    protected abstract void goToClimbingPosition();
     protected abstract void goToStoredPosition();
 
     @Override
@@ -28,38 +33,35 @@ public final void setRawSpeed(double speed) {
             throw new IllegalArgumentException("speed is out of range! speed=" + speed);
         }
         this.speed = speed;
-        startingPosition = false;
-        storedPosition = false;
+        mode = Mode.MANUAL;
     }
 
     @Override
-    public final void startingPosition(double timeoutSeconds) {
-        startingPosition = true;
-        storedPosition = false;
-        timeoutTime = clock.getTimeSeconds() + timeoutSeconds;
+    public final void climbingPosition() {
+        mode = Mode.CLIMBING_POSITION;
     }
 
     @Override
     public final void storedPosition(double timeoutSeconds) {
-        startingPosition = false;
-        storedPosition = true;
+        mode = Mode.STORED_POSITION;
         timeoutTime = clock.getTimeSeconds() + timeoutSeconds;
     }
 
     @Override
     public void run() {
+        final Mode mode = this.mode;
         final double speed = this.speed;
         this.speed = 0.0;
-        if(clock.getTimeSeconds() > timeoutTime){
-            startingPosition = false;
-            storedPosition = false;
-        }
-        if(startingPosition){
-            goToStartingPosition();;
-        } else if(storedPosition){
+        if(mode == Mode.MANUAL){
+            useSpeed(speed);
+        } else if(mode == Mode.STORED_POSITION){
+            if(clock.getTimeSeconds() > timeoutTime){
+                this.mode = Mode.MANUAL;
+            }
             goToStoredPosition();
         } else {
-            useSpeed(speed);
+            assert mode == Mode.CLIMBING_POSITION;
+            goToClimbingPosition();
         }
     }
 }
diff --git a/core/src/main/java/com/first1444/frc/robot2020/subsystems/implementations/DummyClimber.java b/core/src/main/java/com/first1444/frc/robot2020/subsystems/implementations/DummyClimber.java
@@ -24,8 +24,8 @@ protected void useSpeed(double speed) {
     }
 
     @Override
-    protected void goToStartingPosition() {
-        report("Starting position");
+    protected void goToClimbingPosition() {
+        report("Climbing Position");
         stored = false;
     }
 

diff --git a/docs/wpi_links.md b/docs/wpi_links.md
@@ -8,5 +8,22 @@
 ### Misc
 * IP Addresses/Network Stuff: https://docs.wpilib.org/en/latest/docs/networking/networking-introduction/roborio-network-troubleshooting.html
   * `roboRIO-1444-FRC.local`, `10.14.44.2`, `172.22.11.2`
+### Networking on Linux
+```shell script
+sudo vi /etc/network/interfaces
+```
+add this to bottom:
+```
+iface eno1 inet static
+address 10.14.44.<NUMBER BETWEEN 6 AND 9 I GUESS>
+netmask 255.255.255.0
+gateway 10.14.44.1 # may not actually need this, I accidentally put 10.44.44.1 and it still worked
+```
+now run this:
+```shell script
+sudo ifdown eno1 && sudo ifup eno1
+```
+NOTE: `eno1` should be replaced with whatever your ethernet thing is
+
 ### CTRE Links:
 TODO
diff --git a/wpi/src/main/java/com/first1444/frc/robot2020/MotorClimber.java b/wpi/src/main/java/com/first1444/frc/robot2020/MotorClimber.java
@@ -10,13 +10,24 @@
 import com.revrobotics.CANSparkMaxLowLevel;
 import edu.wpi.first.wpilibj.DigitalInput;
 
+import static java.util.Objects.requireNonNull;
+
 public class MotorClimber extends BaseClimber {
+    private static final double LOCK_ENCODER_COUNTS = 205.0;
+    private static final double UNLATCH_STAGE_2_COUNTS = 190.0; // TODO get encoder value
+    private static final double RESET_TO_LOW_STATE_COUNTS = 170.0;
+    private enum State {
+        LOW,
+        UNLATCHING,
+        FREE_TO_CLIMB
+    }
     private final CANSparkMax motor;
     private final CANEncoder encoder;
     private final DigitalInput reverseLimitSwitch;
 
     private final DigitalInput intakeLimitSwitch;
     private Double lastIntakeLimitSwitchPress = null;
+    private State state = State.LOW;
 
     public MotorClimber(Clock clock, DashboardMap dashboardMap) {
         super(clock);
@@ -29,8 +40,6 @@ public MotorClimber(Clock clock, DashboardMap dashboardMap) {
         motor.setSmartCurrentLimit(200); // default is 80
 
         encoder = motor.getEncoder();
-//        motor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true);
-//        motor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, 100000); // TODO get correct value
         dashboardMap.getDebugTab().add("Climb Encoder", new PropertyComponent(ValueProperty.createGetOnly(() -> BasicValue.makeDouble(encoder.getPosition()))));
         dashboardMap.getDevTab().add("Intake Down Limit", new PropertyComponent(ValueProperty.createGetOnly(() -> BasicValue.makeBoolean(isIntakeDown()))));
     }
@@ -47,13 +56,28 @@ private boolean isIntakeLimitSwitchPressed(){
 
     @Override
     public void run() {
-        super.run();
         if(isReverseLimitSwitchPressed()){
             encoder.setPosition(0.0);
         }
         if(isIntakeLimitSwitchPressed()){
             lastIntakeLimitSwitchPress = clock.getTimeSeconds();
         }
+        double counts = encoder.getPosition();
+        if(counts <= RESET_TO_LOW_STATE_COUNTS){
+            state = State.LOW;
+        } else {
+            final State state = requireNonNull(this.state);
+            if(state == State.LOW){
+                if(counts >= LOCK_ENCODER_COUNTS){
+                    this.state = State.UNLATCHING;
+                }
+            } else if(state == State.UNLATCHING){
+                if(counts <= UNLATCH_STAGE_2_COUNTS){
+                    this.state = State.FREE_TO_CLIMB;
+                }
+            }
+        }
+        super.run();
     }
 
     @Override
@@ -75,8 +99,16 @@ protected void goToStoredPosition() {
     }
 
     @Override
-    protected void goToStartingPosition() {
-        motor.set(0.0);
+    protected void goToClimbingPosition() {
+        State state = requireNonNull(this.state);
+        if(state == State.LOW){
+            motor.set(1.0);
+        } else if(state == State.UNLATCHING){
+            motor.set(-1.0);
+        } else {
+            assert state == State.FREE_TO_CLIMB;
+            motor.set(1.0);
+        }
     }
 
     @Override