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 index eb95c20..72ea2ca 100644 --- 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 index 9344671..c646383 100644 --- 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 index 30f84ae..6576d60 100644 --- 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 index 588fc7f..afc083d 100644 --- 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,11 +7,16 @@ 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) { @@ -19,7 +24,7 @@ protected BaseClimber(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 index 2a0fce1..31a7421 100644 --- 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 index e49e3cf..16abe50 100644 --- 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. +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 index 1c0d758..fd90553 100644 --- 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