From c6d9c7de59f7dd04d8c7701b33f30df6a8e0fe53 Mon Sep 17 00:00:00 2001 From: selisine Date: Sat, 15 Jan 2022 16:23:05 -0500 Subject: [PATCH 01/66] Add Climbing.java Climbing plan made just need to code it --- src/main/java/frc/robot/Climbing.java | 39 +++++ vendordeps/Phoenix.json | 214 ++++++++++++++++++++++++++ 2 files changed, 253 insertions(+) create mode 100644 src/main/java/frc/robot/Climbing.java create mode 100644 vendordeps/Phoenix.json diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java new file mode 100644 index 0000000..91cef78 --- /dev/null +++ b/src/main/java/frc/robot/Climbing.java @@ -0,0 +1,39 @@ +package frc.robot; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class Climbing implements Loggable { + TalonFX climbingMotor; + public Climbing(int climbingMotorID) { + this.climbingMotor = new TalonFX(climbingMotorID); + } + + @Override + public void setupLogging(Logger logger) { + // TODO Auto-generated method stub + + } + + @Override + public void log(Logger logger) { + // TODO Auto-generated method stub + + } +} + +//Upon lineup of Bar A shadow tape: +/* +- Press button to run climbing function (e.g. Press 'A' button) + [1] - Measure encoder count/total encoder counts = total rotations + [1] - Rotate to certain point to hit angle for gripping first bar + - Activate first solenoid a very small time after the motor is at the correct rotation + - Repeat [1] with different measurements + - Activate second solenoid a very small time after the motor is at the correct rotation + - Repeat [1] with different measurements + - Activate third solenoid a very small time after the motor is at the correct rotation +*/ \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..87f03cb --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,214 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.19.4", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "https://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.19.4" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.19.4" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.19.4", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.19.4", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.19.4", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.19.4", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.19.4", + "libName": "CTRE_PhoenixDiagnostics", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.19.4", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-sim", + "version": "5.19.4", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.19.4", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.19.4", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.19.4", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file From 6970b434589a4fa0abcb471bfed3842ccf70a0ba Mon Sep 17 00:00:00 2001 From: selisine Date: Mon, 17 Jan 2022 20:57:20 -0500 Subject: [PATCH 02/66] changed Climbing.java Solenoid staging, climbing states, and sensors and motor IDs added --- src/main/java/frc/robot/Climbing.java | 68 +++++++++++++++++++++++++-- src/main/java/frc/robot/Robot.java | 1 + 2 files changed, 64 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java index 91cef78..9045239 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climbing.java @@ -1,16 +1,74 @@ package frc.robot; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; import frc.robot.logging.Logger; +enum ClimbingStates { + RESTING, PRE_STAGE, FIRST_STAGE, SECOND_STAGE, THIRD_STAGE; +} + public class Climbing implements Loggable { - TalonFX climbingMotor; - public Climbing(int climbingMotorID) { + TalonFX climbingMotor, secondaryClimbingMotor; + Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; + ClimbingStates currentStage = ClimbingStates.RESTING; + public Climbing(int climbingMotorID, int secondaryClimbingMotorID, + int climberSolenoidAID, int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, + int collisionSensorA, int collisionSensorB) { this.climbingMotor = new TalonFX(climbingMotorID); + this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); + + this.climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidAID); + this.climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB1ID); + this.climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB2ID); + this.climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidCID); + + secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); + secondaryClimbingMotor.follow(climbingMotor); + } + + public void setClimbingState(ClimbingStates climbingState) { + this.currentStage = climbingState; + switch (climbingState) { + case FIRST_STAGE: + climberSolenoidA.set(false); + climberSolenoidB1.set(true); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + break; + case SECOND_STAGE: + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + break; + case THIRD_STAGE: + climberSolenoidA.set(true); + climberSolenoidB1.set(true); + climberSolenoidB2.set(true); + climberSolenoidC.set(false); + break; + default: + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidC.set(false); + break; + } + } + /** + * Updates the current state of the climber. + * @return true when current state is RESTING + */ + public boolean checkClimbingState() { + // Check whether or not the climber is done climbing during the current stage. + return this.currentStage == ClimbingStates.RESTING; } @Override @@ -28,12 +86,12 @@ public void log(Logger logger) { //Upon lineup of Bar A shadow tape: /* -- Press button to run climbing function (e.g. Press 'A' button) - [1] - Measure encoder count/total encoder counts = total rotations +- Press button to run climbing function (e.g. Press 'A' button) && When buttons triggered on climbing mechanism + [1] - Measure encoder count/total encoder counts = total rotations [1] - Rotate to certain point to hit angle for gripping first bar - Activate first solenoid a very small time after the motor is at the correct rotation - Repeat [1] with different measurements - - Activate second solenoid a very small time after the motor is at the correct rotation + - Activate second (double) solenoid a very small time after the motor is at the correct rotation - Repeat [1] with different measurements - Activate third solenoid a very small time after the motor is at the correct rotation */ \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 00df059..9a4f392 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -68,6 +68,7 @@ public void teleopInit() { @Override public void teleopPeriodic() { + // Robot code goes here logger.log(); logger.writeLine(); From ebe30f53e2a7ad6bdf62ba16eeb42f3776fea264 Mon Sep 17 00:00:00 2001 From: Aiden Pike <61250126+aidenpike@users.noreply.github.com> Date: Tue, 18 Jan 2022 20:17:03 -0500 Subject: [PATCH 03/66] Update Climbing.java Staging added. --- src/main/java/frc/robot/Climbing.java | 70 ++++++++++++++++++++++++--- 1 file changed, 64 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java index 91cef78..b736615 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climbing.java @@ -1,16 +1,74 @@ package frc.robot; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; import frc.robot.logging.Logger; +enum ClimbingStates { + RESTING, PRE_STAGE, FIRST_STAGE, SECOND_STAGE, THIRD_STAGE; +} + public class Climbing implements Loggable { - TalonFX climbingMotor; - public Climbing(int climbingMotorID) { + TalonFX climbingMotor, secondaryClimbingMotor; + Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; + ClimbingStates currentStage = ClimbingStates.RESTING; + public Climbing(int climbingMotorID, int secondaryClimbingMotorID, + int climberSolenoidAID, int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, + int collisionSensorA, int collisionSensorB) { this.climbingMotor = new TalonFX(climbingMotorID); + this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); + + this.climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidAID); + this.climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB1ID); + this.climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB2ID); + this.climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidCID); + + secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); + secondaryClimbingMotor.follow(climbingMotor); + } + + public void setClimbingState(ClimbingStates climbingState) { + this.currentStage = climbingState; + switch (climbingState) { + case FIRST_STAGE: + climberSolenoidA.set(false); + climberSolenoidB1.set(true); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + break; + case SECOND_STAGE: + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + break; + case THIRD_STAGE: + climberSolenoidA.set(true); + climberSolenoidB1.set(true); + climberSolenoidB2.set(true); + climberSolenoidC.set(false); + break; + default: + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidC.set(false); + break; + } + } + /** + * Updates the current state of the climber. + * @return true when current state is RESTING + */ + public boolean checkClimbingState() { + // Check whether or not the climber is done climbing during the current stage. + return this.currentStage == ClimbingStates.RESTING; } @Override @@ -28,12 +86,12 @@ public void log(Logger logger) { //Upon lineup of Bar A shadow tape: /* -- Press button to run climbing function (e.g. Press 'A' button) - [1] - Measure encoder count/total encoder counts = total rotations +- Press button to run climbing function (e.g. Press 'A' button) && When buttons triggered on climbing mechanism + [1] - Measure encoder count/total encoder counts = total rotations [1] - Rotate to certain point to hit angle for gripping first bar - Activate first solenoid a very small time after the motor is at the correct rotation - Repeat [1] with different measurements - - Activate second solenoid a very small time after the motor is at the correct rotation + - Activate second (double) solenoid a very small time after the motor is at the correct rotation - Repeat [1] with different measurements - Activate third solenoid a very small time after the motor is at the correct rotation -*/ \ No newline at end of file +*/ From 75e6903bfbcf9629b9a43edcff8c37dc1efbf4c5 Mon Sep 17 00:00:00 2001 From: selisine Date: Thu, 20 Jan 2022 19:30:17 -0500 Subject: [PATCH 04/66] Refactor LoggableTimer to accept name Co-authored-by: theblindbandet --- src/main/java/frc/robot/logging/LoggableTimer.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/logging/LoggableTimer.java b/src/main/java/frc/robot/logging/LoggableTimer.java index 0cb8d81..938ae07 100644 --- a/src/main/java/frc/robot/logging/LoggableTimer.java +++ b/src/main/java/frc/robot/logging/LoggableTimer.java @@ -1,17 +1,26 @@ package frc.robot.logging; +import java.util.function.LongBinaryOperator; + import edu.wpi.first.wpilibj.Timer; public class LoggableTimer extends Timer implements Loggable { + String name; + public LoggableTimer() { + this("Time"); + } + public LoggableTimer(String name) { + this.name = name; + } @Override public void setupLogging(Logger logger) { - logger.addAttribute("Time"); + logger.addAttribute(this.name); } @Override public void log(Logger logger) { - logger.log("Time", this.get()); + logger.log(this.name, this.get()); } } From ed360cf6d282c4060e2458dc697f145bb9eeaf51 Mon Sep 17 00:00:00 2001 From: selisine Date: Thu, 20 Jan 2022 19:57:12 -0500 Subject: [PATCH 05/66] Implement timer and cycle algorithm Co-authored-by: theblindbandet Co-authored-by: ultragamer1010 Co-authored-by: HungryIronApple --- src/main/java/frc/robot/Climbing.java | 76 ++++++++++++++++++++------- 1 file changed, 56 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java index b736615..442e639 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climbing.java @@ -9,16 +9,18 @@ import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; +import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; enum ClimbingStates { - RESTING, PRE_STAGE, FIRST_STAGE, SECOND_STAGE, THIRD_STAGE; + RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; } public class Climbing implements Loggable { TalonFX climbingMotor, secondaryClimbingMotor; Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; ClimbingStates currentStage = ClimbingStates.RESTING; + LoggableTimer timer; public Climbing(int climbingMotorID, int secondaryClimbingMotorID, int climberSolenoidAID, int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, int collisionSensorA, int collisionSensorB) { @@ -32,34 +34,56 @@ public Climbing(int climbingMotorID, int secondaryClimbingMotorID, secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); + this.timer = new LoggableTimer("Climbing/Time"); + } public void setClimbingState(ClimbingStates climbingState) { this.currentStage = climbingState; + switch (climbingState) { - case FIRST_STAGE: + case PRE_STAGE: + this.timer.start(); + break; + case TOUCH_A: climberSolenoidA.set(false); climberSolenoidB1.set(true); climberSolenoidB2.set(false); climberSolenoidC.set(true); break; - case SECOND_STAGE: + case TRANS_AB: + this.timer.reset(); + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + break; + case TOUCH_B: climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); break; - case THIRD_STAGE: + case TRANS_BC: + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(false); + break; + case TOUCH_C: climberSolenoidA.set(true); climberSolenoidB1.set(true); climberSolenoidB2.set(true); climberSolenoidC.set(false); break; - default: + case RESTING: climberSolenoidA.set(false); climberSolenoidB1.set(false); + climberSolenoidB2.set(false); climberSolenoidC.set(false); break; + default: + break; } } /** @@ -68,30 +92,42 @@ public void setClimbingState(ClimbingStates climbingState) { */ public boolean checkClimbingState() { // Check whether or not the climber is done climbing during the current stage. + switch (currentStage) { + case PRE_STAGE: + // Check if A is touching yet. + case TOUCH_A: + // Check if B is touching yet. + break; + case TRANS_AB: + if (timer.hasElapsed(1)) { + this.setClimbingState(ClimbingStates.TOUCH_B); + } + break; + case TOUCH_B: + // Check if C is touching yet. + case TRANS_BC: + if (timer.hasElapsed(1)) { + this.setClimbingState(ClimbingStates.TOUCH_C); + } + break; + case TOUCH_C: + // Success! \o/ + break; + default: + break; + } return this.currentStage == ClimbingStates.RESTING; } @Override public void setupLogging(Logger logger) { - // TODO Auto-generated method stub - + this.timer.setupLogging(logger); + } @Override public void log(Logger logger) { - // TODO Auto-generated method stub + this.timer.log(logger); } } - -//Upon lineup of Bar A shadow tape: -/* -- Press button to run climbing function (e.g. Press 'A' button) && When buttons triggered on climbing mechanism - [1] - Measure encoder count/total encoder counts = total rotations - [1] - Rotate to certain point to hit angle for gripping first bar - - Activate first solenoid a very small time after the motor is at the correct rotation - - Repeat [1] with different measurements - - Activate second (double) solenoid a very small time after the motor is at the correct rotation - - Repeat [1] with different measurements - - Activate third solenoid a very small time after the motor is at the correct rotation -*/ From b7a0dc9fd2edc8a6b5ee38647c58d73bdf127bb2 Mon Sep 17 00:00:00 2001 From: selisine Date: Thu, 20 Jan 2022 20:17:48 -0500 Subject: [PATCH 06/66] Add ClimbingSensors class --- src/main/java/frc/robot/ClimbingSensors.java | 32 ++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/main/java/frc/robot/ClimbingSensors.java diff --git a/src/main/java/frc/robot/ClimbingSensors.java b/src/main/java/frc/robot/ClimbingSensors.java new file mode 100644 index 0000000..f01872c --- /dev/null +++ b/src/main/java/frc/robot/ClimbingSensors.java @@ -0,0 +1,32 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class ClimbingSensors implements Loggable { + String name; + DigitalInput a, b; + + public ClimbingSensors(String name, int aID, int bID) { + this.name = name; + this.a = new DigitalInput(aID); + this.b = new DigitalInput(bID); + } + + public boolean get() { + return this.a.get() && this.b.get(); + } + + @Override + public void setupLogging(Logger logger) { + logger.log(this.name, this.get()); + + } + + @Override + public void log(Logger logger) { + // TODO Auto-generated method stub + + } +} From 91f05a3014e43735454c18a8508508d7c7066d1d Mon Sep 17 00:00:00 2001 From: selisine Date: Thu, 20 Jan 2022 20:34:14 -0500 Subject: [PATCH 07/66] Implement ClimbingSensors --- src/main/java/frc/robot/Climbing.java | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java index 442e639..617d785 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climbing.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; +import frc.robot.ClimbingSensors; import frc.robot.logging.Loggable; import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; @@ -19,11 +20,12 @@ enum ClimbingStates { public class Climbing implements Loggable { TalonFX climbingMotor, secondaryClimbingMotor; Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; + ClimbingSensors touchA, touchB, touchC; ClimbingStates currentStage = ClimbingStates.RESTING; LoggableTimer timer; public Climbing(int climbingMotorID, int secondaryClimbingMotorID, int climberSolenoidAID, int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, - int collisionSensorA, int collisionSensorB) { + ClimbingSensors touchA, ClimbingSensors touchB, ClimbingSensors touchC) { this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -32,6 +34,10 @@ public Climbing(int climbingMotorID, int secondaryClimbingMotorID, this.climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB2ID); this.climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidCID); + this.touchA = touchA; + this.touchB = touchB; + this.touchC = touchC; + secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climbing/Time"); @@ -95,8 +101,14 @@ public boolean checkClimbingState() { switch (currentStage) { case PRE_STAGE: // Check if A is touching yet. + if (touchA.get()){ + this.setClimbingState(ClimbingStates.TOUCH_A); + } case TOUCH_A: // Check if B is touching yet. + if (touchB.get()){ + this.setClimbingState(ClimbingStates.TRANS_AB); + } break; case TRANS_AB: if (timer.hasElapsed(1)) { @@ -105,6 +117,9 @@ public boolean checkClimbingState() { break; case TOUCH_B: // Check if C is touching yet. + if (touchC.get()){ + this.setClimbingState(ClimbingStates.TRANS_BC); + } case TRANS_BC: if (timer.hasElapsed(1)) { this.setClimbingState(ClimbingStates.TOUCH_C); From b91979bee062fa612f5b0a9471737fe79fc06d7d Mon Sep 17 00:00:00 2001 From: selisine Date: Thu, 20 Jan 2022 20:42:07 -0500 Subject: [PATCH 08/66] Add climberEnabled --- src/main/java/frc/robot/Robot.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9a4f392..0c7a542 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,9 +35,25 @@ public class Robot extends TimedRobot { public void robotInit() { pdp = new LoggablePowerDistribution(1, ModuleType.kRev); - driver = new LoggableController("Driver", 0); + climber = new LoggableController("Climber", 0); operator = new LoggableController("Operator", 1); + if(this.climberEnabled) { + System.out.println("Initializing climber..."); + ClimbingSensors touchA = new ClimbingSensors("TouchA", 0, 1); + ClimbingSensors touchB = new ClimbingSensors("TouchB", 0, 1); + ClimbingSensors touchC = new ClimbingSensors("TouchC", 0, 1); + climb = new ClimbingSensors(touchA, touchB, touchC); + + logger.addLoggable(touchA); + logger.addLoggable(touchB); + logger.addLoggable(touchC); + logger.addLoggable(climb); + } else { + System.out.println("Drivetrain initialization disabled."); + } + + logger = new Logger(); timer = new LoggableTimer(); From 0636f20a369dd8b1a5f231d4fb98df15b050c40b Mon Sep 17 00:00:00 2001 From: HungryIronApple Date: Sat, 22 Jan 2022 11:06:51 -0500 Subject: [PATCH 09/66] Declared climbing variables --- src/main/java/frc/robot/Robot.java | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0c7a542..0dc5b3a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -27,6 +27,10 @@ public class Robot extends TimedRobot { LoggablePowerDistribution pdp; + Climbing climb; + + boolean climberEnabled = true; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -35,22 +39,22 @@ public class Robot extends TimedRobot { public void robotInit() { pdp = new LoggablePowerDistribution(1, ModuleType.kRev); - climber = new LoggableController("Climber", 0); + driver = new LoggableController("Driver", 0); operator = new LoggableController("Operator", 1); if(this.climberEnabled) { System.out.println("Initializing climber..."); ClimbingSensors touchA = new ClimbingSensors("TouchA", 0, 1); - ClimbingSensors touchB = new ClimbingSensors("TouchB", 0, 1); - ClimbingSensors touchC = new ClimbingSensors("TouchC", 0, 1); - climb = new ClimbingSensors(touchA, touchB, touchC); + ClimbingSensors touchB = new ClimbingSensors("TouchB", 2, 3); + ClimbingSensors touchC = new ClimbingSensors("TouchC", 4, 5); + climb = new Climbing(9, 10, 2, 3, 4, 5, touchA, touchB, touchC); logger.addLoggable(touchA); logger.addLoggable(touchB); logger.addLoggable(touchC); logger.addLoggable(climb); } else { - System.out.println("Drivetrain initialization disabled."); + System.out.println("Climber initialization disabled."); } From eca328d7a717645ae3453f353081dc8dfdbd39bb Mon Sep 17 00:00:00 2001 From: HungryIronApple Date: Sat, 22 Jan 2022 11:12:25 -0500 Subject: [PATCH 10/66] Added setPower method --- src/main/java/frc/robot/Climbing.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java index 617d785..0d93162 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climbing.java @@ -96,7 +96,7 @@ public void setClimbingState(ClimbingStates climbingState) { * Updates the current state of the climber. * @return true when current state is RESTING */ - public boolean checkClimbingState() { + public void checkClimbingState() { // Check whether or not the climber is done climbing during the current stage. switch (currentStage) { case PRE_STAGE: @@ -131,7 +131,10 @@ public boolean checkClimbingState() { default: break; } - return this.currentStage == ClimbingStates.RESTING; + } + + public void setPower(double power) { + climbingMotor.set(ControlMode.PercentOutput, power); } @Override From 22618781db483cfbd4d0ebf88b33399898c9d3b2 Mon Sep 17 00:00:00 2001 From: HungryIronApple Date: Sat, 22 Jan 2022 11:21:29 -0500 Subject: [PATCH 11/66] Add climber controls --- src/main/java/frc/robot/Climbing.java | 6 ++++++ src/main/java/frc/robot/Robot.java | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climbing.java index 0d93162..d909587 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climbing.java @@ -133,6 +133,12 @@ public void checkClimbingState() { } } + public void setPrestage(boolean stage) { + if (stage) { + this.setClimbingState(ClimbingStates.PRE_STAGE); + } + } + public void setPower(double power) { climbingMotor.set(ControlMode.PercentOutput, power); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0dc5b3a..8a7ad2e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,6 +114,12 @@ public void testInit() { @Override public void testPeriodic() { // Robot code goes here + if (climberEnabled) { + climb.setPrestage(operator.getXButtonPressed()); + climb.setPower(operator.getRightY()); //Deadband + climb.checkClimbingState(); + } + logger.log(); logger.writeLine(); } From e432045076ba920dde2e433fb117a94a1724defb Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 18 Feb 2022 19:40:49 -0500 Subject: [PATCH 12/66] LoggableTimer formatting --- src/main/java/frc/robot/logging/LoggableTimer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/logging/LoggableTimer.java b/src/main/java/frc/robot/logging/LoggableTimer.java index 938ae07..3e6c128 100644 --- a/src/main/java/frc/robot/logging/LoggableTimer.java +++ b/src/main/java/frc/robot/logging/LoggableTimer.java @@ -1,14 +1,14 @@ package frc.robot.logging; -import java.util.function.LongBinaryOperator; - import edu.wpi.first.wpilibj.Timer; public class LoggableTimer extends Timer implements Loggable { String name; + public LoggableTimer() { this("Time"); } + public LoggableTimer(String name) { this.name = name; } From 801fc3383b59b4860ba250a187eea231f7bdb767 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 18 Feb 2022 20:02:35 -0500 Subject: [PATCH 13/66] Climbing -> Climber and ClimbingSensors -> ClimberSensors --- .../frc/robot/{Climbing.java => Climber.java} | 18 +++++++----------- ...limbingSensors.java => ClimberSensors.java} | 10 +++++----- src/main/java/frc/robot/Robot.java | 12 ++++++------ 3 files changed, 18 insertions(+), 22 deletions(-) rename src/main/java/frc/robot/{Climbing.java => Climber.java} (92%) rename src/main/java/frc/robot/{ClimbingSensors.java => ClimberSensors.java} (81%) diff --git a/src/main/java/frc/robot/Climbing.java b/src/main/java/frc/robot/Climber.java similarity index 92% rename from src/main/java/frc/robot/Climbing.java rename to src/main/java/frc/robot/Climber.java index d909587..9ff39a3 100644 --- a/src/main/java/frc/robot/Climbing.java +++ b/src/main/java/frc/robot/Climber.java @@ -2,13 +2,9 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; - -import frc.robot.ClimbingSensors; import frc.robot.logging.Loggable; import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; @@ -17,15 +13,15 @@ enum ClimbingStates { RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; } -public class Climbing implements Loggable { +public class Climber implements Loggable { TalonFX climbingMotor, secondaryClimbingMotor; Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; - ClimbingSensors touchA, touchB, touchC; + ClimberSensors touchA, touchB, touchC; ClimbingStates currentStage = ClimbingStates.RESTING; LoggableTimer timer; - public Climbing(int climbingMotorID, int secondaryClimbingMotorID, + public Climber(int climbingMotorID, int secondaryClimbingMotorID, int climberSolenoidAID, int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, - ClimbingSensors touchA, ClimbingSensors touchB, ClimbingSensors touchC) { + ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -41,12 +37,12 @@ public Climbing(int climbingMotorID, int secondaryClimbingMotorID, secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climbing/Time"); - + } public void setClimbingState(ClimbingStates climbingState) { this.currentStage = climbingState; - + switch (climbingState) { case PRE_STAGE: this.timer.start(); @@ -152,6 +148,6 @@ public void setupLogging(Logger logger) { @Override public void log(Logger logger) { this.timer.log(logger); - + } } diff --git a/src/main/java/frc/robot/ClimbingSensors.java b/src/main/java/frc/robot/ClimberSensors.java similarity index 81% rename from src/main/java/frc/robot/ClimbingSensors.java rename to src/main/java/frc/robot/ClimberSensors.java index f01872c..a580bad 100644 --- a/src/main/java/frc/robot/ClimbingSensors.java +++ b/src/main/java/frc/robot/ClimberSensors.java @@ -4,11 +4,11 @@ import frc.robot.logging.Loggable; import frc.robot.logging.Logger; -public class ClimbingSensors implements Loggable { +public class ClimberSensors implements Loggable { String name; DigitalInput a, b; - public ClimbingSensors(String name, int aID, int bID) { + public ClimberSensors(String name, int aID, int bID) { this.name = name; this.a = new DigitalInput(aID); this.b = new DigitalInput(bID); @@ -17,16 +17,16 @@ public ClimbingSensors(String name, int aID, int bID) { public boolean get() { return this.a.get() && this.b.get(); } - + @Override public void setupLogging(Logger logger) { logger.log(this.name, this.get()); - + } @Override public void log(Logger logger) { // TODO Auto-generated method stub - + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bb5fae0..2d25ba3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,12 +47,12 @@ public double deadband(double in) { return joystickDeadband.scale(out); } - Climbing climb; + Climber climb; boolean climberEnabled = true; /** - * This function is run when the robot is first started up and should be used for any + * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override @@ -64,10 +64,10 @@ public void robotInit() { if (this.climberEnabled) { System.out.println("Initializing climber..."); - ClimbingSensors touchA = new ClimbingSensors("TouchA", 0, 1); - ClimbingSensors touchB = new ClimbingSensors("TouchB", 2, 3); - ClimbingSensors touchC = new ClimbingSensors("TouchC", 4, 5); - climb = new Climbing(9, 10, 2, 3, 4, 5, touchA, touchB, touchC); + ClimberSensors touchA = new ClimberSensors("TouchA", 0, 1); + ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); + ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); + climb = new Climber(9, 10, 2, 3, 4, 5, touchA, touchB, touchC); logger.addLoggable(touchA); logger.addLoggable(touchB); From 187354a8d9e0886bc6b20cd44ad638863337eb34 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 18 Feb 2022 20:05:58 -0500 Subject: [PATCH 14/66] More auto formatting --- src/main/java/frc/robot/Climber.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 9ff39a3..3f66f8a 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -13,15 +13,17 @@ enum ClimbingStates { RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; } + public class Climber implements Loggable { TalonFX climbingMotor, secondaryClimbingMotor; Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; ClimberSensors touchA, touchB, touchC; ClimbingStates currentStage = ClimbingStates.RESTING; LoggableTimer timer; - public Climber(int climbingMotorID, int secondaryClimbingMotorID, - int climberSolenoidAID, int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, - ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { + + public Climber(int climbingMotorID, int secondaryClimbingMotorID, int climberSolenoidAID, + int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, + ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -88,8 +90,10 @@ public void setClimbingState(ClimbingStates climbingState) { break; } } + /** * Updates the current state of the climber. + * * @return true when current state is RESTING */ public void checkClimbingState() { @@ -97,12 +101,12 @@ public void checkClimbingState() { switch (currentStage) { case PRE_STAGE: // Check if A is touching yet. - if (touchA.get()){ + if (touchA.get()) { this.setClimbingState(ClimbingStates.TOUCH_A); } case TOUCH_A: // Check if B is touching yet. - if (touchB.get()){ + if (touchB.get()) { this.setClimbingState(ClimbingStates.TRANS_AB); } break; @@ -113,7 +117,7 @@ public void checkClimbingState() { break; case TOUCH_B: // Check if C is touching yet. - if (touchC.get()){ + if (touchC.get()) { this.setClimbingState(ClimbingStates.TRANS_BC); } case TRANS_BC: From 5b9cd232b30248d209382b931fa0687063b98d85 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 18 Feb 2022 20:42:39 -0500 Subject: [PATCH 15/66] Move to passing the Solenoids into the climber --- src/main/java/frc/robot/Climber.java | 38 +++++++++++++-------- src/main/java/frc/robot/ClimberSensors.java | 4 ++- src/main/java/frc/robot/Drivetrain.java | 6 ++-- src/main/java/frc/robot/Robot.java | 23 ++++++++----- 4 files changed, 42 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 3f66f8a..67bf8a9 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -3,34 +3,42 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.can.TalonFX; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; -enum ClimbingStates { - RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; -} +public class Climber implements Loggable { + enum ClimbingStates { + RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; + } + TalonFX climbingMotor; + TalonFX secondaryClimbingMotor; + + Solenoid climberSolenoidA; + Solenoid climberSolenoidB1; + Solenoid climberSolenoidB2; + Solenoid climberSolenoidC; + + ClimberSensors touchA; + ClimberSensors touchB; + ClimberSensors touchC; -public class Climber implements Loggable { - TalonFX climbingMotor, secondaryClimbingMotor; - Solenoid climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC; - ClimberSensors touchA, touchB, touchC; ClimbingStates currentStage = ClimbingStates.RESTING; LoggableTimer timer; - public Climber(int climbingMotorID, int secondaryClimbingMotorID, int climberSolenoidAID, - int climberSolenoidB1ID, int climberSolenoidB2ID, int climberSolenoidCID, + public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { + this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); - this.climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidAID); - this.climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB1ID); - this.climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidB2ID); - this.climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, climberSolenoidCID); + this.climberSolenoidA = climberSolenoidA; + this.climberSolenoidB1 = climberSolenoidB1; + this.climberSolenoidB2 = climberSolenoidB2; + this.climberSolenoidC = climberSolenoidC; this.touchA = touchA; this.touchB = touchB; @@ -93,7 +101,7 @@ public void setClimbingState(ClimbingStates climbingState) { /** * Updates the current state of the climber. - * + * * @return true when current state is RESTING */ public void checkClimbingState() { diff --git a/src/main/java/frc/robot/ClimberSensors.java b/src/main/java/frc/robot/ClimberSensors.java index a580bad..7a0c921 100644 --- a/src/main/java/frc/robot/ClimberSensors.java +++ b/src/main/java/frc/robot/ClimberSensors.java @@ -6,7 +6,9 @@ public class ClimberSensors implements Loggable { String name; - DigitalInput a, b; + + DigitalInput a; + DigitalInput b; public ClimberSensors(String name, int aID, int bID) { this.name = name; diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index 3430837..ccaab96 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -118,14 +118,12 @@ public void checkGears() { @Override public void setupLogging(Logger logger) { - // TODO Auto-generated method stub - + logger.addLoggable(left); + logger.addLoggable(right); } @Override public void log(Logger logger) { // TODO Auto-generated method stub - } - } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2d25ba3..a81974d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.TimedRobot; import frc.robot.logging.LoggableCompressor; import frc.robot.logging.LoggableController; @@ -57,6 +58,10 @@ public double deadband(double in) { */ @Override public void robotInit() { + logger = new Logger(); + timer = new LoggableTimer(); + logger.addLoggable(timer); + pdp = new LoggablePowerDistribution(1, ModuleType.kRev); driver = new LoggableController("Driver", 0); @@ -64,10 +69,17 @@ public void robotInit() { if (this.climberEnabled) { System.out.println("Initializing climber..."); + + Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); + Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); + Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); + Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); + ClimberSensors touchA = new ClimberSensors("TouchA", 0, 1); ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); - climb = new Climber(9, 10, 2, 3, 4, 5, touchA, touchB, touchC); + climb = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, + climberSolenoidC, touchA, touchB, touchC); logger.addLoggable(touchA); logger.addLoggable(touchB); @@ -77,11 +89,6 @@ public void robotInit() { System.out.println("Climber initialization disabled."); } - logger = new Logger(); - - timer = new LoggableTimer(); - logger.addLoggable(timer); - if (this.drivetrainEnabled) { System.out.println("Initializing drivetrain..."); leftModule = new DriveModule("LeftDriveModule", 2, 3); // 2, 3 @@ -92,8 +99,6 @@ public void robotInit() { drive = new Drivetrain(leftModule, rightModule, 6); - logger.addLoggable(leftModule); - logger.addLoggable(rightModule); logger.addLoggable(drive); } else { System.out.println("Drivetrain initialization disabled."); @@ -181,7 +186,7 @@ public void testPeriodic() { // Robot code goes here if (climberEnabled) { climb.setPrestage(operator.getXButtonPressed()); - climb.setPower(operator.getRightY()); //Deadband + climb.setPower(operator.getRightY()); // Deadband climb.checkClimbingState(); } From bf2571cedecb100bf6943b032ea05c6946974a81 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 18 Feb 2022 22:10:14 -0500 Subject: [PATCH 16/66] Add temporary manual climber driver controls --- src/main/java/frc/robot/Climber.java | 34 ++++++++++++++++++++-- src/main/java/frc/robot/Robot.java | 43 ++++++++++++++++++++-------- 2 files changed, 63 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 67bf8a9..7eb5513 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -32,6 +32,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { + // TODO: figure out if the motors are inverted correctly this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -47,7 +48,6 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climbing/Time"); - } public void setClimbingState(ClimbingStates climbingState) { @@ -154,12 +154,42 @@ public void setPower(double power) { @Override public void setupLogging(Logger logger) { this.timer.setupLogging(logger); - } @Override public void log(Logger logger) { this.timer.log(logger); + } + + public boolean getClimberSolenoidAState() { + return climberSolenoidA.get(); + } + + public void setClimberSolenoidAState(Boolean state) { + this.climberSolenoidA.set(state); + } + + public boolean getClimberSolenoidB1State() { + return climberSolenoidB1.get(); + } + + public void setClimberSolenoidB1State(Boolean state) { + this.climberSolenoidB1.set(state); + } + + public boolean getClimberSolenoidB2State() { + return climberSolenoidB2.get(); + } + + public void setClimberSolenoidB2State(Boolean state) { + this.climberSolenoidB2.set(state); + } + + public boolean getClimberSolenoidCState() { + return climberSolenoidC.get(); + } + public void setClimberSolenoidCState(Boolean state) { + this.climberSolenoidC.set(state); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a81974d..98e366b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,9 @@ public class Robot extends TimedRobot { Drivetrain drive; DriveModule leftModule; DriveModule rightModule; + + Climber climber; + LoggableController driver; LoggableController operator; @@ -36,6 +39,7 @@ public class Robot extends TimedRobot { boolean drivetrainEnabled = true; boolean tankDriveEnabled = true; + boolean climberEnabled = true; private static final double DEADBAND_LIMIT = 0.01; private static final double SPEED_CAP = 0.6; @@ -48,10 +52,6 @@ public double deadband(double in) { return joystickDeadband.scale(out); } - Climber climb; - - boolean climberEnabled = true; - /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -78,13 +78,13 @@ public void robotInit() { ClimberSensors touchA = new ClimberSensors("TouchA", 0, 1); ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); - climb = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, + climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC, touchA, touchB, touchC); logger.addLoggable(touchA); logger.addLoggable(touchB); logger.addLoggable(touchC); - logger.addLoggable(climb); + logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); } @@ -109,7 +109,7 @@ public void robotInit() { System.out.println("done"); logger.addLoggable(driver); - // logger.addLoggable(operator); + logger.addLoggable(operator); logger.addLoggable(compressor); } @@ -160,6 +160,25 @@ public void teleopPeriodic() { rightModule.updateCurrent(); } + if (this.climberEnabled) { + if (operator.getYButtonPressed()) { + climber.setClimberSolenoidAState(!climber.getClimberSolenoidAState()); + } + if (operator.getBButtonPressed()) { + climber.setClimberSolenoidB1State(!climber.getClimberSolenoidB1State()); + } + if (operator.getAButtonPressed()) { + climber.setClimberSolenoidB2State(!climber.getClimberSolenoidB2State()); + } + if (operator.getXButtonPressed()) { + climber.setClimberSolenoidCState(!climber.getClimberSolenoidCState()); + } + + // TODO: Enable this when we're ready to test the climber + // double climberInput = deadband(operator.getLeftY()); + // climber.setPower(climberInput); + } + logger.log(); logger.writeLine(); } @@ -184,11 +203,11 @@ public void testInit() { @Override public void testPeriodic() { // Robot code goes here - if (climberEnabled) { - climb.setPrestage(operator.getXButtonPressed()); - climb.setPower(operator.getRightY()); // Deadband - climb.checkClimbingState(); - } + // if (climberEnabled) { + // climber.setPrestage(operator.getXButtonPressed()); + // climber.setPower(operator.getRightY()); // Deadband + // climber.checkClimbingState(); + // } logger.log(); logger.writeLine(); From 40ea3cc132faf7d8aa3bc02617931fcfc3847ec6 Mon Sep 17 00:00:00 2001 From: Programmer Date: Sat, 19 Feb 2022 15:40:16 -0500 Subject: [PATCH 17/66] Test Climber Controls --- .wpilib/wpilib_preferences.json | 4 +-- src/main/java/frc/robot/Climber.java | 41 +++++++++++++++++++++---- src/main/java/frc/robot/Drivetrain.java | 4 +-- src/main/java/frc/robot/Robot.java | 18 +++++------ 4 files changed, 48 insertions(+), 19 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 04a1fc9..388ae55 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2022", - "teamNumber": 1741 -} + "teamNumber": 1742 +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 7eb5513..5d227e8 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -2,6 +2,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; @@ -29,8 +30,8 @@ enum ClimbingStates { LoggableTimer timer; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, - ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC) { + // ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { // TODO: figure out if the motors are inverted correctly this.climbingMotor = new TalonFX(climbingMotorID); @@ -41,9 +42,15 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climberSolenoidB2 = climberSolenoidB2; this.climberSolenoidC = climberSolenoidC; - this.touchA = touchA; - this.touchB = touchB; - this.touchC = touchC; + // this.touchA = touchA; + // this.touchB = touchB; + // this.touchC = touchC; + + this.climbingMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, + 30); + this.climbingMotor.config_kP(0, 0.1); + this.climbingMotor.config_kI(0, 0.001); + this.climbingMotor.config_kD(0, 5); secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); @@ -148,17 +155,31 @@ public void setPrestage(boolean stage) { } public void setPower(double power) { - climbingMotor.set(ControlMode.PercentOutput, power); + climbingMotor.set(ControlMode.PercentOutput, power * 0.3); + } + + public void setSpeed(double speed) { + climbingMotor.set(ControlMode.Velocity, speed * 6000); + } + + public double getSpeed() { + return climbingMotor.getSelectedSensorVelocity(); } @Override public void setupLogging(Logger logger) { this.timer.setupLogging(logger); + logger.addAttribute("LeftClimberCurrent"); + logger.addAttribute("RightClimberCurrent"); + logger.addAttribute("ClimberSpeed"); } @Override public void log(Logger logger) { this.timer.log(logger); + logger.log("LeftClimberCurrent", getLeftCurrent()); + logger.log("RightClimberMotorCurrent", getRightCurrent()); + logger.log("ClimberSpeed", getSpeed()); } public boolean getClimberSolenoidAState() { @@ -192,4 +213,12 @@ public boolean getClimberSolenoidCState() { public void setClimberSolenoidCState(Boolean state) { this.climberSolenoidC.set(state); } + + public double getLeftCurrent() { + return climbingMotor.getStatorCurrent(); + } + + public double getRightCurrent() { + return secondaryClimbingMotor.getStatorCurrent(); + } } diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index ccaab96..cf260d5 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -118,8 +118,8 @@ public void checkGears() { @Override public void setupLogging(Logger logger) { - logger.addLoggable(left); - logger.addLoggable(right); + // logger.addLoggable(left); + // logger.addLoggable(right); } @Override diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 98e366b..0ba7a03 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -75,15 +75,15 @@ public void robotInit() { Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); - ClimberSensors touchA = new ClimberSensors("TouchA", 0, 1); - ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); - ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); + // ClimberSensors touchA = new ClimberSensors("TouchA", 0, 1); + // ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); + // ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC, touchA, touchB, touchC); + climberSolenoidC);// , touchA, touchB, touchC); - logger.addLoggable(touchA); - logger.addLoggable(touchB); - logger.addLoggable(touchC); + // logger.addLoggable(touchA); + // logger.addLoggable(touchB); + // logger.addLoggable(touchC); logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); @@ -175,8 +175,8 @@ public void teleopPeriodic() { } // TODO: Enable this when we're ready to test the climber - // double climberInput = deadband(operator.getLeftY()); - // climber.setPower(climberInput); + double climberInput = deadband(operator.getLeftY()); + climber.setSpeed(climberInput); } logger.log(); From a56be21cc21e9d7220def5735059548777d88788 Mon Sep 17 00:00:00 2001 From: theblindbandet Date: Sat, 19 Feb 2022 15:47:11 -0500 Subject: [PATCH 18/66] Fix possibly missing break statements in climber --- src/main/java/frc/robot/Climber.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 5d227e8..6cf22bb 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -119,6 +119,7 @@ public void checkClimbingState() { if (touchA.get()) { this.setClimbingState(ClimbingStates.TOUCH_A); } + break; case TOUCH_A: // Check if B is touching yet. if (touchB.get()) { @@ -135,6 +136,7 @@ public void checkClimbingState() { if (touchC.get()) { this.setClimbingState(ClimbingStates.TRANS_BC); } + break; case TRANS_BC: if (timer.hasElapsed(1)) { this.setClimbingState(ClimbingStates.TOUCH_C); From 42b8a1d7df9efee240a1a61b50e75c1cbd779ad2 Mon Sep 17 00:00:00 2001 From: theblindbandet Date: Sat, 19 Feb 2022 15:51:01 -0500 Subject: [PATCH 19/66] TEMP: Advance climber state manually --- src/main/java/frc/robot/Climber.java | 12 ++++++------ src/main/java/frc/robot/Robot.java | 13 +------------ 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 6cf22bb..c0c71fa 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -111,34 +111,34 @@ public void setClimbingState(ClimbingStates climbingState) { * * @return true when current state is RESTING */ - public void checkClimbingState() { + public void checkClimbingState(boolean advanceStage) { // Check whether or not the climber is done climbing during the current stage. switch (currentStage) { case PRE_STAGE: // Check if A is touching yet. - if (touchA.get()) { + if (advanceStage) { this.setClimbingState(ClimbingStates.TOUCH_A); } break; case TOUCH_A: // Check if B is touching yet. - if (touchB.get()) { + if (advanceStage) { this.setClimbingState(ClimbingStates.TRANS_AB); } break; case TRANS_AB: - if (timer.hasElapsed(1)) { + if (advanceStage) { this.setClimbingState(ClimbingStates.TOUCH_B); } break; case TOUCH_B: // Check if C is touching yet. - if (touchC.get()) { + if (advanceStage) { this.setClimbingState(ClimbingStates.TRANS_BC); } break; case TRANS_BC: - if (timer.hasElapsed(1)) { + if (advanceStage) { this.setClimbingState(ClimbingStates.TOUCH_C); } break; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0ba7a03..aac8e2a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -161,18 +161,7 @@ public void teleopPeriodic() { } if (this.climberEnabled) { - if (operator.getYButtonPressed()) { - climber.setClimberSolenoidAState(!climber.getClimberSolenoidAState()); - } - if (operator.getBButtonPressed()) { - climber.setClimberSolenoidB1State(!climber.getClimberSolenoidB1State()); - } - if (operator.getAButtonPressed()) { - climber.setClimberSolenoidB2State(!climber.getClimberSolenoidB2State()); - } - if (operator.getXButtonPressed()) { - climber.setClimberSolenoidCState(!climber.getClimberSolenoidCState()); - } + climber.checkClimbingState(operator.getAButtonPressed()); // TODO: Enable this when we're ready to test the climber double climberInput = deadband(operator.getLeftY()); From c3810c5d01d3f6d5347ee03455ded5419625e764 Mon Sep 17 00:00:00 2001 From: Programmer Date: Sat, 19 Feb 2022 15:54:06 -0500 Subject: [PATCH 20/66] Changed Idle Mode --- src/main/java/frc/robot/Climber.java | 4 ++++ src/main/java/frc/robot/Drivetrain.java | 4 ++-- src/main/java/frc/robot/Robot.java | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 5d227e8..4732757 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -2,6 +2,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.wpilibj.Solenoid; @@ -46,6 +47,9 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb // this.touchB = touchB; // this.touchC = touchC; + this.climbingMotor.setNeutralMode(NeutralMode.Coast); + this.secondaryClimbingMotor.setNeutralMode(NeutralMode.Coast); + this.climbingMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); this.climbingMotor.config_kP(0, 0.1); diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index cf260d5..00f2d8d 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -38,8 +38,8 @@ public class Drivetrain implements Loggable { * @param rightSpeed The speed of the right motors */ public void drive(double leftSpeed, double rightSpeed) { // Probably implement deadbands later - left.setSpeed(leftSpeed); - right.setSpeed(rightSpeed); + left.set(leftSpeed); + right.set(rightSpeed); } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0ba7a03..eb5ba92 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -142,7 +142,7 @@ public void teleopPeriodic() { if (tankDriveEnabled) { double leftInput = deadband(-driver.getLeftY()); double rightInput = deadband(-driver.getRightY()); - drive.tankDrive(leftInput, rightInput); + drive.tankDrive(leftInput * 0.4, rightInput * 0.4); } else { double turnInput = deadband(driver.getRightX()); double speedInput = deadband(-driver.getLeftY()); From dd21d55b6119ed41b0b576c34c8854aad6965cb0 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Mon, 21 Feb 2022 20:29:33 -0500 Subject: [PATCH 21/66] add loggablegyro --- .../java/frc/robot/logging/LoggableGyro.java | 24 +++++++++++++ vendordeps/navx_frc.json | 35 +++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 src/main/java/frc/robot/logging/LoggableGyro.java create mode 100644 vendordeps/navx_frc.json diff --git a/src/main/java/frc/robot/logging/LoggableGyro.java b/src/main/java/frc/robot/logging/LoggableGyro.java new file mode 100644 index 0000000..a728a1a --- /dev/null +++ b/src/main/java/frc/robot/logging/LoggableGyro.java @@ -0,0 +1,24 @@ +package frc.robot.logging; + +import com.kauailabs.navx.frc.AHRS; + +public class LoggableGyro extends AHRS implements Loggable { + + public LoggableGyro() { + super(); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("AHRS/velocityY"); + logger.addAttribute("AHRS/pitch"); + logger.addAttribute("AHRS/accelerationY"); + } + + @Override + public void log(Logger logger) { + logger.log("AHRS/velocityY", this.getVelocityY()); + logger.log("AHRS/pitch", this.getPitch()); + logger.log("AHRS/accelerationY", this.getWorldLinearAccelY()); + } +} diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json new file mode 100644 index 0000000..0fb49a7 --- /dev/null +++ b/vendordeps/navx_frc.json @@ -0,0 +1,35 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "4.0.442", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "4.0.442" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "4.0.442", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file From d3da78b4ae2234b025ed1cfec53b2d6b3bf12d9d Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Tue, 22 Feb 2022 18:50:41 -0500 Subject: [PATCH 22/66] whitespace --- src/main/java/frc/robot/logging/LoggableGyro.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/logging/LoggableGyro.java b/src/main/java/frc/robot/logging/LoggableGyro.java index a728a1a..cb67840 100644 --- a/src/main/java/frc/robot/logging/LoggableGyro.java +++ b/src/main/java/frc/robot/logging/LoggableGyro.java @@ -3,7 +3,6 @@ import com.kauailabs.navx.frc.AHRS; public class LoggableGyro extends AHRS implements Loggable { - public LoggableGyro() { super(); } From 4a0bcafb6de7f6d54411afad6216eb554dce5e76 Mon Sep 17 00:00:00 2001 From: selisine Date: Tue, 22 Feb 2022 18:51:39 -0500 Subject: [PATCH 23/66] Add To-Do Reminder Comment --- src/main/java/frc/robot/Robot.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a5db039..876910a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -162,6 +162,7 @@ public void teleopPeriodic() { if (this.climberEnabled) { climber.checkClimbingState(operator.getAButtonPressed()); + // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro // TODO: Enable this when we're ready to test the climber double climberInput = deadband(operator.getLeftY()); From af09047001a52e1c2eb60c93a825c80110398522 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Tue, 22 Feb 2022 19:36:28 -0500 Subject: [PATCH 24/66] add motorstates --- src/main/java/frc/robot/Climber.java | 24 +++++++++++++++++++++++- src/main/java/frc/robot/Robot.java | 5 ++++- 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 03d66e5..04891a3 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -9,11 +9,15 @@ import frc.robot.logging.Loggable; import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; +import frc.robot.logging.LoggableGyro; public class Climber implements Loggable { enum ClimbingStates { RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; } + enum MotorStates { + STATIC, ACTIVE; + } TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -27,11 +31,13 @@ enum ClimbingStates { ClimberSensors touchB; ClimberSensors touchC; + MotorStates currentState = MotorStates.STATIC; ClimbingStates currentStage = ClimbingStates.RESTING; LoggableTimer timer; + LoggableGyro gyro; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, LoggableGyro gyro) { // ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { // TODO: figure out if the motors are inverted correctly @@ -59,6 +65,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climbing/Time"); + this.gyro = gyro; } public void setClimbingState(ClimbingStates climbingState) { @@ -110,6 +117,21 @@ public void setClimbingState(ClimbingStates climbingState) { } } + public void setMotorState(MotorStates state){ + this.currentState = state; + + switch (state) { + case STATIC: + break; + + case ACTIVE: + break; + + default: + break; + } + } + /** * Updates the current state of the climber. * diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a5db039..2b4ea19 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import frc.robot.logging.LoggableCompressor; import frc.robot.logging.LoggableController; +import frc.robot.logging.LoggableGyro; import frc.robot.logging.LoggablePowerDistribution; import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; @@ -30,6 +31,7 @@ public class Robot extends TimedRobot { DriveModule rightModule; Climber climber; + LoggableGyro gyro; LoggableController driver; LoggableController operator; @@ -61,6 +63,7 @@ public void robotInit() { logger = new Logger(); timer = new LoggableTimer(); logger.addLoggable(timer); + gyro = new LoggableGyro(); pdp = new LoggablePowerDistribution(1, ModuleType.kRev); @@ -79,7 +82,7 @@ public void robotInit() { // ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); // ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC);// , touchA, touchB, touchC); + climberSolenoidC, gyro);// , touchA, touchB, touchC); // logger.addLoggable(touchA); // logger.addLoggable(touchB); From b84cf4d0bac528246ad6dae614d3769de2ccc884 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Tue, 22 Feb 2022 20:03:27 -0500 Subject: [PATCH 25/66] add instances for motor states --- src/main/java/frc/robot/Climber.java | 56 ++++++++++++++++++++-------- src/main/java/frc/robot/Robot.java | 2 +- 2 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 04891a3..ec3dfe2 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -117,21 +117,6 @@ public void setClimbingState(ClimbingStates climbingState) { } } - public void setMotorState(MotorStates state){ - this.currentState = state; - - switch (state) { - case STATIC: - break; - - case ACTIVE: - break; - - default: - break; - } - } - /** * Updates the current state of the climber. * @@ -154,6 +139,7 @@ public void checkClimbingState(boolean advanceStage) { break; case TRANS_AB: if (advanceStage) { + this.setMotorState(MotorStates.STATIC); this.setClimbingState(ClimbingStates.TOUCH_B); } break; @@ -165,6 +151,7 @@ public void checkClimbingState(boolean advanceStage) { break; case TRANS_BC: if (advanceStage) { + this.setMotorState(MotorStates.STATIC); this.setClimbingState(ClimbingStates.TOUCH_C); } break; @@ -194,6 +181,45 @@ public double getSpeed() { return climbingMotor.getSelectedSensorVelocity(); } + public void setMotorState(MotorStates currentState) { + this.currentState = currentState; + } + + public void setMotors(double value) { + if (value != 0) { + setMotorState(MotorStates.ACTIVE); + } + switch (currentState) { + case STATIC: + setPower(0); + break; + + case ACTIVE: + setSpeed(value); + break; + + default: + setPower(0); + break; + } + } + + public void checkMotorState() { + switch (currentState) { + case STATIC: + if (Math.abs(gyro.getVelocityY()) < 2 && Math.abs(gyro.getWorldLinearAccelY()) < 0.1) { + setMotorState(MotorStates.ACTIVE); + } + break; + + case ACTIVE: + break; + + default: + break; + } + } + @Override public void setupLogging(Logger logger) { this.timer.setupLogging(logger); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ae85ca3..bb9f82e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -169,7 +169,7 @@ public void teleopPeriodic() { // TODO: Enable this when we're ready to test the climber double climberInput = deadband(operator.getLeftY()); - climber.setSpeed(climberInput); + climber.setMotors(climberInput); } logger.log(); From 21ce2b6b19895b108ac196d42b3fd2f33fd72013 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 22 Feb 2022 20:10:23 -0500 Subject: [PATCH 26/66] Formatting --- src/main/java/frc/robot/Climber.java | 17 +++++++----- src/main/java/frc/robot/FirstOrderFilter.java | 26 +++++++++++++++++++ 2 files changed, 36 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/FirstOrderFilter.java diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index ec3dfe2..a6e4033 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -7,14 +7,15 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; +import frc.robot.logging.LoggableGyro; import frc.robot.logging.LoggableTimer; import frc.robot.logging.Logger; -import frc.robot.logging.LoggableGyro; public class Climber implements Loggable { enum ClimbingStates { RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; } + enum MotorStates { STATIC, ACTIVE; } @@ -37,7 +38,8 @@ enum MotorStates { LoggableGyro gyro; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, LoggableGyro gyro) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, + LoggableGyro gyro) { // ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { // TODO: figure out if the motors are inverted correctly @@ -193,11 +195,11 @@ public void setMotors(double value) { case STATIC: setPower(0); break; - + case ACTIVE: setSpeed(value); break; - + default: setPower(0); break; @@ -207,14 +209,15 @@ public void setMotors(double value) { public void checkMotorState() { switch (currentState) { case STATIC: - if (Math.abs(gyro.getVelocityY()) < 2 && Math.abs(gyro.getWorldLinearAccelY()) < 0.1) { + if (Math.abs(gyro.getVelocityY()) < 2 + && Math.abs(gyro.getWorldLinearAccelY()) < 0.1) { setMotorState(MotorStates.ACTIVE); } break; - + case ACTIVE: break; - + default: break; } diff --git a/src/main/java/frc/robot/FirstOrderFilter.java b/src/main/java/frc/robot/FirstOrderFilter.java new file mode 100644 index 0000000..09d6698 --- /dev/null +++ b/src/main/java/frc/robot/FirstOrderFilter.java @@ -0,0 +1,26 @@ +package frc.robot; + +public class FirstOrderFilter { + private double[] previousValues; + private int index = 0; + private int range; + + public FirstOrderFilter(int range) { + this.range = range; + this.previousValues = new double[range]; + } + + public double update(double value) { + this.previousValues[this.index] = this.value; + this.index = (this.index + 1) % this.range; + return value; + } + + public double get() { + double total = 0.0; + for (int i = 0; i < range; i++) { + total += this.previousValues[i]; + } + return total / this.range; + } +} From 2b32468a15e442be3fdcd4ed3f4af4750ec08466 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 22 Feb 2022 20:15:02 -0500 Subject: [PATCH 27/66] Add FirstOrderFilter --- src/main/java/frc/robot/Climber.java | 15 +++++++++++++++ src/main/java/frc/robot/DriveModule.java | 2 +- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index a6e4033..8aad877 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -119,6 +119,21 @@ public void setClimbingState(ClimbingStates climbingState) { } } + public void setMotorState(MotorStates state) { + this.currentState = state; + + switch (state) { + case STATIC: + break; + + case ACTIVE: + break; + + default: + break; + } + } + /** * Updates the current state of the climber. * diff --git a/src/main/java/frc/robot/DriveModule.java b/src/main/java/frc/robot/DriveModule.java index 805418e..91c54f4 100644 --- a/src/main/java/frc/robot/DriveModule.java +++ b/src/main/java/frc/robot/DriveModule.java @@ -18,7 +18,7 @@ public class DriveModule implements Loggable { private Encoder encoder; private double power; - private double current[] = new double[30]; + private double[] current = new double[30]; private int indexCurrent; /** From 8b59184f718fb8b1aac92fe32d123c89678b58b4 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Tue, 22 Feb 2022 20:22:56 -0500 Subject: [PATCH 28/66] remove extra motor state method --- src/main/java/frc/robot/Climber.java | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 8aad877..a6e4033 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -119,21 +119,6 @@ public void setClimbingState(ClimbingStates climbingState) { } } - public void setMotorState(MotorStates state) { - this.currentState = state; - - switch (state) { - case STATIC: - break; - - case ACTIVE: - break; - - default: - break; - } - } - /** * Updates the current state of the climber. * From 9699d1cee7e8db56df0100a3575e71b63e84fcce Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 22 Feb 2022 20:39:00 -0500 Subject: [PATCH 29/66] Fix LoggableFirstOrderFilter --- src/main/java/frc/robot/Climber.java | 11 +++++++++ src/main/java/frc/robot/DriveModule.java | 4 ++++ src/main/java/frc/robot/Drivetrain.java | 8 ++++--- ...ter.java => LoggableFirstOrderFilter.java} | 23 +++++++++++++++---- src/main/java/frc/robot/Robot.java | 2 ++ 5 files changed, 41 insertions(+), 7 deletions(-) rename src/main/java/frc/robot/{FirstOrderFilter.java => LoggableFirstOrderFilter.java} (51%) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 8aad877..f884f78 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -37,6 +37,9 @@ enum MotorStates { LoggableTimer timer; LoggableGyro gyro; + LoggableFirstOrderFilter leftFilter; + LoggableFirstOrderFilter rightFilter; + public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, LoggableGyro gyro) { @@ -68,6 +71,14 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climbing/Time"); this.gyro = gyro; + + this.leftFilter = new LoggableFirstOrderFilter(10, "Climbing/LeftCurrent"); + this.rightFilter = new LoggableFirstOrderFilter(10, "Climbing/RightCurrent"); + } + + public void update() { + this.leftFilter.update(climbingMotor.getStatorCurrent()); + this.rightFilter.update(secondaryClimbingMotor.getStatorCurrent()); } public void setClimbingState(ClimbingStates climbingState) { diff --git a/src/main/java/frc/robot/DriveModule.java b/src/main/java/frc/robot/DriveModule.java index 91c54f4..a422020 100644 --- a/src/main/java/frc/robot/DriveModule.java +++ b/src/main/java/frc/robot/DriveModule.java @@ -46,6 +46,10 @@ public class DriveModule implements Loggable { indexCurrent = 0; } + public void update() { + updateCurrent(); + } + public void setEncoder(int encoderPortA, int encoderPortB, boolean reverseDirection) { this.encoder = new Encoder(encoderPortA, encoderPortB); diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index 00f2d8d..e284eb3 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -31,6 +31,11 @@ public class Drivetrain implements Loggable { right.setInverted(true); } + public void update() { + this.left.update(); + this.right.update(); + } + /** * Controls the power of the motors in the drivetrain. * @@ -84,9 +89,6 @@ public boolean getShifter() { * Shifts gears based on current. */ public void checkGears() { - left.updateCurrent(); - right.updateCurrent(); - if (getShifter()) { if (left.getAverageCurrent() > SHIFT_CURRENT_LOW || right.getAverageCurrent() > SHIFT_CURRENT_LOW) { diff --git a/src/main/java/frc/robot/FirstOrderFilter.java b/src/main/java/frc/robot/LoggableFirstOrderFilter.java similarity index 51% rename from src/main/java/frc/robot/FirstOrderFilter.java rename to src/main/java/frc/robot/LoggableFirstOrderFilter.java index 09d6698..751a8e0 100644 --- a/src/main/java/frc/robot/FirstOrderFilter.java +++ b/src/main/java/frc/robot/LoggableFirstOrderFilter.java @@ -1,26 +1,41 @@ package frc.robot; -public class FirstOrderFilter { +import frc.robot.logging.Loggable; + +public class LoggableFirstOrderFilter implements Loggable { private double[] previousValues; private int index = 0; private int range; + private String name; - public FirstOrderFilter(int range) { + public LoggableFirstOrderFilter(int range, String name) { this.range = range; + this.name = name; this.previousValues = new double[range]; } - public double update(double value) { + public void update(double value) { this.previousValues[this.index] = this.value; this.index = (this.index + 1) % this.range; - return value; } public double get() { double total = 0.0; + for (int i = 0; i < range; i++) { total += this.previousValues[i]; } + return total / this.range; } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute(this.name + "/Average"); + } + + @Override + public void log(Logger logger) { + logger.log(this.name + "/Average", get()); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bb9f82e..b4dec3c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -119,6 +119,8 @@ public void robotInit() { @Override public void robotPeriodic() { // Robot code goes here + drive.update(); + climber.update(); } @Override From a1d1bb7298126d41f76496ed62e4272364075d23 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 22 Feb 2022 20:40:04 -0500 Subject: [PATCH 30/66] Fix conflict in setMotorState --- src/main/java/frc/robot/Climber.java | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index f884f78..94afa09 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -130,21 +130,6 @@ public void setClimbingState(ClimbingStates climbingState) { } } - public void setMotorState(MotorStates state) { - this.currentState = state; - - switch (state) { - case STATIC: - break; - - case ACTIVE: - break; - - default: - break; - } - } - /** * Updates the current state of the climber. * From a53e235d8da1e8b14e3a2d1ae8e3eac960474121 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Tue, 22 Feb 2022 20:45:07 -0500 Subject: [PATCH 31/66] Ops... --- src/main/java/frc/robot/LoggableFirstOrderFilter.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/LoggableFirstOrderFilter.java b/src/main/java/frc/robot/LoggableFirstOrderFilter.java index 751a8e0..6dcbf26 100644 --- a/src/main/java/frc/robot/LoggableFirstOrderFilter.java +++ b/src/main/java/frc/robot/LoggableFirstOrderFilter.java @@ -1,6 +1,7 @@ package frc.robot; import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; public class LoggableFirstOrderFilter implements Loggable { private double[] previousValues; @@ -15,7 +16,7 @@ public LoggableFirstOrderFilter(int range, String name) { } public void update(double value) { - this.previousValues[this.index] = this.value; + this.previousValues[this.index] = value; this.index = (this.index + 1) % this.range; } From 2495eaddded4344072abf2604e654a116ff96d36 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 13:35:32 -0500 Subject: [PATCH 32/66] Log both climber filters --- src/main/java/frc/robot/Climber.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 94afa09..6a7bbb9 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -239,6 +239,10 @@ public void setupLogging(Logger logger) { this.timer.setupLogging(logger); logger.addAttribute("LeftClimberCurrent"); logger.addAttribute("RightClimberCurrent"); + + logger.addAttribute("LeftClimberFilter"); + logger.addAttribute("RightClimberFilter"); + logger.addAttribute("ClimberSpeed"); } @@ -247,6 +251,10 @@ public void log(Logger logger) { this.timer.log(logger); logger.log("LeftClimberCurrent", getLeftCurrent()); logger.log("RightClimberMotorCurrent", getRightCurrent()); + + logger.log("LeftClimberFilter", leftFilter.get()); + logger.log("RightClimberFilter", rightFilter.get()); + logger.log("ClimberSpeed", getSpeed()); } From cca5439bf7a4943e4a7e8a102e4f0b3faa115160 Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Sat, 26 Feb 2022 13:41:43 -0500 Subject: [PATCH 33/66] Add more manual controls for testing --- src/main/java/frc/robot/Climber.java | 7 ++++++- src/main/java/frc/robot/Robot.java | 5 ++++- src/main/java/frc/robot/logging/LoggableGyro.java | 12 ++++++++++++ 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 94afa09..b73ddf4 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -198,7 +198,12 @@ public void setMotorState(MotorStates currentState) { this.currentState = currentState; } + public MotorStates getMotorState() { + return this.currentState; + } + public void setMotors(double value) { + checkMotorState(); if (value != 0) { setMotorState(MotorStates.ACTIVE); } @@ -246,7 +251,7 @@ public void setupLogging(Logger logger) { public void log(Logger logger) { this.timer.log(logger); logger.log("LeftClimberCurrent", getLeftCurrent()); - logger.log("RightClimberMotorCurrent", getRightCurrent()); + logger.log("RightClimberCurrent", getRightCurrent()); logger.log("ClimberSpeed", getSpeed()); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b4dec3c..e386d3f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.TimedRobot; +import frc.robot.Climber.MotorStates; import frc.robot.logging.LoggableCompressor; import frc.robot.logging.LoggableController; import frc.robot.logging.LoggableGyro; @@ -168,7 +169,9 @@ public void teleopPeriodic() { if (this.climberEnabled) { climber.checkClimbingState(operator.getAButtonPressed()); // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro - + if(operator.getRightBumperPressed()){ + climber.setMotorState(climber.getMotorState()==MotorStates.ACTIVE?MotorStates.STATIC:MotorStates.ACTIVE); + } // TODO: Enable this when we're ready to test the climber double climberInput = deadband(operator.getLeftY()); climber.setMotors(climberInput); diff --git a/src/main/java/frc/robot/logging/LoggableGyro.java b/src/main/java/frc/robot/logging/LoggableGyro.java index cb67840..bb7083d 100644 --- a/src/main/java/frc/robot/logging/LoggableGyro.java +++ b/src/main/java/frc/robot/logging/LoggableGyro.java @@ -9,15 +9,27 @@ public LoggableGyro() { @Override public void setupLogging(Logger logger) { + logger.addAttribute("AHRS/velocityX"); + logger.addAttribute("AHRS/yaw"); + logger.addAttribute("AHRS/accelerationX"); logger.addAttribute("AHRS/velocityY"); logger.addAttribute("AHRS/pitch"); logger.addAttribute("AHRS/accelerationY"); + logger.addAttribute("AHRS/velocityZ"); + logger.addAttribute("AHRS/roll"); + logger.addAttribute("AHRS/accelerationZ"); } @Override public void log(Logger logger) { + logger.log("AHRS/velocityX", this.getVelocityX()); + logger.log("AHRS/yaw", this.getYaw()); + logger.log("AHRS/accelerationX", this.getWorldLinearAccelX()); logger.log("AHRS/velocityY", this.getVelocityY()); logger.log("AHRS/pitch", this.getPitch()); logger.log("AHRS/accelerationY", this.getWorldLinearAccelY()); + logger.log("AHRS/velocityZ", this.getVelocityZ()); + logger.log("AHRS/roll", this.getRoll()); + logger.log("AHRS/accelerationZ", this.getWorldLinearAccelZ()); } } From e259f8aba71944f05165b385261dd61dc14b8ab6 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 14:15:06 -0500 Subject: [PATCH 34/66] Update climber logging --- src/main/java/frc/robot/Climber.java | 29 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index d389bfe..cae67a5 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -70,11 +70,11 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); - this.timer = new LoggableTimer("Climbing/Time"); + this.timer = new LoggableTimer("Climber/Time"); this.gyro = gyro; - this.leftFilter = new LoggableFirstOrderFilter(10, "Climbing/LeftCurrent"); - this.rightFilter = new LoggableFirstOrderFilter(10, "Climbing/RightCurrent"); + this.leftFilter = new LoggableFirstOrderFilter(10, "Climber/Left/Current"); + this.rightFilter = new LoggableFirstOrderFilter(10, "Climber/Right/Current"); } public void update() { @@ -243,26 +243,25 @@ public void checkMotorState() { @Override public void setupLogging(Logger logger) { this.timer.setupLogging(logger); - logger.addAttribute("LeftClimberCurrent"); - logger.addAttribute("RightClimberCurrent"); - logger.addAttribute("LeftClimberFilter"); - logger.addAttribute("RightClimberFilter"); + logger.addLoggable(this.leftFilter); + logger.addLoggable(this.rightFilter); + this.leftFilter.setupLogging(logger); + this.rightFilter.setupLogging(logger); - logger.addAttribute("ClimberSpeed"); + logger.addAttribute("Climber/Left/Current"); + logger.addAttribute("Climber/Right/Current"); + + logger.addAttribute("Climber/Speed"); } @Override public void log(Logger logger) { this.timer.log(logger); - logger.log("LeftClimberCurrent", getLeftCurrent()); - logger.log("RightClimberCurrent", getRightCurrent()); - logger.log("RightClimberMotorCurrent", getRightCurrent()); - - logger.log("LeftClimberFilter", leftFilter.get()); - logger.log("RightClimberFilter", rightFilter.get()); + logger.log("Climber/Left/Current", getLeftCurrent()); + logger.log("Climber/Right/Current", getRightCurrent()); - logger.log("ClimberSpeed", getSpeed()); + logger.log("Climber/Speed", getSpeed()); } public boolean getClimberSolenoidAState() { From 443f1d75047005237d749a8d8fccffae6ba8d69d Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 15:05:12 -0500 Subject: [PATCH 35/66] Redfine the ClimbingStates enum --- src/main/java/frc/robot/Climber.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index cae67a5..332e6e8 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -14,7 +14,17 @@ public class Climber implements Loggable { enum ClimbingStates { - RESTING, PRE_STAGE, TOUCH_A, TRANS_AB, TOUCH_B, TRANS_BC, TOUCH_C; + RESTING(0, "Resting"), PRE_STAGE(10, "Pre-stage"), TOUCH_A(20, "Latched A"), TRANS_AB(25, + "Latched A and B"), TOUCH_B(30, + "Latched B"), TRANS_BC(35, "Latched B and C"), TOUCH_C(40, "Latched C"); + + public int id; + public String name; + + ClimbingStates(int id, String name) { + this.id = id; + this.name = name; + } } enum MotorStates { From 904435011b64cd374178c0e3dc432844fdacd9e4 Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Sat, 26 Feb 2022 16:05:18 -0500 Subject: [PATCH 36/66] Put touch sensors into the same class --- src/main/java/frc/robot/Climber.java | 16 ++++------- src/main/java/frc/robot/ClimberSensors.java | 31 +++++++++++++-------- src/main/java/frc/robot/Robot.java | 10 ++----- 3 files changed, 29 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 332e6e8..86c3ef6 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -39,9 +39,7 @@ enum MotorStates { Solenoid climberSolenoidB2; Solenoid climberSolenoidC; - ClimberSensors touchA; - ClimberSensors touchB; - ClimberSensors touchC; + ClimberSensors touch; MotorStates currentState = MotorStates.STATIC; ClimbingStates currentStage = ClimbingStates.RESTING; @@ -54,7 +52,7 @@ enum MotorStates { public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, LoggableGyro gyro) { - // ClimberSensors touchA, ClimberSensors touchB, ClimberSensors touchC) { + // ClimberSensors touch) { // TODO: figure out if the motors are inverted correctly this.climbingMotor = new TalonFX(climbingMotorID); @@ -65,9 +63,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climberSolenoidB2 = climberSolenoidB2; this.climberSolenoidC = climberSolenoidC; - // this.touchA = touchA; - // this.touchB = touchB; - // this.touchC = touchC; + // this.touch = touch; this.climbingMotor.setNeutralMode(NeutralMode.Coast); this.secondaryClimbingMotor.setNeutralMode(NeutralMode.Coast); @@ -151,13 +147,13 @@ public void checkClimbingState(boolean advanceStage) { switch (currentStage) { case PRE_STAGE: // Check if A is touching yet. - if (advanceStage) { + if (advanceStage) { //touch.getA() this.setClimbingState(ClimbingStates.TOUCH_A); } break; case TOUCH_A: // Check if B is touching yet. - if (advanceStage) { + if (advanceStage) { //touch.getB() this.setClimbingState(ClimbingStates.TRANS_AB); } break; @@ -169,7 +165,7 @@ public void checkClimbingState(boolean advanceStage) { break; case TOUCH_B: // Check if C is touching yet. - if (advanceStage) { + if (advanceStage) { //touch.getC() this.setClimbingState(ClimbingStates.TRANS_BC); } break; diff --git a/src/main/java/frc/robot/ClimberSensors.java b/src/main/java/frc/robot/ClimberSensors.java index 7a0c921..00f085d 100644 --- a/src/main/java/frc/robot/ClimberSensors.java +++ b/src/main/java/frc/robot/ClimberSensors.java @@ -5,25 +5,34 @@ import frc.robot.logging.Logger; public class ClimberSensors implements Loggable { - String name; + DigitalInput a1, a2, b1, b2, c1, c2; + + public ClimberSensors(int a1ID, int a2ID, int b1ID, int b2ID, int c1ID, int c2ID) { + this.a1 = new DigitalInput(a1ID); + this.a2 = new DigitalInput(a2ID); + this.b1 = new DigitalInput(b1ID); + this.b2 = new DigitalInput(b2ID); + this.c1 = new DigitalInput(c1ID); + this.c2 = new DigitalInput(c2ID); + } - DigitalInput a; - DigitalInput b; + public boolean getA() { + return this.a1.get() && this.a2.get(); + } - public ClimberSensors(String name, int aID, int bID) { - this.name = name; - this.a = new DigitalInput(aID); - this.b = new DigitalInput(bID); + public boolean getB() { + return this.b1.get() && this.b2.get(); } - public boolean get() { - return this.a.get() && this.b.get(); + public boolean getC() { + return this.c1.get() && this.c2.get(); } @Override public void setupLogging(Logger logger) { - logger.log(this.name, this.get()); - + logger.log("TouchA", this.getA()); + logger.log("TouchB", this.getB()); + logger.log("TouchC", this.getC()); } @Override diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e386d3f..9a52ace 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -79,15 +79,11 @@ public void robotInit() { Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); - // ClimberSensors touchA = new ClimberSensors("TouchA", 0, 1); - // ClimberSensors touchB = new ClimberSensors("TouchB", 2, 3); - // ClimberSensors touchC = new ClimberSensors("TouchC", 4, 5); + // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC, gyro);// , touchA, touchB, touchC); + climberSolenoidC, gyro);// , climberSensors); - // logger.addLoggable(touchA); - // logger.addLoggable(touchB); - // logger.addLoggable(touchC); + // logger.addLoggable(climberSensors); logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); From d9c9e3c9f2db4ec7f28e13855dd8de781ae753b8 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 17:13:45 -0500 Subject: [PATCH 37/66] Add more climbing states --- src/main/java/frc/robot/Climber.java | 45 ++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 332e6e8..5520006 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -14,9 +14,26 @@ public class Climber implements Loggable { enum ClimbingStates { - RESTING(0, "Resting"), PRE_STAGE(10, "Pre-stage"), TOUCH_A(20, "Latched A"), TRANS_AB(25, - "Latched A and B"), TOUCH_B(30, - "Latched B"), TRANS_BC(35, "Latched B and C"), TOUCH_C(40, "Latched C"); + RESTING(0, "Default resting"), PRE_STAGE(5, + "Rotate climber and set pre-stage pin position (button)"), TOUCH_A(10, + "Pin A (button/sensor)"), ROTATE_B(15, "Rotate to B bar (photo)"), TOUCH_AB( + 20, "Pin B (high current/sensor)"), ROTATE_AB_DOWN(25, + "Rotate down to plumb (photo)"), RELEASE_A(30, + "Unpin A (gyro/accel)"), ROTATE_B_DOWN(35, + "Wait for swinging (photo)"), ROTATE_C(40, + "Rotate to C bar (gyro/accel)"), TOUCH_BC( + 50, + "Pin C (high current/sensor)"), ROTATE_BC_DOWN( + 55, + "Rotate down to plumb (photo)"), RELEASE_B( + 60, + "Unpin B (gyro/accel)"), ROTATE_C_DOWN( + 65, + "Wait for swinging ()"), DONE( + 70, + "Climbing is done"), ERROR( + 100, + "Error"); public int id; public String name; @@ -27,6 +44,22 @@ enum ClimbingStates { } } + // 00 RESTING: Default resting + // 05 PRE_STAGE: Rotate climber and set pre-stage pin position (button) + // 10 TOUCH_A: Pin A (button/sensor) + // 15 ROTATE_B: Rotate to B bar (photo) + // 20 TOUCH_AB: Pin B (high current/sensor) + // 25 ROTATE_AB_DOWN: Rotate down to plumb (photo) + // 30 RELEASE_A: Unpin A (gyro/accel) + // 35 ROTATE_B_DOWN: Wait for swinging (photo) + // 40 ROTATE_C: Rotate to C bar (gyro/accel) + // 50 TOUCH_BC: Pin C (high current/sensor) + // 55 ROTATE_BC_DOWN: Rotate down to plumb (photo) + // 60 RELEASE_B: Unpin B (gyro/accel) + // 65 ROTATE_C_DOWN: Wait for swinging () + // 70 DONE: Climbing is done + // 100 ERROR: Error + enum MotorStates { STATIC, ACTIVE; } @@ -105,7 +138,7 @@ public void setClimbingState(ClimbingStates climbingState) { climberSolenoidB2.set(false); climberSolenoidC.set(true); break; - case TRANS_AB: + case TOUCH_AB: this.timer.reset(); climberSolenoidA.set(false); climberSolenoidB1.set(false); @@ -158,10 +191,10 @@ public void checkClimbingState(boolean advanceStage) { case TOUCH_A: // Check if B is touching yet. if (advanceStage) { - this.setClimbingState(ClimbingStates.TRANS_AB); + this.setClimbingState(ClimbingStates.TOUCH_AB); } break; - case TRANS_AB: + case TOUCH_AB: if (advanceStage) { this.setMotorState(MotorStates.STATIC); this.setClimbingState(ClimbingStates.TOUCH_B); From 56a384f18a4aa4eb47d895a5ed2548971cd3832e Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 17:54:28 -0500 Subject: [PATCH 38/66] WIP on climbing states --- src/main/java/frc/robot/Climber.java | 165 ++++++++++++++++++--------- 1 file changed, 114 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index e32d21c..f0fa9f1 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -16,24 +16,26 @@ public class Climber implements Loggable { enum ClimbingStates { RESTING(0, "Default resting"), PRE_STAGE(5, "Rotate climber and set pre-stage pin position (button)"), TOUCH_A(10, - "Pin A (button/sensor)"), ROTATE_B(15, "Rotate to B bar (photo)"), TOUCH_AB( - 20, "Pin B (high current/sensor)"), ROTATE_AB_DOWN(25, - "Rotate down to plumb (photo)"), RELEASE_A(30, - "Unpin A (gyro/accel)"), ROTATE_B_DOWN(35, - "Wait for swinging (photo)"), ROTATE_C(40, - "Rotate to C bar (gyro/accel)"), TOUCH_BC( - 50, - "Pin C (high current/sensor)"), ROTATE_BC_DOWN( - 55, - "Rotate down to plumb (photo)"), RELEASE_B( - 60, - "Unpin B (gyro/accel)"), ROTATE_C_DOWN( - 65, - "Wait for swinging ()"), DONE( - 70, - "Climbing is done"), ERROR( - 100, - "Error"); + "Pin A (button/sensor)"), ROTATE_B(15, + "Rotate to B bar (photogate)"), TOUCH_AB(20, + "Pin B (high current/sensor)"), ROTATE_AB_DOWN(25, + "Rotate down to plumb (photogate)"), RELEASE_A(30, + "Unpin A (gyro/accel)"), ROTATE_B_DOWN(35, + "Wait for swinging (photogate)"), ROTATE_C( + 40, + "Rotate to C bar (gyro/accel)"), TOUCH_BC( + 50, + "Pin C (high current/sensor)"), ROTATE_BC_DOWN( + 55, + "Rotate down to plumb (photogate)"), RELEASE_B( + 60, + "Unpin B (gyro/accel)"), ROTATE_C_DOWN( + 65, + "Wait for swinging ()"), DONE( + 70, + "Climbing is done"), ERROR( + 100, + "Error"); public int id; public String name; @@ -47,14 +49,14 @@ enum ClimbingStates { // 00 RESTING: Default resting // 05 PRE_STAGE: Rotate climber and set pre-stage pin position (button) // 10 TOUCH_A: Pin A (button/sensor) - // 15 ROTATE_B: Rotate to B bar (photo) + // 15 ROTATE_B: Rotate to B bar (photogate) // 20 TOUCH_AB: Pin B (high current/sensor) - // 25 ROTATE_AB_DOWN: Rotate down to plumb (photo) + // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) // 30 RELEASE_A: Unpin A (gyro/accel) - // 35 ROTATE_B_DOWN: Wait for swinging (photo) + // 35 ROTATE_B_DOWN: Wait for swinging (photogate) // 40 ROTATE_C: Rotate to C bar (gyro/accel) // 50 TOUCH_BC: Pin C (high current/sensor) - // 55 ROTATE_BC_DOWN: Rotate down to plumb (photo) + // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) // 60 RELEASE_B: Unpin B (gyro/accel) // 65 ROTATE_C_DOWN: Wait for swinging () // 70 DONE: Climbing is done @@ -64,6 +66,10 @@ enum MotorStates { STATIC, ACTIVE; } + public static double MAX_INSTANT_CURRENT = 70.0; + public static double MAX_AVERAGE_CURRENT = 50.0; + public static double NEXT_STATE_CURRENT = 50.0; + TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -119,74 +125,131 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb public void update() { this.leftFilter.update(climbingMotor.getStatorCurrent()); this.rightFilter.update(secondaryClimbingMotor.getStatorCurrent()); - } - public void setClimbingState(ClimbingStates climbingState) { - this.currentStage = climbingState; + switch (this.currentStage) { + // 00 RESTING: Default resting + case RESTING: + break; - // 00 RESTING: Default resting - - // 10 TOUCH_A: Pin A (button/sensor) - // 15 ROTATE_B: Rotate to B bar (photo) - // 20 TOUCH_AB: Pin B (high current/sensor) - // 25 ROTATE_AB_DOWN: Rotate down to plumb (photo) - // 30 RELEASE_A: Unpin A (gyro/accel) - // 35 ROTATE_B_DOWN: Wait for swinging (photo) - // 40 ROTATE_C: Rotate to C bar (gyro/accel) - // 50 TOUCH_BC: Pin C (high current/sensor) - // 55 ROTATE_BC_DOWN: Rotate down to plumb (photo) - // 60 RELEASE_B: Unpin B (gyro/accel) - // 65 ROTATE_C_DOWN: Wait for swinging () - // 70 DONE: Climbing is done - // 100 ERROR: Error - - switch (climbingState) { - + // 05 PRE_STAGE: Rotate climber and set pre-stage pin position (button) case PRE_STAGE: this.timer.start(); + + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(false); + // TODO: set motor target here break; + + // 10 TOUCH_A: Pin A (button/sensor) case TOUCH_A: climberSolenoidA.set(false); climberSolenoidB1.set(true); climberSolenoidB2.set(false); climberSolenoidC.set(true); break; + + // 15 ROTATE_B: Rotate to B bar (photogate) + case ROTATE_B: + // TODO: set motor power here + break; + + // 20 TOUCH_AB: Pin B (high current/sensor) case TOUCH_AB: - this.timer.reset(); climberSolenoidA.set(false); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); + // TODO: set motor target here break; - case TOUCH_B: + + // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) + case ROTATE_AB_DOWN: + // TODO: set motor target here + break; + + // 30 RELEASE_A: Unpin A (gyro/accel) + case RELEASE_A: climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); + // TODO: set motor target here + break; + + // 35 ROTATE_B_DOWN: Wait for swinging (photogate) + case ROTATE_B_DOWN: + // TODO: set motor target here break; + + // 40 ROTATE_C: Rotate to C bar (gyro/accel) + case ROTATE_B_DOWN: + // TODO: set motor target here + break; + + // 50 TOUCH_BC: Pin C (high current/sensor) case TOUCH_BC: climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(false); + // TODO: set motor target here break; - case TOUCH_C: + + // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) + case ROTATE_BC_DOWN: + // TODO: set motor target here + break; + + // 60 RELEASE_B: Unpin B (gyro/accel) + case RELEASE_B: climberSolenoidA.set(true); climberSolenoidB1.set(true); climberSolenoidB2.set(true); climberSolenoidC.set(false); break; - case RESTING: - climberSolenoidA.set(false); - climberSolenoidB1.set(false); - climberSolenoidB2.set(false); - climberSolenoidC.set(false); + + // 65 ROTATE_C_DOWN: Wait for swinging () + case ROTATE_C_DOWN: + // TODO: set motor target here + break; + + // 70 DONE: Climbing is done + case DONE: + // Success! \o/ + break; + + // 100 ERROR: Error + case ERROR: + System.out.println("Climber ERROR: something has gone wrong"); break; + default: + System.out.println("Climber: Invalid state"); break; } } + public void setClimbingState(ClimbingStates climbingState) { + this.currentStage = climbingState; + } + + public void disableClimber() { + // Stop the motors + this.climbingMotor.set(ControlMode.PercentOutput, 0); + this.secondaryClimbingMotor.set(ControlMode.PercentOutput, 0); + + // Set the solenoids to their default extended positions + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(false); + + // Set the current stage to the default + setClimbingState(ClimbingStates.ERROR); + } + /** * Updates the current state of the climber. * From 1533de43fd029c52ac3a82ae5fe29eb1bb32fa8f Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 17:55:13 -0500 Subject: [PATCH 39/66] Add automatic current shutoff --- src/main/java/frc/robot/Climber.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index f0fa9f1..bb5808c 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -126,6 +126,17 @@ public void update() { this.leftFilter.update(climbingMotor.getStatorCurrent()); this.rightFilter.update(secondaryClimbingMotor.getStatorCurrent()); + // Make sure we're not pulling too much current instantly + if (climbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT + || secondaryClimbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT) { + setClimbingState(ClimbingStates.ERROR); + } + + // Make sure we're not pulling too much current over time + if (leftFilter.get() > MAX_AVERAGE_CURRENT || rightFilter.get() > MAX_AVERAGE_CURRENT) { + setClimbingState(ClimbingStates.ERROR); + } + switch (this.currentStage) { // 00 RESTING: Default resting case RESTING: From 811be5131f45bfbf5cf7d02b0d587249135b71f8 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 18:02:59 -0500 Subject: [PATCH 40/66] Ad getNextClimbingState --- src/main/java/frc/robot/Climber.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index bb5808c..f5576c5 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -246,6 +246,10 @@ public void setClimbingState(ClimbingStates climbingState) { this.currentStage = climbingState; } + public ClimbingStates getNextClimbingState() { + return ClimbingStates.values()[this.currentStage.ordinal() + 1]; + } + public void disableClimber() { // Stop the motors this.climbingMotor.set(ControlMode.PercentOutput, 0); From 67741c162dc8811c869b00a3d0b3281cb95000fa Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 18:11:36 -0500 Subject: [PATCH 41/66] Update next step setup --- src/main/java/frc/robot/Climber.java | 6 ++---- src/main/java/frc/robot/Robot.java | 12 +++++++++--- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index f5576c5..065268a 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -172,7 +172,6 @@ public void update() { climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); - // TODO: set motor target here break; // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) @@ -234,10 +233,12 @@ public void update() { // 100 ERROR: Error case ERROR: System.out.println("Climber ERROR: something has gone wrong"); + disableClimber(); break; default: System.out.println("Climber: Invalid state"); + disableClimber(); break; } } @@ -260,9 +261,6 @@ public void disableClimber() { climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(false); - - // Set the current stage to the default - setClimbingState(ClimbingStates.ERROR); } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9a52ace..00a8ea1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -163,10 +163,16 @@ public void teleopPeriodic() { } if (this.climberEnabled) { - climber.checkClimbingState(operator.getAButtonPressed()); + if (operator.getAButtonPressed()) { + climber.setClimbingState(climber.getNextClimbingState()); + } + // climber.checkClimbingState(operator.getAButtonPressed()); + // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro - if(operator.getRightBumperPressed()){ - climber.setMotorState(climber.getMotorState()==MotorStates.ACTIVE?MotorStates.STATIC:MotorStates.ACTIVE); + if (operator.getRightBumperPressed()) { + climber.setMotorState( + climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC + : MotorStates.ACTIVE); } // TODO: Enable this when we're ready to test the climber double climberInput = deadband(operator.getLeftY()); From c2b3fe62d834ba0d38eb3aa3275e949d71a75e29 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 18:13:10 -0500 Subject: [PATCH 42/66] Comment out old code --- src/main/java/frc/robot/Climber.java | 82 ++++++++++++++-------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 065268a..4e4bae8 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -194,7 +194,7 @@ public void update() { break; // 40 ROTATE_C: Rotate to C bar (gyro/accel) - case ROTATE_B_DOWN: + case ROTATE_C: // TODO: set motor target here break; @@ -268,46 +268,46 @@ public void disableClimber() { * * @return true when current state is RESTING */ - public void checkClimbingState(boolean advanceStage) { - // Check whether or not the climber is done climbing during the current stage. - switch (currentStage) { - case PRE_STAGE: - // Check if A is touching yet. - if (advanceStage) { // touch.getA() - this.setClimbingState(ClimbingStates.TOUCH_A); - } - break; - case TOUCH_A: - // Check if B is touching yet. - if (advanceStage) { // touch.getB() - this.setClimbingState(ClimbingStates.TOUCH_AB); - } - break; - case TOUCH_AB: - if (advanceStage) { - this.setMotorState(MotorStates.STATIC); - this.setClimbingState(ClimbingStates.TOUCH_B); - } - break; - case TOUCH_B: - // Check if C is touching yet. - if (advanceStage) { // touch.getC() - this.setClimbingState(ClimbingStates.TOUCH_BC); - } - break; - case TOUCH_BC: - if (advanceStage) { - this.setMotorState(MotorStates.STATIC); - this.setClimbingState(ClimbingStates.TOUCH_C); - } - break; - case TOUCH_C: - // Success! \o/ - break; - default: - break; - } - } + // public void checkClimbingState(boolean advanceStage) { + // // Check whether or not the climber is done climbing during the current stage. + // switch (currentStage) { + // case PRE_STAGE: + // // Check if A is touching yet. + // if (advanceStage) { // touch.getA() + // this.setClimbingState(ClimbingStates.TOUCH_A); + // } + // break; + // case TOUCH_A: + // // Check if B is touching yet. + // if (advanceStage) { // touch.getB() + // this.setClimbingState(ClimbingStates.TOUCH_AB); + // } + // break; + // case TOUCH_AB: + // if (advanceStage) { + // this.setMotorState(MotorStates.STATIC); + // this.setClimbingState(ClimbingStates.TOUCH_B); + // } + // break; + // case TOUCH_B: + // // Check if C is touching yet. + // if (advanceStage) { // touch.getC() + // this.setClimbingState(ClimbingStates.TOUCH_BC); + // } + // break; + // case TOUCH_BC: + // if (advanceStage) { + // this.setMotorState(MotorStates.STATIC); + // this.setClimbingState(ClimbingStates.TOUCH_C); + // } + // break; + // case TOUCH_C: + // // Success! \o/ + // break; + // default: + // break; + // } + // } public void setPrestage(boolean stage) { if (stage) { From efaadf883e2b59070f3d9750fa58ba9673cc361b Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Sat, 26 Feb 2022 18:18:27 -0500 Subject: [PATCH 43/66] Add next state current detection --- src/main/java/frc/robot/Climber.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 4e4bae8..6211d2a 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -164,6 +164,10 @@ public void update() { // 15 ROTATE_B: Rotate to B bar (photogate) case ROTATE_B: // TODO: set motor power here + if (climbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT + || secondaryClimbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT) { + setClimbingState(ClimbingStates.TOUCH_AB); + } break; // 20 TOUCH_AB: Pin B (high current/sensor) @@ -196,6 +200,11 @@ public void update() { // 40 ROTATE_C: Rotate to C bar (gyro/accel) case ROTATE_C: // TODO: set motor target here + + if (climbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT + || secondaryClimbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT) { + setClimbingState(ClimbingStates.TOUCH_BC); + } break; // 50 TOUCH_BC: Pin C (high current/sensor) From 1ec70321e02b42b63c5588584a8958a97ae53ed7 Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Sat, 26 Feb 2022 18:18:52 -0500 Subject: [PATCH 44/66] Add climbing photogates for pistons --- src/main/java/frc/robot/ClimberGates.java | 50 +++++++++++++++++++++++ src/main/java/frc/robot/Robot.java | 1 + 2 files changed, 51 insertions(+) create mode 100644 src/main/java/frc/robot/ClimberGates.java diff --git a/src/main/java/frc/robot/ClimberGates.java b/src/main/java/frc/robot/ClimberGates.java new file mode 100644 index 0000000..ec244a8 --- /dev/null +++ b/src/main/java/frc/robot/ClimberGates.java @@ -0,0 +1,50 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class ClimberGates implements Loggable{ + DigitalInput aL, aR, b1L, b1R, b2L, b2R, cL, cR; + + public ClimberGates(int aLID, int aRID, int b1LID, int b1RID, int b2LID, int b2RID, int cLID, int cRID) { + this.aL = new DigitalInput(aLID); + this.aR = new DigitalInput(aRID); + this.b1L = new DigitalInput(b1LID); + this.b1R = new DigitalInput(b1RID); + this.b2L = new DigitalInput(b2LID); + this.b2R = new DigitalInput(b2RID); + this.cL = new DigitalInput(cLID); + this.cR = new DigitalInput(cRID); + } + + public boolean getA() { + return this.aL.get() && this.aR.get(); + } + + public boolean getB1() { + return this.b1L.get() && this.b1R.get(); + } + + public boolean getB2() { + return this.b2L.get() && this.b2R.get(); + } + + public boolean getC() { + return this.cL.get() && this.cR.get(); + } + + @Override + public void setupLogging(Logger logger) { + logger.log("GateA", this.getA()); + logger.log("GateB1", this.getB1()); + logger.log("GateB2", this.getB2()); + logger.log("GateC", this.getC()); + } + + @Override + public void log(Logger logger) { + // TODO Auto-generated method stub + + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9a52ace..77a4d0f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -80,6 +80,7 @@ public void robotInit() { Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); + // ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, climberSolenoidC, gyro);// , climberSensors); From 8f6b8aabf8210e036dca88356ae5435562872dd6 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Mon, 28 Feb 2022 18:50:19 -0500 Subject: [PATCH 45/66] Fix climber variable naming --- src/main/java/frc/robot/Climber.java | 34 ++++++++++++++++++---------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 6211d2a..c03fdf2 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -69,6 +69,7 @@ enum MotorStates { public static double MAX_INSTANT_CURRENT = 70.0; public static double MAX_AVERAGE_CURRENT = 50.0; public static double NEXT_STATE_CURRENT = 50.0; + public static int FILTER_FRAME_RANGE = 10; TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -80,8 +81,8 @@ enum MotorStates { ClimberSensors touch; - MotorStates currentState = MotorStates.STATIC; - ClimbingStates currentStage = ClimbingStates.RESTING; + MotorStates currentMotorState = MotorStates.STATIC; + ClimbingStates currentClimberState = ClimbingStates.RESTING; LoggableTimer timer; LoggableGyro gyro; @@ -93,7 +94,6 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb LoggableGyro gyro) { // ClimberSensors touch) { - // TODO: figure out if the motors are inverted correctly this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -115,11 +115,13 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); + this.timer = new LoggableTimer("Climber/Time"); this.gyro = gyro; - this.leftFilter = new LoggableFirstOrderFilter(10, "Climber/Left/Current"); - this.rightFilter = new LoggableFirstOrderFilter(10, "Climber/Right/Current"); + this.leftFilter = new LoggableFirstOrderFilter(FILTER_FRAME_RANGE, "Climber/Left/Current"); + this.rightFilter = + new LoggableFirstOrderFilter(FILTER_FRAME_RANGE, "Climber/Right/Current"); } public void update() { @@ -137,7 +139,7 @@ public void update() { setClimbingState(ClimbingStates.ERROR); } - switch (this.currentStage) { + switch (this.currentClimberState) { // 00 RESTING: Default resting case RESTING: break; @@ -253,11 +255,11 @@ public void update() { } public void setClimbingState(ClimbingStates climbingState) { - this.currentStage = climbingState; + this.currentClimberState = climbingState; } public ClimbingStates getNextClimbingState() { - return ClimbingStates.values()[this.currentStage.ordinal() + 1]; + return ClimbingStates.values()[this.currentClimberState.ordinal() + 1]; } public void disableClimber() { @@ -337,11 +339,11 @@ public double getSpeed() { } public void setMotorState(MotorStates currentState) { - this.currentState = currentState; + this.currentMotorState = currentState; } public MotorStates getMotorState() { - return this.currentState; + return this.currentMotorState; } public void setMotors(double value) { @@ -349,7 +351,7 @@ public void setMotors(double value) { if (value != 0) { setMotorState(MotorStates.ACTIVE); } - switch (currentState) { + switch (currentMotorState) { case STATIC: setPower(0); break; @@ -365,7 +367,7 @@ public void setMotors(double value) { } public void checkMotorState() { - switch (currentState) { + switch (currentMotorState) { case STATIC: if (Math.abs(gyro.getVelocityY()) < 2 && Math.abs(gyro.getWorldLinearAccelY()) < 0.1) { @@ -394,6 +396,10 @@ public void setupLogging(Logger logger) { logger.addAttribute("Climber/Right/Current"); logger.addAttribute("Climber/Speed"); + + logger.addAttribute("Climber/State/Name"); + logger.addAttribute("Climber/State/Id"); + logger.addAttribute("Climber/State/Ordinal"); } @Override @@ -403,6 +409,10 @@ public void log(Logger logger) { logger.log("Climber/Right/Current", getRightCurrent()); logger.log("Climber/Speed", getSpeed()); + + logger.log("Climber/State/Name", this.currentClimberState.name); + logger.log("Climber/State/Id", this.currentClimberState.id); + logger.log("Climber/State/Ordinal", this.currentClimberState.ordinal()); } public boolean getClimberSolenoidAState() { From b0a398a6f16fd10e93afdc6cb1bd9cafa75d380e Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Mon, 28 Feb 2022 19:38:44 -0500 Subject: [PATCH 46/66] change wpilib version --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index b3e805a..104b058 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.3.1" + id "edu.wpi.first.GradleRIO" version "2022.4.1" id "jacoco" } From c0958e87366406948f2a7012e43c2d6f47a6a3be Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Mon, 28 Feb 2022 20:24:50 -0500 Subject: [PATCH 47/66] add simulation configs --- simgui-ds.json | 103 +++++++++++++++++++++++++++++++++++++++++++++++++ simgui.json | 57 +++++++++++++++++++++++++++ 2 files changed, 160 insertions(+) create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..1781fd7 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,103 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" + }, + { + "guid": "Keyboard2" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..486afab --- /dev/null +++ b/simgui.json @@ -0,0 +1,57 @@ +{ + "HALProvider": { + "Encoders": { + "0": { + "header": { + "open": true + } + }, + "1": { + "header": { + "open": true + } + } + }, + "Other Devices": { + "navX-Sensor[0]": { + "header": { + "open": true + } + }, + "window": { + "visible": false + } + }, + "Timing": { + "window": { + "visible": false + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro" + }, + "windows": { + "/FMSInfo": { + "window": { + "visible": true + } + }, + "/LiveWindow/Ungrouped/navX-Sensor[4]": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "FMSInfo": { + "open": true + }, + "LiveWindow": { + "open": true + } + } +} From efac58a07ee880c11ef179cadcad256e2b3bcdf7 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Mon, 28 Feb 2022 20:27:49 -0500 Subject: [PATCH 48/66] modify debug configs --- .vscode/launch.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..893bd6c 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -15,7 +15,7 @@ "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": true, } ] } From 6a13a8adedd6cf166641e04731978172721d9929 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Tue, 1 Mar 2022 19:03:19 -0500 Subject: [PATCH 49/66] fix indenting --- src/main/java/frc/robot/Climber.java | 33 +++++++++++----------------- 1 file changed, 13 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index c03fdf2..5ba9be1 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -16,26 +16,19 @@ public class Climber implements Loggable { enum ClimbingStates { RESTING(0, "Default resting"), PRE_STAGE(5, "Rotate climber and set pre-stage pin position (button)"), TOUCH_A(10, - "Pin A (button/sensor)"), ROTATE_B(15, - "Rotate to B bar (photogate)"), TOUCH_AB(20, - "Pin B (high current/sensor)"), ROTATE_AB_DOWN(25, - "Rotate down to plumb (photogate)"), RELEASE_A(30, - "Unpin A (gyro/accel)"), ROTATE_B_DOWN(35, - "Wait for swinging (photogate)"), ROTATE_C( - 40, - "Rotate to C bar (gyro/accel)"), TOUCH_BC( - 50, - "Pin C (high current/sensor)"), ROTATE_BC_DOWN( - 55, - "Rotate down to plumb (photogate)"), RELEASE_B( - 60, - "Unpin B (gyro/accel)"), ROTATE_C_DOWN( - 65, - "Wait for swinging ()"), DONE( - 70, - "Climbing is done"), ERROR( - 100, - "Error"); + "Pin A (button/sensor)"), ROTATE_B(15, + "Rotate to B bar (photogate)"), TOUCH_AB(20, + "Pin B (high current/sensor)"), ROTATE_AB_DOWN(25, + "Rotate down to plumb (photogate)"), RELEASE_A(30, + "Unpin A (gyro/accel)"), ROTATE_B_DOWN(35, + "Wait for swinging (photogate)"), ROTATE_C(40, + "Rotate to C bar (gyro/accel)"), TOUCH_BC(50, + "Pin C (high current/sensor)"), ROTATE_BC_DOWN(55, + "Rotate down to plumb (photogate)"), RELEASE_B(60, + "Unpin B (gyro/accel)"), ROTATE_C_DOWN(65, + "Wait for swinging ()"), DONE(70, + "Climbing is done"), ERROR(100, + "Error"); public int id; public String name; From d02495c72ff145a88501032bd9563a94624df804 Mon Sep 17 00:00:00 2001 From: kryllyxofficial01 Date: Tue, 1 Mar 2022 19:06:52 -0500 Subject: [PATCH 50/66] change climbingstates formatting --- src/main/java/frc/robot/Climber.java | 30 ++++++++++++++-------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 5ba9be1..839edd6 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -14,21 +14,21 @@ public class Climber implements Loggable { enum ClimbingStates { - RESTING(0, "Default resting"), PRE_STAGE(5, - "Rotate climber and set pre-stage pin position (button)"), TOUCH_A(10, - "Pin A (button/sensor)"), ROTATE_B(15, - "Rotate to B bar (photogate)"), TOUCH_AB(20, - "Pin B (high current/sensor)"), ROTATE_AB_DOWN(25, - "Rotate down to plumb (photogate)"), RELEASE_A(30, - "Unpin A (gyro/accel)"), ROTATE_B_DOWN(35, - "Wait for swinging (photogate)"), ROTATE_C(40, - "Rotate to C bar (gyro/accel)"), TOUCH_BC(50, - "Pin C (high current/sensor)"), ROTATE_BC_DOWN(55, - "Rotate down to plumb (photogate)"), RELEASE_B(60, - "Unpin B (gyro/accel)"), ROTATE_C_DOWN(65, - "Wait for swinging ()"), DONE(70, - "Climbing is done"), ERROR(100, - "Error"); + RESTING(0, "Default resting"), + PRE_STAGE(5,"Rotate climber and set pre-stage pin position (button)"), + TOUCH_A(10,"Pin A (button/sensor)"), + ROTATE_B(15,"Rotate to B bar (photogate)"), + TOUCH_AB(20,"Pin B (high current/sensor)"), + ROTATE_AB_DOWN(25,"Rotate down to plumb (photogate)"), + RELEASE_A(30,"Unpin A (gyro/accel)"), + ROTATE_B_DOWN(35,"Wait for swinging (photogate)"), + ROTATE_C(40,"Rotate to C bar (gyro/accel)"), + TOUCH_BC(50,"Pin C (high current/sensor)"), + ROTATE_BC_DOWN(55,"Rotate down to plumb (photogate)"), + RELEASE_B(60,"Unpin B (gyro/accel)"), + ROTATE_C_DOWN(65,"Wait for swinging ()"), + DONE(70,"Climbing is done"), + ERROR(100,"Error"); public int id; public String name; From 60aa96f6ce355a7a5f55d7472808941387d0736b Mon Sep 17 00:00:00 2001 From: Programmer Date: Thu, 3 Mar 2022 13:51:29 -0500 Subject: [PATCH 51/66] Initial testing for using current to change states --- src/main/java/frc/robot/Climber.java | 51 ++++++++++++++++------------ src/main/java/frc/robot/Robot.java | 8 ++--- 2 files changed, 33 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index c03fdf2..92fc61f 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -66,9 +66,10 @@ enum MotorStates { STATIC, ACTIVE; } - public static double MAX_INSTANT_CURRENT = 70.0; - public static double MAX_AVERAGE_CURRENT = 50.0; - public static double NEXT_STATE_CURRENT = 50.0; + public static double MAX_INSTANT_CURRENT = 200.0; + public static double MAX_AVERAGE_CURRENT = 110.0; + public static double NEXT_AB_STATE_CURRENT = 53.0; + public static double NEXT_BC_STATE_CURRENT = 40.0; public static int FILTER_FRAME_RANGE = 10; TalonFX climbingMotor; @@ -90,8 +91,8 @@ enum MotorStates { LoggableFirstOrderFilter rightFilter; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, - LoggableGyro gyro) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC) { + // LoggableGyro gyro) { // ClimberSensors touch) { this.climbingMotor = new TalonFX(climbingMotorID); @@ -117,7 +118,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climber/Time"); - this.gyro = gyro; + // this.gyro = gyro; this.leftFilter = new LoggableFirstOrderFilter(FILTER_FRAME_RANGE, "Climber/Left/Current"); this.rightFilter = @@ -132,26 +133,32 @@ public void update() { if (climbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT || secondaryClimbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT) { setClimbingState(ClimbingStates.ERROR); + System.out.println("--------HIT MAX CURRENT--------"); } // Make sure we're not pulling too much current over time if (leftFilter.get() > MAX_AVERAGE_CURRENT || rightFilter.get() > MAX_AVERAGE_CURRENT) { + System.out.println("--------HIT MAX AVERAGE CURRENT--------"); setClimbingState(ClimbingStates.ERROR); } switch (this.currentClimberState) { // 00 RESTING: Default resting case RESTING: + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(false); break; // 05 PRE_STAGE: Rotate climber and set pre-stage pin position (button) case PRE_STAGE: this.timer.start(); - climberSolenoidA.set(false); - climberSolenoidB1.set(false); + climberSolenoidA.set(true); + climberSolenoidB1.set(true); climberSolenoidB2.set(false); - climberSolenoidC.set(false); + climberSolenoidC.set(true); // TODO: set motor target here break; @@ -166,8 +173,8 @@ public void update() { // 15 ROTATE_B: Rotate to B bar (photogate) case ROTATE_B: // TODO: set motor power here - if (climbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT) { + if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT + || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_AB); } break; @@ -203,8 +210,8 @@ public void update() { case ROTATE_C: // TODO: set motor target here - if (climbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > NEXT_STATE_CURRENT) { + if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT + || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_BC); } break; @@ -226,7 +233,7 @@ public void update() { // 60 RELEASE_B: Unpin B (gyro/accel) case RELEASE_B: climberSolenoidA.set(true); - climberSolenoidB1.set(true); + climberSolenoidB1.set(false); climberSolenoidB2.set(true); climberSolenoidC.set(false); break; @@ -369,10 +376,10 @@ public void setMotors(double value) { public void checkMotorState() { switch (currentMotorState) { case STATIC: - if (Math.abs(gyro.getVelocityY()) < 2 - && Math.abs(gyro.getWorldLinearAccelY()) < 0.1) { - setMotorState(MotorStates.ACTIVE); - } + // if (Math.abs(gyro.getVelocityY()) < 2 + // && Math.abs(gyro.getWorldLinearAccelY()) < 0.1) { + // setMotorState(MotorStates.ACTIVE); + // } break; case ACTIVE: @@ -387,10 +394,10 @@ public void checkMotorState() { public void setupLogging(Logger logger) { this.timer.setupLogging(logger); - logger.addLoggable(this.leftFilter); - logger.addLoggable(this.rightFilter); - this.leftFilter.setupLogging(logger); - this.rightFilter.setupLogging(logger); + // logger.addLoggable(this.leftFilter); + // logger.addLoggable(this.rightFilter); + // this.leftFilter.setupLogging(logger); + // this.rightFilter.setupLogging(logger); logger.addAttribute("Climber/Left/Current"); logger.addAttribute("Climber/Right/Current"); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b4272cd..b5c69c9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,7 +32,7 @@ public class Robot extends TimedRobot { DriveModule rightModule; Climber climber; - LoggableGyro gyro; + // LoggableGyro gyro; LoggableController driver; LoggableController operator; @@ -64,7 +64,7 @@ public void robotInit() { logger = new Logger(); timer = new LoggableTimer(); logger.addLoggable(timer); - gyro = new LoggableGyro(); + // gyro = new LoggableGyro(); pdp = new LoggablePowerDistribution(1, ModuleType.kRev); @@ -82,7 +82,7 @@ public void robotInit() { // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); // ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC, gyro);// , climberSensors); + climberSolenoidC);// ,gyro, climberSensors); // logger.addLoggable(climberSensors); logger.addLoggable(climber); @@ -164,7 +164,7 @@ public void teleopPeriodic() { } if (this.climberEnabled) { - if (operator.getAButtonPressed()) { + if (operator.getLeftBumperPressed()) { climber.setClimbingState(climber.getNextClimbingState()); } // climber.checkClimbingState(operator.getAButtonPressed()); From 2a7e5cf1cc5e113860d4ab711e529542e25295fe Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Thu, 3 Mar 2022 14:00:08 -0500 Subject: [PATCH 52/66] Set motors to predetermined velocity when climbing --- src/main/java/frc/robot/Climber.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index ee86616..5d0ce2e 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -166,9 +166,11 @@ public void update() { // 15 ROTATE_B: Rotate to B bar (photogate) case ROTATE_B: // TODO: set motor power here + climbingMotor.set(ControlMode.Velocity, 0.15); if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_AB); + climbingMotor.set(ControlMode.Velocity, 0); } break; @@ -182,6 +184,7 @@ public void update() { // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) case ROTATE_AB_DOWN: + climbingMotor.set(ControlMode.PercentOutput, 0); // TODO: set motor target here break; @@ -197,15 +200,17 @@ public void update() { // 35 ROTATE_B_DOWN: Wait for swinging (photogate) case ROTATE_B_DOWN: // TODO: set motor target here + climbingMotor.set(ControlMode.PercentOutput, 0); break; // 40 ROTATE_C: Rotate to C bar (gyro/accel) case ROTATE_C: // TODO: set motor target here - + climbingMotor.set(ControlMode.Velocity, 0.15); if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_BC); + climbingMotor.set(ControlMode.Velocity, 0); } break; @@ -221,6 +226,7 @@ public void update() { // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) case ROTATE_BC_DOWN: // TODO: set motor target here + climbingMotor.set(ControlMode.PercentOutput, 0); break; // 60 RELEASE_B: Unpin B (gyro/accel) @@ -234,6 +240,7 @@ public void update() { // 65 ROTATE_C_DOWN: Wait for swinging () case ROTATE_C_DOWN: // TODO: set motor target here + climbingMotor.set(ControlMode.PercentOutput, 0); break; // 70 DONE: Climbing is done From 14a3aba4ac8969d4ae60b45bf1258c706de7cb69 Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Thu, 3 Mar 2022 14:14:55 -0500 Subject: [PATCH 53/66] Add in automatic progression with climbing gates --- src/main/java/frc/robot/Climber.java | 26 +++++++++++++++++++++++--- src/main/java/frc/robot/Robot.java | 4 ++-- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 5d0ce2e..948526a 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -74,6 +74,7 @@ enum MotorStates { Solenoid climberSolenoidC; ClimberSensors touch; + ClimberGates gates; MotorStates currentMotorState = MotorStates.STATIC; ClimbingStates currentClimberState = ClimbingStates.RESTING; @@ -84,9 +85,10 @@ enum MotorStates { LoggableFirstOrderFilter rightFilter; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC) { - // LoggableGyro gyro) { - // ClimberSensors touch) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC //) { + , ClimberGates gates) { + // , LoggableGyro gyro) { + // , ClimberSensors touch) { this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -97,6 +99,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climberSolenoidC = climberSolenoidC; // this.touch = touch; + this.gates = gates; this.climbingMotor.setNeutralMode(NeutralMode.Coast); this.secondaryClimbingMotor.setNeutralMode(NeutralMode.Coast); @@ -161,6 +164,11 @@ public void update() { climberSolenoidB1.set(true); climberSolenoidB2.set(false); climberSolenoidC.set(true); + + // Uncomment after testing + // if (gates.getA()) { + // setClimbingState(ClimbingStates.ROTATE_B); + // } break; // 15 ROTATE_B: Rotate to B bar (photogate) @@ -180,6 +188,9 @@ public void update() { climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); + if (gates.getB1()) { + setClimbingState(ClimbingStates.ROTATE_AB_DOWN); + } break; // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) @@ -195,6 +206,9 @@ public void update() { climberSolenoidB2.set(false); climberSolenoidC.set(true); // TODO: set motor target here + if (!gates.getA()) { + setClimbingState(ClimbingStates.ROTATE_B_DOWN);; + } break; // 35 ROTATE_B_DOWN: Wait for swinging (photogate) @@ -221,6 +235,9 @@ public void update() { climberSolenoidB2.set(false); climberSolenoidC.set(false); // TODO: set motor target here + if (gates.getC()) { + setClimbingState(ClimbingStates.ROTATE_BC_DOWN); + } break; // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) @@ -235,6 +252,9 @@ public void update() { climberSolenoidB1.set(false); climberSolenoidB2.set(true); climberSolenoidC.set(false); + if (!gates.getB2()) { + setClimbingState(ClimbingStates.ROTATE_C_DOWN); + } break; // 65 ROTATE_C_DOWN: Wait for swinging () diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b5c69c9..baddd61 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -80,9 +80,9 @@ public void robotInit() { Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); - // ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); + ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC);// ,gyro, climberSensors); + climberSolenoidC, climberGates);// ,gyro, climberSensors); // logger.addLoggable(climberSensors); logger.addLoggable(climber); From 461e060f00639396361782a90334a451c07262df Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Thu, 3 Mar 2022 14:52:22 -0500 Subject: [PATCH 54/66] Add automatic progression after swinging --- src/main/java/frc/robot/Climber.java | 41 +++++++++++++++++++++------- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 948526a..0fcdd44 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -61,10 +61,16 @@ enum MotorStates { public static double MAX_INSTANT_CURRENT = 200.0; public static double MAX_AVERAGE_CURRENT = 110.0; - public static double NEXT_AB_STATE_CURRENT = 53.0; - public static double NEXT_BC_STATE_CURRENT = 40.0; + public static double NEXT_AB_STATE_CURRENT = 55.0; + public static double NEXT_BC_STATE_CURRENT = 35.0; public static int FILTER_FRAME_RANGE = 10; + public static double TOUCH_A_POSITION = 0; //TBD + public static double SWING_AB_POSITION = 0; //TBD + public static double SWING_B_POSITION = 0; //TBD + public static double SWING_BC_POSITION = 0; //TBD + public static double SWING_MIN_VELOCITY = 1000; //TBD + TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -106,6 +112,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climbingMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + this.climbingMotor.setSelectedSensorPosition(0); this.climbingMotor.config_kP(0, 0.1); this.climbingMotor.config_kI(0, 0.001); this.climbingMotor.config_kD(0, 5); @@ -156,6 +163,7 @@ public void update() { climberSolenoidB2.set(false); climberSolenoidC.set(true); // TODO: set motor target here + // climbingMotor.set(ControlMode.Position, TOUCH_A_POSITION); break; // 10 TOUCH_A: Pin A (button/sensor) @@ -174,11 +182,11 @@ public void update() { // 15 ROTATE_B: Rotate to B bar (photogate) case ROTATE_B: // TODO: set motor power here - climbingMotor.set(ControlMode.Velocity, 0.15); + this.setSpeed(0.15); if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_AB); - climbingMotor.set(ControlMode.Velocity, 0); + this.setSpeed(0); } break; @@ -195,8 +203,12 @@ public void update() { // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) case ROTATE_AB_DOWN: - climbingMotor.set(ControlMode.PercentOutput, 0); + this.setPower(0); // TODO: set motor target here + if (Math.abs(climbingMotor.getSelectedSensorPosition()-SWING_AB_POSITION) < 500) {//Determine tolerance + System.out.println("DONE SWINGING!"); + // setClimbingState(ClimbingStates.RELEASE_A); + } break; // 30 RELEASE_A: Unpin A (gyro/accel) @@ -214,17 +226,22 @@ public void update() { // 35 ROTATE_B_DOWN: Wait for swinging (photogate) case ROTATE_B_DOWN: // TODO: set motor target here - climbingMotor.set(ControlMode.PercentOutput, 0); + this.setPower(0); + if (Math.abs(climbingMotor.getSelectedSensorPosition()-SWING_B_POSITION) < 500 + && Math.abs(climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {//Determine tolerance + System.out.println("DONE SWINGING!"); + // setClimbingState(ClimbingStates.ROTATE_C); + } break; // 40 ROTATE_C: Rotate to C bar (gyro/accel) case ROTATE_C: // TODO: set motor target here - climbingMotor.set(ControlMode.Velocity, 0.15); + this.setSpeed(0.15); if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_BC); - climbingMotor.set(ControlMode.Velocity, 0); + this.setSpeed(0); } break; @@ -243,7 +260,11 @@ public void update() { // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) case ROTATE_BC_DOWN: // TODO: set motor target here - climbingMotor.set(ControlMode.PercentOutput, 0); + this.setPower(0); + if (Math.abs(climbingMotor.getSelectedSensorPosition()-SWING_BC_POSITION) < 500) {//Determine tolerance + System.out.println("DONE SWINGING!"); + // setClimbingState(ClimbingStates.RELEASE_B); + } break; // 60 RELEASE_B: Unpin B (gyro/accel) @@ -260,7 +281,7 @@ public void update() { // 65 ROTATE_C_DOWN: Wait for swinging () case ROTATE_C_DOWN: // TODO: set motor target here - climbingMotor.set(ControlMode.PercentOutput, 0); + this.setPower(0); break; // 70 DONE: Climbing is done From 3411dbb9b9d4d3ea4d52a00a995aa753b5567e75 Mon Sep 17 00:00:00 2001 From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com> Date: Thu, 3 Mar 2022 15:07:50 -0500 Subject: [PATCH 55/66] Change resting piston positions --- src/main/java/frc/robot/Climber.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 0fcdd44..191f7a4 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -149,8 +149,8 @@ public void update() { // 00 RESTING: Default resting case RESTING: climberSolenoidA.set(false); - climberSolenoidB1.set(false); - climberSolenoidB2.set(false); + climberSolenoidB1.set(true); + climberSolenoidB2.set(true); climberSolenoidC.set(false); break; From b7c0e4107317b6301152414d60559abd209f32b3 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 3 Mar 2022 16:40:55 -0500 Subject: [PATCH 56/66] Manually override formatting --- src/main/java/frc/robot/Climber.java | 30 ++++++++++++++-------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 5d0ce2e..949fd3d 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -14,21 +14,21 @@ public class Climber implements Loggable { enum ClimbingStates { - RESTING(0, "Default resting"), - PRE_STAGE(5,"Rotate climber and set pre-stage pin position (button)"), - TOUCH_A(10,"Pin A (button/sensor)"), - ROTATE_B(15,"Rotate to B bar (photogate)"), - TOUCH_AB(20,"Pin B (high current/sensor)"), - ROTATE_AB_DOWN(25,"Rotate down to plumb (photogate)"), - RELEASE_A(30,"Unpin A (gyro/accel)"), - ROTATE_B_DOWN(35,"Wait for swinging (photogate)"), - ROTATE_C(40,"Rotate to C bar (gyro/accel)"), - TOUCH_BC(50,"Pin C (high current/sensor)"), - ROTATE_BC_DOWN(55,"Rotate down to plumb (photogate)"), - RELEASE_B(60,"Unpin B (gyro/accel)"), - ROTATE_C_DOWN(65,"Wait for swinging ()"), - DONE(70,"Climbing is done"), - ERROR(100,"Error"); + RESTING(0, "Default resting"), // + PRE_STAGE(5, "Rotate climber and set pre-stage pin position (button)"), // + TOUCH_A(10, "Pin A (button/sensor)"), // + ROTATE_B(15, "Rotate to B bar (photogate)"), // + TOUCH_AB(20, "Pin B (high current/sensor)"), // + ROTATE_AB_DOWN(25, "Rotate down to plumb (photogate)"), // + RELEASE_A(30, "Unpin A (gyro/accel)"), // + ROTATE_B_DOWN(35, "Wait for swinging (photogate)"), // + ROTATE_C(40, "Rotate to C bar (gyro/accel)"), // + TOUCH_BC(50, "Pin C (high current/sensor)"), // + ROTATE_BC_DOWN(55, "Rotate down to plumb (photogate)"), // + RELEASE_B(60, "Unpin B (gyro/accel)"), // + ROTATE_C_DOWN(65, "Wait for swinging ()"), // + DONE(70, "Climbing is done"), // + ERROR(100, "Error"); public int id; public String name; From 7c3cc737d3eb82e85927cecd9db288ec4c4495c8 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 3 Mar 2022 16:41:49 -0500 Subject: [PATCH 57/66] Remove old comment section --- src/main/java/frc/robot/Climber.java | 42 ++++++++++------------------ 1 file changed, 15 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 563c72c..e50c55b 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -39,22 +39,6 @@ enum ClimbingStates { } } - // 00 RESTING: Default resting - // 05 PRE_STAGE: Rotate climber and set pre-stage pin position (button) - // 10 TOUCH_A: Pin A (button/sensor) - // 15 ROTATE_B: Rotate to B bar (photogate) - // 20 TOUCH_AB: Pin B (high current/sensor) - // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) - // 30 RELEASE_A: Unpin A (gyro/accel) - // 35 ROTATE_B_DOWN: Wait for swinging (photogate) - // 40 ROTATE_C: Rotate to C bar (gyro/accel) - // 50 TOUCH_BC: Pin C (high current/sensor) - // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) - // 60 RELEASE_B: Unpin B (gyro/accel) - // 65 ROTATE_C_DOWN: Wait for swinging () - // 70 DONE: Climbing is done - // 100 ERROR: Error - enum MotorStates { STATIC, ACTIVE; } @@ -65,11 +49,11 @@ enum MotorStates { public static double NEXT_BC_STATE_CURRENT = 35.0; public static int FILTER_FRAME_RANGE = 10; - public static double TOUCH_A_POSITION = 0; //TBD - public static double SWING_AB_POSITION = 0; //TBD - public static double SWING_B_POSITION = 0; //TBD - public static double SWING_BC_POSITION = 0; //TBD - public static double SWING_MIN_VELOCITY = 1000; //TBD + public static double TOUCH_A_POSITION = 0; // TBD + public static double SWING_AB_POSITION = 0; // TBD + public static double SWING_B_POSITION = 0; // TBD + public static double SWING_BC_POSITION = 0; // TBD + public static double SWING_MIN_VELOCITY = 1000; // TBD TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -91,7 +75,7 @@ enum MotorStates { LoggableFirstOrderFilter rightFilter; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC //) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC // ) { , ClimberGates gates) { // , LoggableGyro gyro) { // , ClimberSensors touch) { @@ -175,7 +159,7 @@ public void update() { // Uncomment after testing // if (gates.getA()) { - // setClimbingState(ClimbingStates.ROTATE_B); + // setClimbingState(ClimbingStates.ROTATE_B); // } break; @@ -205,7 +189,8 @@ public void update() { case ROTATE_AB_DOWN: this.setPower(0); // TODO: set motor target here - if (Math.abs(climbingMotor.getSelectedSensorPosition()-SWING_AB_POSITION) < 500) {//Determine tolerance + if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_AB_POSITION) < 500) {// Determine + // tolerance System.out.println("DONE SWINGING!"); // setClimbingState(ClimbingStates.RELEASE_A); } @@ -227,8 +212,10 @@ public void update() { case ROTATE_B_DOWN: // TODO: set motor target here this.setPower(0); - if (Math.abs(climbingMotor.getSelectedSensorPosition()-SWING_B_POSITION) < 500 - && Math.abs(climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {//Determine tolerance + if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 500 + && Math.abs( + climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {// Determine + // tolerance System.out.println("DONE SWINGING!"); // setClimbingState(ClimbingStates.ROTATE_C); } @@ -261,7 +248,8 @@ public void update() { case ROTATE_BC_DOWN: // TODO: set motor target here this.setPower(0); - if (Math.abs(climbingMotor.getSelectedSensorPosition()-SWING_BC_POSITION) < 500) {//Determine tolerance + if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_BC_POSITION) < 500) {// Determine + // tolerance System.out.println("DONE SWINGING!"); // setClimbingState(ClimbingStates.RELEASE_B); } From 5f40e264d9e35d554babacdf00edacd8af31047e Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 3 Mar 2022 16:43:57 -0500 Subject: [PATCH 58/66] Add more allowed words --- .vscode/settings.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5fb10b8..2dc4958 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -43,6 +43,7 @@ "loggables", "lvuser", "markdownlint", + "photogate", "puppycrawl" ], "java.checkstyle.configuration": "${workspaceFolder}/checkstyle.xml", From a2533ad65efbb60253ffa0d0d4f1bdd96f2ddf11 Mon Sep 17 00:00:00 2001 From: Programmer Date: Mon, 14 Mar 2022 23:14:02 -0400 Subject: [PATCH 59/66] It worked 3 times in a row... --- .wpilib/wpilib_preferences.json | 2 +- src/main/java/frc/robot/Climber.java | 40 ++++++++++++++++------------ src/main/java/frc/robot/Robot.java | 8 +++--- 3 files changed, 28 insertions(+), 22 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 388ae55..951e239 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2022", - "teamNumber": 1742 + "teamNumber": 1741 } \ No newline at end of file diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index e50c55b..a667f11 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -45,8 +45,8 @@ enum MotorStates { public static double MAX_INSTANT_CURRENT = 200.0; public static double MAX_AVERAGE_CURRENT = 110.0; - public static double NEXT_AB_STATE_CURRENT = 55.0; - public static double NEXT_BC_STATE_CURRENT = 35.0; + public static double NEXT_AB_STATE_CURRENT = 65.0; + public static double NEXT_BC_STATE_CURRENT = 45.0; public static int FILTER_FRAME_RANGE = 10; public static double TOUCH_A_POSITION = 0; // TBD @@ -101,7 +101,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climbingMotor.config_kI(0, 0.001); this.climbingMotor.config_kD(0, 5); - secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); + climbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climber/Time"); @@ -166,7 +166,7 @@ public void update() { // 15 ROTATE_B: Rotate to B bar (photogate) case ROTATE_B: // TODO: set motor power here - this.setSpeed(0.15); + this.setSpeed(-0.4); if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_AB); @@ -176,13 +176,14 @@ public void update() { // 20 TOUCH_AB: Pin B (high current/sensor) case TOUCH_AB: + this.setSpeed(0); climberSolenoidA.set(false); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); - if (gates.getB1()) { - setClimbingState(ClimbingStates.ROTATE_AB_DOWN); - } + // if (gates.getB1()) { + // setClimbingState(ClimbingStates.ROTATE_AB_DOWN); + // } break; // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) @@ -198,14 +199,15 @@ public void update() { // 30 RELEASE_A: Unpin A (gyro/accel) case RELEASE_A: + this.setPower(0); climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); // TODO: set motor target here - if (!gates.getA()) { - setClimbingState(ClimbingStates.ROTATE_B_DOWN);; - } + // if (!gates.getA()) { + // setClimbingState(ClimbingStates.ROTATE_B_DOWN);; + // } break; // 35 ROTATE_B_DOWN: Wait for swinging (photogate) @@ -224,7 +226,7 @@ public void update() { // 40 ROTATE_C: Rotate to C bar (gyro/accel) case ROTATE_C: // TODO: set motor target here - this.setSpeed(0.15); + this.setSpeed(-0.2); if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { setClimbingState(ClimbingStates.TOUCH_BC); @@ -234,14 +236,15 @@ public void update() { // 50 TOUCH_BC: Pin C (high current/sensor) case TOUCH_BC: + this.setSpeed(-0.05); climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(false); // TODO: set motor target here - if (gates.getC()) { - setClimbingState(ClimbingStates.ROTATE_BC_DOWN); - } + // if (gates.getC()) { + // setClimbingState(ClimbingStates.ROTATE_BC_DOWN); + // } break; // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) @@ -257,13 +260,14 @@ public void update() { // 60 RELEASE_B: Unpin B (gyro/accel) case RELEASE_B: + this.setPower(0); climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(true); climberSolenoidC.set(false); - if (!gates.getB2()) { - setClimbingState(ClimbingStates.ROTATE_C_DOWN); - } + // if (!gates.getB2()) { + // setClimbingState(ClimbingStates.ROTATE_C_DOWN); + // } break; // 65 ROTATE_C_DOWN: Wait for swinging () @@ -432,6 +436,7 @@ public void setupLogging(Logger logger) { logger.addAttribute("Climber/Right/Current"); logger.addAttribute("Climber/Speed"); + logger.addAttribute("Climber/Position"); logger.addAttribute("Climber/State/Name"); logger.addAttribute("Climber/State/Id"); @@ -445,6 +450,7 @@ public void log(Logger logger) { logger.log("Climber/Right/Current", getRightCurrent()); logger.log("Climber/Speed", getSpeed()); + logger.log("Climber/Position", climbingMotor.getSelectedSensorPosition()); logger.log("Climber/State/Name", this.currentClimberState.name); logger.log("Climber/State/Id", this.currentClimberState.id); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index baddd61..83ff985 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,7 +45,7 @@ public class Robot extends TimedRobot { boolean climberEnabled = true; private static final double DEADBAND_LIMIT = 0.01; - private static final double SPEED_CAP = 0.6; + private static final double SPEED_CAP = 0.3; InputScaler joystickDeadband = new Deadband(DEADBAND_LIMIT); InputScaler joystickSquared = new SquaredInput(DEADBAND_LIMIT); BoostInput boost = new BoostInput(SPEED_CAP); @@ -145,7 +145,7 @@ public void teleopPeriodic() { if (tankDriveEnabled) { double leftInput = deadband(-driver.getLeftY()); double rightInput = deadband(-driver.getRightY()); - drive.tankDrive(leftInput * 0.4, rightInput * 0.4); + drive.tankDrive(leftInput, rightInput); } else { double turnInput = deadband(driver.getRightX()); double speedInput = deadband(-driver.getLeftY()); @@ -164,6 +164,8 @@ public void teleopPeriodic() { } if (this.climberEnabled) { + double climberInput = deadband(operator.getLeftY()); + climber.setMotors(climberInput); if (operator.getLeftBumperPressed()) { climber.setClimbingState(climber.getNextClimbingState()); } @@ -176,8 +178,6 @@ public void teleopPeriodic() { : MotorStates.ACTIVE); } // TODO: Enable this when we're ready to test the climber - double climberInput = deadband(operator.getLeftY()); - climber.setMotors(climberInput); } logger.log(); From 5a1d54687b5f2d9947faa872d14b7c85f00562bf Mon Sep 17 00:00:00 2001 From: Programmer Date: Mon, 21 Mar 2022 18:06:08 -0400 Subject: [PATCH 60/66] Comp Code --- build.gradle | 2 + src/main/deploy/autos/auto-test.json | 28 ++++ src/main/deploy/autos/autonomous.json | 13 ++ src/main/deploy/autos/current-test.json | 28 ++++ src/main/deploy/autos/distance-test.json | 76 ++++++++++ src/main/deploy/autos/shooter-test.json | 17 +++ src/main/java/frc/robot/Autonomous.java | 5 + src/main/java/frc/robot/BoostInput.java | 28 ++-- src/main/java/frc/robot/Climber.java | 125 +++++++++------- src/main/java/frc/robot/ClimberGates.java | 18 +-- src/main/java/frc/robot/Drivetrain.java | 7 + src/main/java/frc/robot/Manipulation.java | 82 +++++++++++ src/main/java/frc/robot/Robot.java | 168 ++++++++++++++++------ src/main/java/frc/robot/Shooter.java | 41 ++++++ vendordeps/REVLib.json | 73 ++++++++++ 15 files changed, 597 insertions(+), 114 deletions(-) create mode 100644 src/main/deploy/autos/auto-test.json create mode 100644 src/main/deploy/autos/autonomous.json create mode 100644 src/main/deploy/autos/current-test.json create mode 100644 src/main/deploy/autos/distance-test.json create mode 100644 src/main/deploy/autos/shooter-test.json create mode 100644 src/main/java/frc/robot/Autonomous.java create mode 100644 src/main/java/frc/robot/Manipulation.java create mode 100644 src/main/java/frc/robot/Shooter.java create mode 100644 vendordeps/REVLib.json diff --git a/build.gradle b/build.gradle index 104b058..7df1e34 100644 --- a/build.gradle +++ b/build.gradle @@ -54,6 +54,8 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation 'com.google.code.gson:gson:2.9.0' + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/src/main/deploy/autos/auto-test.json b/src/main/deploy/autos/auto-test.json new file mode 100644 index 0000000..d8d2137 --- /dev/null +++ b/src/main/deploy/autos/auto-test.json @@ -0,0 +1,28 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "SECONDS", + "amount": 0.1, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "wait", + "args": [ + 1 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": -90, + "args": [ + 0.1, + -0.1 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/autonomous.json b/src/main/deploy/autos/autonomous.json new file mode 100644 index 0000000..9775e0b --- /dev/null +++ b/src/main/deploy/autos/autonomous.json @@ -0,0 +1,13 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "FEET", + "amount": 3.00, + "args": [ + -0.1, + -0.1 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/current-test.json b/src/main/deploy/autos/current-test.json new file mode 100644 index 0000000..92e6352 --- /dev/null +++ b/src/main/deploy/autos/current-test.json @@ -0,0 +1,28 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "CURRENT", + "amount": 20, + "args": [ + 0.05, + 0.05 + ] + }, + { + "type": "wait", + "args": [ + 1 + ] + }, + { + "type": "drive", + "unit": "FEET", + "amount": 5, + "args": [ + -0.05, + -0.05 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/distance-test.json b/src/main/deploy/autos/distance-test.json new file mode 100644 index 0000000..01bd8fa --- /dev/null +++ b/src/main/deploy/autos/distance-test.json @@ -0,0 +1,76 @@ +{ + "auto": [ + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + }, + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + }, + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + }, + { + "type": "drive", + "unit": "INCHES", + "amount": 50, + "args": [ + -0.05, + -0.05 + ] + }, + { + "type": "turnDeg", + "unit": "DEGREES", + "amount": 90, + "args": [ + -0.05, + 0.05 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/shooter-test.json b/src/main/deploy/autos/shooter-test.json new file mode 100644 index 0000000..18204e8 --- /dev/null +++ b/src/main/deploy/autos/shooter-test.json @@ -0,0 +1,17 @@ +{ + "auto": [ + { + "type": "shoot", + "unit": "SPEED", + "args": [ + 1000 + ] + }, + { + "type": "wait", + "args": [ + 5 + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autonomous.java b/src/main/java/frc/robot/Autonomous.java new file mode 100644 index 0000000..2c45964 --- /dev/null +++ b/src/main/java/frc/robot/Autonomous.java @@ -0,0 +1,5 @@ +package frc.robot; + +public abstract class Autonomous { + public abstract void run(); +} diff --git a/src/main/java/frc/robot/BoostInput.java b/src/main/java/frc/robot/BoostInput.java index ebe4891..9b7d159 100644 --- a/src/main/java/frc/robot/BoostInput.java +++ b/src/main/java/frc/robot/BoostInput.java @@ -1,21 +1,21 @@ package frc.robot; -public class BoostInput implements InputScaler -{ - private final double powerCap; - private boolean enabled; +public class BoostInput implements InputScaler { + private final double powerCap; + private double boostScale; - public BoostInput(double powerCap) { - this.powerCap = powerCap; - this.enabled = false; - } + public BoostInput(double powerCap) { + this.powerCap = powerCap; + this.boostScale = 0; + } - public double scale(double input) { - return enabled ? input : input * powerCap; - } + public double scale(double input) { + double scale = (1 - powerCap) * boostScale; + return input * (powerCap + scale); + } - public void setEnabled(boolean enabled) { - this.enabled = enabled; - } + public void setScale(double boostScale) { + this.boostScale = boostScale; + } } diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index a667f11..f7cee72 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -44,16 +44,18 @@ enum MotorStates { } public static double MAX_INSTANT_CURRENT = 200.0; - public static double MAX_AVERAGE_CURRENT = 110.0; + public static double MAX_AVERAGE_CURRENT = 120.0; public static double NEXT_AB_STATE_CURRENT = 65.0; public static double NEXT_BC_STATE_CURRENT = 45.0; public static int FILTER_FRAME_RANGE = 10; - public static double TOUCH_A_POSITION = 0; // TBD - public static double SWING_AB_POSITION = 0; // TBD - public static double SWING_B_POSITION = 0; // TBD - public static double SWING_BC_POSITION = 0; // TBD - public static double SWING_MIN_VELOCITY = 1000; // TBD + public static double TOUCH_A_POSITION = 144000; // TBD + public static double SWING_AB_POSITION = 43000; // TBD + public static double SWING_B_POSITION = 900; // TBD + public static double SWING_BC_POSITION = 107000; // TBD + public static double SWING_MIN_VELOCITY = 1500; // TBD + + private double motorSpeed; TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -117,24 +119,24 @@ public void update() { this.rightFilter.update(secondaryClimbingMotor.getStatorCurrent()); // Make sure we're not pulling too much current instantly - if (climbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT) { - setClimbingState(ClimbingStates.ERROR); - System.out.println("--------HIT MAX CURRENT--------"); - } - - // Make sure we're not pulling too much current over time - if (leftFilter.get() > MAX_AVERAGE_CURRENT || rightFilter.get() > MAX_AVERAGE_CURRENT) { - System.out.println("--------HIT MAX AVERAGE CURRENT--------"); - setClimbingState(ClimbingStates.ERROR); - } + // if (climbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT + // || secondaryClimbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT) { + // setClimbingState(ClimbingStates.ERROR); + // System.out.println("--------HIT MAX CURRENT--------"); + // } + + // // Make sure we're not pulling too much current over time + // if (leftFilter.get() > MAX_AVERAGE_CURRENT || rightFilter.get() > MAX_AVERAGE_CURRENT) { + // System.out.println("--------HIT MAX AVERAGE CURRENT--------"); + // setClimbingState(ClimbingStates.ERROR); + // } switch (this.currentClimberState) { // 00 RESTING: Default resting case RESTING: climberSolenoidA.set(false); - climberSolenoidB1.set(true); - climberSolenoidB2.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); climberSolenoidC.set(false); break; @@ -147,11 +149,12 @@ public void update() { climberSolenoidB2.set(false); climberSolenoidC.set(true); // TODO: set motor target here - // climbingMotor.set(ControlMode.Position, TOUCH_A_POSITION); + setSpeed(motorSpeed); break; // 10 TOUCH_A: Pin A (button/sensor) case TOUCH_A: + setSpeed(0); climberSolenoidA.set(false); climberSolenoidB1.set(true); climberSolenoidB2.set(false); @@ -167,16 +170,20 @@ public void update() { case ROTATE_B: // TODO: set motor power here this.setSpeed(-0.4); - if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) { - setClimbingState(ClimbingStates.TOUCH_AB); - this.setSpeed(0); - } + climberSolenoidA.set(false); + climberSolenoidB1.set(true); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + // if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT + // || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) { + // setClimbingState(ClimbingStates.TOUCH_AB); + // this.setSpeed(0); + // } break; // 20 TOUCH_AB: Pin B (high current/sensor) case TOUCH_AB: - this.setSpeed(0); + this.setSpeed(-0.05); climberSolenoidA.set(false); climberSolenoidB1.set(false); climberSolenoidB2.set(false); @@ -190,11 +197,12 @@ public void update() { case ROTATE_AB_DOWN: this.setPower(0); // TODO: set motor target here - if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_AB_POSITION) < 500) {// Determine - // tolerance - System.out.println("DONE SWINGING!"); - // setClimbingState(ClimbingStates.RELEASE_A); - } + // if (climbingMotor.getSelectedSensorPosition() > SWING_AB_POSITION) {// Was less + // then + // // when broken + // System.out.println("DONE SWINGING!"); + // setClimbingState(ClimbingStates.RELEASE_A); + // } break; // 30 RELEASE_A: Unpin A (gyro/accel) @@ -206,7 +214,7 @@ public void update() { climberSolenoidC.set(true); // TODO: set motor target here // if (!gates.getA()) { - // setClimbingState(ClimbingStates.ROTATE_B_DOWN);; + setClimbingState(ClimbingStates.ROTATE_B_DOWN);; // } break; @@ -214,24 +222,28 @@ public void update() { case ROTATE_B_DOWN: // TODO: set motor target here this.setPower(0); - if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 500 - && Math.abs( - climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {// Determine - // tolerance - System.out.println("DONE SWINGING!"); - // setClimbingState(ClimbingStates.ROTATE_C); - } + // if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 1000 + // && Math.abs( + // climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {// Determine + // // tolerance + // System.out.println("DONE SWINGING!"); + // setClimbingState(ClimbingStates.ROTATE_C); + // } break; // 40 ROTATE_C: Rotate to C bar (gyro/accel) case ROTATE_C: // TODO: set motor target here - this.setSpeed(-0.2); - if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { - setClimbingState(ClimbingStates.TOUCH_BC); - this.setSpeed(0); - } + this.setSpeed(-0.3); + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + // if (leftFilter.get() > NEXT_BC_STATE_CURRENT + // || rightFilter.get() > NEXT_BC_STATE_CURRENT) { + // setClimbingState(ClimbingStates.TOUCH_BC); + // this.setSpeed(0); + // } break; // 50 TOUCH_BC: Pin C (high current/sensor) @@ -251,11 +263,11 @@ public void update() { case ROTATE_BC_DOWN: // TODO: set motor target here this.setPower(0); - if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_BC_POSITION) < 500) {// Determine - // tolerance - System.out.println("DONE SWINGING!"); - // setClimbingState(ClimbingStates.RELEASE_B); - } + // if (climbingMotor.getSelectedSensorPosition() < SWING_BC_POSITION) {// Determine + // // tolerance + // System.out.println("DONE SWINGING!"); + // setClimbingState(ClimbingStates.RELEASE_B); + // } break; // 60 RELEASE_B: Unpin B (gyro/accel) @@ -266,7 +278,7 @@ public void update() { climberSolenoidB2.set(true); climberSolenoidC.set(false); // if (!gates.getB2()) { - // setClimbingState(ClimbingStates.ROTATE_C_DOWN); + setClimbingState(ClimbingStates.ROTATE_C_DOWN); // } break; @@ -302,6 +314,14 @@ public ClimbingStates getNextClimbingState() { return ClimbingStates.values()[this.currentClimberState.ordinal() + 1]; } + public ClimbingStates getPreviousClimbingState() { + return ClimbingStates.values()[this.currentClimberState.ordinal() - 1]; + } + + public int getClimberStateId() { + return this.currentClimberState.ordinal(); + } + public void disableClimber() { // Stop the motors this.climbingMotor.set(ControlMode.PercentOutput, 0); @@ -397,7 +417,8 @@ public void setMotors(double value) { break; case ACTIVE: - setSpeed(value); + motorSpeed = value; + setSpeed(motorSpeed); break; default: diff --git a/src/main/java/frc/robot/ClimberGates.java b/src/main/java/frc/robot/ClimberGates.java index ec244a8..21934f9 100644 --- a/src/main/java/frc/robot/ClimberGates.java +++ b/src/main/java/frc/robot/ClimberGates.java @@ -4,16 +4,18 @@ import frc.robot.logging.Loggable; import frc.robot.logging.Logger; -public class ClimberGates implements Loggable{ +public class ClimberGates implements Loggable { DigitalInput aL, aR, b1L, b1R, b2L, b2R, cL, cR; - public ClimberGates(int aLID, int aRID, int b1LID, int b1RID, int b2LID, int b2RID, int cLID, int cRID) { - this.aL = new DigitalInput(aLID); - this.aR = new DigitalInput(aRID); + public ClimberGates(int b1LID, int b1RID, int cLID, int cRID) {// int aLID, int aRID, int b1LID, + // int b1RID, int b2LID, int + // b2RID, int cLID, int cRID) { + // this.aL = new DigitalInput(aLID); + // this.aR = new DigitalInput(aRID); this.b1L = new DigitalInput(b1LID); this.b1R = new DigitalInput(b1RID); - this.b2L = new DigitalInput(b2LID); - this.b2R = new DigitalInput(b2RID); + // this.b2L = new DigitalInput(b2LID); + // this.b2R = new DigitalInput(b2RID); this.cL = new DigitalInput(cLID); this.cR = new DigitalInput(cRID); } @@ -36,9 +38,9 @@ public boolean getC() { @Override public void setupLogging(Logger logger) { - logger.log("GateA", this.getA()); + // logger.log("GateA", this.getA()); logger.log("GateB1", this.getB1()); - logger.log("GateB2", this.getB2()); + // logger.log("GateB2", this.getB2()); logger.log("GateC", this.getC()); } diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index e284eb3..0f44e31 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -11,6 +11,8 @@ public class Drivetrain implements Loggable { private final double SHIFT_CURRENT_LOW = 0; private final double SHIFT_VELOCITY = 0; // Wheel velocity + private boolean climbMode = false; + private DriveModule left; private DriveModule right; private Solenoid shifter; @@ -47,6 +49,10 @@ public void drive(double leftSpeed, double rightSpeed) { // Probably implement d right.set(rightSpeed); } + public void setClimbMode() { + climbMode = !climbMode; + } + /** * Drive with arcade-style controls. * @@ -54,6 +60,7 @@ public void drive(double leftSpeed, double rightSpeed) { // Probably implement d * @param speedInput The speed to drive */ public void arcadeDrive(double turnInput, double speedInput) { + speedInput = speedInput * (climbMode ? 0.3 : 1); this.drive(speedInput - turnInput, speedInput + turnInput); } diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java new file mode 100644 index 0000000..5c74c1e --- /dev/null +++ b/src/main/java/frc/robot/Manipulation.java @@ -0,0 +1,82 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class Manipulation implements Loggable { + + private CANSparkMax intakeWheel; + private DoubleSolenoid intakePneumatics; + private CANSparkMax indexLoad; + + private double speed; + private boolean spinning; + + /** + * Constructor + * + * @param pneumaticsForwardChannel The Solenoid id for the forward channel for the intake + * @param pneumaticsReverseChannel The Solenoid id for the reverse channel for the intake + * @param intakeWheelID The CAN id of the spark for the intake + * @param indexLoadID The CAN id of the spark for the index loader + * + */ + Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID, int indexLoadID) { + this.intakeWheel = new CANSparkMax(intakeWheelID, MotorType.kBrushless); + this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); + this.intakePneumatics = new DoubleSolenoid(PneumaticsModuleType.REVPH, pneumaticsForwardChannel, pneumaticsReverseChannel); + + intakeWheel.setInverted(true); + } + + /** + * Spins the intake motor + * + * @param spin True if the motor should spin; false if not + * + */ + public void setIntakeSpin(boolean spin) { + this.speed = spin ? 0.3 : 0.0; + intakeWheel.set(speed); + this.spinning = spin; + } + + /** + * Moves the intake system + * + * @param extend True if the pneumatics should extend; false if not + * + */ + public void setIntakeExtend(boolean extend) { + intakePneumatics.set(extend ? Value.kForward : Value.kReverse); + } + /** + * Moves power cells down indexing system + * + * @param load True if it should load; false if not + * + */ + public void setIndexLoad(boolean load) { + indexLoad.set(load ? 0.5 : 0.0); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("Manipulation/IntakeWheelSpeed"); + logger.addAttribute("Manipulation/IntakeWheelEnabled"); + } + + @Override + public void log(Logger logger) { + logger.log("Manipulation/IntakeWheelSpeed", speed); + logger.log("Manipulation/IntakeWheelEnabled", spinning); + } + +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 83ff985..2380a4d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,13 @@ package frc.robot; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.Solenoid; @@ -33,19 +40,28 @@ public class Robot extends TimedRobot { Climber climber; // LoggableGyro gyro; + Shooter shooter; + Manipulation manipulation; + CANSparkMax tempClimber; LoggableController driver; LoggableController operator; LoggablePowerDistribution pdp; LoggableCompressor compressor; + // ClimberGates climberGates; + LoggableGyro gyro; boolean drivetrainEnabled = true; - boolean tankDriveEnabled = true; - boolean climberEnabled = true; + boolean tankDriveEnabled = false; + boolean climberEnabled = false; + boolean manipulationEnabled = false; + double shootSpeed = 0.40; + + // private JsonAutonomous auto; private static final double DEADBAND_LIMIT = 0.01; - private static final double SPEED_CAP = 0.3; + private static final double SPEED_CAP = 0.45; InputScaler joystickDeadband = new Deadband(DEADBAND_LIMIT); InputScaler joystickSquared = new SquaredInput(DEADBAND_LIMIT); BoostInput boost = new BoostInput(SPEED_CAP); @@ -64,7 +80,8 @@ public void robotInit() { logger = new Logger(); timer = new LoggableTimer(); logger.addLoggable(timer); - // gyro = new LoggableGyro(); + gyro = new LoggableGyro(); + gyro.enableLogging(false); pdp = new LoggablePowerDistribution(1, ModuleType.kRev); @@ -74,18 +91,19 @@ public void robotInit() { if (this.climberEnabled) { System.out.println("Initializing climber..."); - Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); - Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); - Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); - Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); + // Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); + // Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); + // Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); + // Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); - // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); - ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); - climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC, climberGates);// ,gyro, climberSensors); + // // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); + // climberGates = new ClimberGates(4, 5, 6, 7); + // climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, + // climberSolenoidC, climberGates);// ,gyro, climberSensors); - // logger.addLoggable(climberSensors); - logger.addLoggable(climber); + // // logger.addLoggable(climberSensors); + // logger.addLoggable(climberGates); + // logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); } @@ -105,6 +123,18 @@ public void robotInit() { System.out.println("Drivetrain initialization disabled."); } + // shooter = new Shooter(new TalonFX(6)); + + if (manipulationEnabled) { + System.out.println("Initializing manipulation..."); + manipulation = new Manipulation(0, 1, 7, 8); + } else { + System.out.println("Manipulation initialization disabled."); + } + + tempClimber = new CANSparkMax(8, MotorType.kBrushless); + tempClimber.setIdleMode(IdleMode.kBrake); + System.out.print("Initializing compressor..."); compressor = new LoggableCompressor(PneumaticsModuleType.REVPH); System.out.println("done"); @@ -118,17 +148,37 @@ public void robotInit() { public void robotPeriodic() { // Robot code goes here drive.update(); - climber.update(); + // climber.update(); } @Override public void autonomousInit() { + gyro.reset(); + timer.reset(); + + // auto = new JsonAutonomous("/home/lvuser/deploy/autos/autonomous.json", gyro, drive, + // shooter, + // manipulation); + // System.out.println("Auto Initialized"); + // logger.addLoggable(auto); resetLogging(); } @Override public void autonomousPeriodic() { // Robot code goes here + + if (timer.get() < 2) { + drive.tankDrive(-0.3, -0.3); + } else { + drive.tankDrive(0, 0); + } + + leftModule.updateCurrent(); + rightModule.updateCurrent(); + System.out.println("running - Robot"); + // auto.run(); + logger.log(); logger.writeLine(); } @@ -142,42 +192,80 @@ public void teleopInit() { public void teleopPeriodic() { // Robot code goes here if (this.drivetrainEnabled) { - if (tankDriveEnabled) { - double leftInput = deadband(-driver.getLeftY()); - double rightInput = deadband(-driver.getRightY()); - drive.tankDrive(leftInput, rightInput); - } else { - double turnInput = deadband(driver.getRightX()); - double speedInput = deadband(-driver.getLeftY()); - boost.setEnabled(driver.getRightTriggerAxis() > 0.5); - drive.arcadeDrive(turnInput, boost.scale(speedInput)); - } + // if (tankDriveEnabled) { + // double leftInput = deadband(-driver.getLeftY()); + // double rightInput = deadband(-driver.getRightY()); + // drive.tankDrive(leftInput, rightInput); + // } else { + double turnInput = deadband(driver.getRightX()) * -0.3; + double speedInput = deadband(driver.getLeftY()); + boost.setScale(driver.getRightTriggerAxis()); + drive.arcadeDrive(turnInput, boost.scale(speedInput)); + // } if (driver.getXButtonPressed()) { tankDriveEnabled = !tankDriveEnabled; } - if (driver.getLeftBumperPressed()) { - drive.setShifter(!drive.getShifter()); + // if (driver.getAButtonPressed()) { + // drive.setShifter(!drive.getShifter()); + // } + if (driver.getAButtonPressed()) { + drive.setClimbMode(); } leftModule.updateCurrent(); rightModule.updateCurrent(); } - if (this.climberEnabled) { - double climberInput = deadband(operator.getLeftY()); - climber.setMotors(climberInput); - if (operator.getLeftBumperPressed()) { - climber.setClimbingState(climber.getNextClimbingState()); - } - // climber.checkClimbingState(operator.getAButtonPressed()); + // if (this.climberEnabled) { + // double climberInput = deadband(operator.getLeftY()); + // climber.setMotors(climberInput); + // double tempClimberInput = deadband(operator.getRightY()); + // tempClimber.set(tempClimberInput); + // if (operator.getLeftBumperPressed()) { + // climber.setClimbingState(climber.getNextClimbingState()); + // } + // // climber.checkClimbingState(operator.getAButtonPressed()); + + // // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro + // if (operator.getRightBumperPressed()) { + // climber.setMotorState( + // climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC + // : MotorStates.ACTIVE); + // } + + // if (operator.getBButtonPressed() && climber.getClimberStateId() != 0) { + // climber.setClimbingState(climber.getPreviousClimbingState()); + // } + // // TODO: Enable this when we're ready to test the climber + // } + + double tempClimberInput = deadband(operator.getRightY()); + tempClimber.set(tempClimberInput); + + // System.out.println((climberGates.getB1() ? "B1: true" : "B1: false") + " " + // + (climberGates.getC() ? "C: true" : "C: false")); + + // if (operator.getYButtonPressed()) { + // shootSpeed += 0.01; + // System.out.println(shootSpeed); + // } else if (operator.getAButtonPressed()) { + // shootSpeed -= 0.01; + // System.out.println(shootSpeed); + // } else if (operator.getBButtonPressed()) { + // shootSpeed = 0; + // } + + // shooter.setPower(operator.getRightTriggerAxis()); + // shooter.setPower(operator.getRightTriggerAxis() > 0.5 ? shootSpeed : 0); - // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro - if (operator.getRightBumperPressed()) { - climber.setMotorState( - climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC - : MotorStates.ACTIVE); + if (this.manipulationEnabled) { + if (driver.getRightBumperPressed()) { + manipulation.setIntakeExtend(true); + } else if (driver.getLeftBumperPressed()) { + manipulation.setIntakeExtend(false); } - // TODO: Enable this when we're ready to test the climber + manipulation.setIntakeSpin(operator.getYButton()); + manipulation.setIndexLoad(operator.getLeftTriggerAxis() > 0.5); } logger.log(); diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java new file mode 100644 index 0000000..05ac672 --- /dev/null +++ b/src/main/java/frc/robot/Shooter.java @@ -0,0 +1,41 @@ +package frc.robot; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class Shooter implements Loggable { + TalonFX shooterMotor; + + public Shooter(TalonFX s) { + this.shooterMotor = s; + + this.shooterMotor.setNeutralMode(NeutralMode.Coast); + } + + public void setSpeed(double speed) { + this.shooterMotor.set(ControlMode.Velocity, speed); + } + + public void setPower(double power) { + this.shooterMotor.set(ControlMode.PercentOutput, power); + } + + public double getSpeed() { + return this.shooterMotor.getSelectedSensorVelocity(); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("Shooter/speed"); + } + + @Override + public void log(Logger logger) { + logger.log("Shooter/speed", this.getSpeed()); + } +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..997e2a4 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} \ No newline at end of file From 982f8ca75b088e688a11710b18f7071c77178f7a Mon Sep 17 00:00:00 2001 From: Programmer Date: Thu, 7 Apr 2022 19:53:22 -0400 Subject: [PATCH 61/66] Tippy code featuring autobalancing & speed control --- src/main/java/frc/robot/ClimberGates.java | 6 +- src/main/java/frc/robot/DriveModule.java | 26 +++- src/main/java/frc/robot/Drivetrain.java | 53 ++++++-- src/main/java/frc/robot/Manipulation.java | 4 +- src/main/java/frc/robot/Robot.java | 147 +++++++++++----------- 5 files changed, 147 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc/robot/ClimberGates.java b/src/main/java/frc/robot/ClimberGates.java index 21934f9..6927034 100644 --- a/src/main/java/frc/robot/ClimberGates.java +++ b/src/main/java/frc/robot/ClimberGates.java @@ -46,7 +46,9 @@ public void setupLogging(Logger logger) { @Override public void log(Logger logger) { - // TODO Auto-generated method stub - + // logger.log("GateA", this.getA()); + logger.log("GateB1", this.getB2()); + // logger.log("GateB2", this.getB2()); + logger.log("GateC", this.getC()); } } diff --git a/src/main/java/frc/robot/DriveModule.java b/src/main/java/frc/robot/DriveModule.java index a422020..3872dea 100644 --- a/src/main/java/frc/robot/DriveModule.java +++ b/src/main/java/frc/robot/DriveModule.java @@ -10,7 +10,7 @@ public class DriveModule implements Loggable { - private final double VELOCITY_COEFFICIENT = 600 / 2048; + private final double MAX_VELOCITY = 22000; private TalonFX main; private TalonFX sub; @@ -18,6 +18,7 @@ public class DriveModule implements Loggable { private Encoder encoder; private double power; + private double targetSpeed; private double[] current = new double[30]; private int indexCurrent; @@ -39,9 +40,13 @@ public class DriveModule implements Loggable { this.sub.follow(this.main); main.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); - main.config_kP(0, 0.1); - main.config_kI(0, 0.001); - main.config_kD(0, 5); + main.config_kF(0, 1023 / MAX_VELOCITY); + main.config_kP(0, (0.1 * 1023) / 800); + main.config_kI(0, 0.000); + main.config_kD(0, 0); + main.configMotionCruiseVelocity(MAX_VELOCITY / 2); + main.configMotionAcceleration(MAX_VELOCITY / 2); + main.configClosedloopRamp(0.3); indexCurrent = 0; } @@ -84,7 +89,12 @@ public void set(double input) { * @param speed The speed to set the module to */ public void setSpeed(double speed) { - main.set(TalonFXControlMode.Velocity, speed * 22000); + targetSpeed = speed * MAX_VELOCITY; + main.set(TalonFXControlMode.Velocity, targetSpeed); + } + + public void setMagic(double speed) { + main.set(TalonFXControlMode.MotionMagic, speed * 22000); } /** @@ -93,7 +103,7 @@ public void setSpeed(double speed) { * @return velocity (rpm) of the motor */ public double getSpeed() { - return main.getSelectedSensorVelocity();// * VELOCITY_COEFFICIENT; + return main.getSelectedSensorVelocity(); } /** @@ -131,7 +141,9 @@ public void setupLogging(Logger logger) { logger.addAttribute(this.moduleName + "/MotorPower"); logger.addAttribute(this.moduleName + "/Distance"); logger.addAttribute(this.moduleName + "/EncoderRate"); + logger.addAttribute(this.moduleName + "/TargetVelocity"); logger.addAttribute(this.moduleName + "/MotorVelocity"); + logger.addAttribute(this.moduleName + "/VelocityError"); logger.addAttribute(this.moduleName + "/MotorCurrent"); logger.addAttribute(this.moduleName + "/MotorAverageCurrent"); } @@ -141,7 +153,9 @@ public void log(Logger logger) { logger.log(this.moduleName + "/MotorPower", power); logger.log(this.moduleName + "/Distance", this.encoder.getDistance()); logger.log(this.moduleName + "/EncoderRate", this.encoder.getRate()); + logger.log(this.moduleName + "/TargetVelocity", targetSpeed); logger.log(this.moduleName + "/MotorVelocity", getSpeed()); + logger.log(this.moduleName + "/VelocityError", targetSpeed - getSpeed()); logger.log(this.moduleName + "/MotorCurrent", getCurrent()); logger.log(this.moduleName + "/MotorAverageCurrent", getAverageCurrent()); } diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index 0f44e31..aa2e148 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; import frc.robot.logging.Loggable; +import frc.robot.logging.LoggableGyro; import frc.robot.logging.Logger; public class Drivetrain implements Loggable { @@ -10,12 +11,19 @@ public class Drivetrain implements Loggable { private final double SHIFT_CURRENT_HIGH = 80; // TODO Get actual values when we test drivetrain private final double SHIFT_CURRENT_LOW = 0; private final double SHIFT_VELOCITY = 0; // Wheel velocity + private final double OFF_BALANCE_THRESHOLD = 10; + private final double ON_BALANCE_THRESHOLD = 5; - private boolean climbMode = false; + private boolean climbMode; + private boolean autoBalanceMode; + private boolean autoBalanceEnabled; + + private double balanceScale; private DriveModule left; private DriveModule right; private Solenoid shifter; + private LoggableGyro gyro; /** * Constructor. @@ -24,18 +32,44 @@ public class Drivetrain implements Loggable { * @param right DriveModule of the drivetrain's right half * @param shifter ID for the shifter solenoid */ - Drivetrain(DriveModule left, DriveModule right, int shifterID) { + Drivetrain(DriveModule left, DriveModule right, int shifterID, LoggableGyro gyro) { this.left = left; this.right = right; this.shifter = new Solenoid(PneumaticsModuleType.REVPH, shifterID); + this.gyro = gyro; + right.setInverted(true); + + climbMode = false; + autoBalanceMode = false; + autoBalanceEnabled = true; } public void update() { this.left.update(); this.right.update(); + this.autoBalance(); + } + + private void autoBalance() { + if (!autoBalanceMode && Math.abs(gyro.getPitch()) >= Math.abs(OFF_BALANCE_THRESHOLD)) { + autoBalanceMode = true; + } else if (autoBalanceMode && Math.abs(gyro.getPitch()) <= Math.abs(ON_BALANCE_THRESHOLD)) { + autoBalanceMode = false; + } + + if (autoBalanceMode) { + double pitchAngleRadians = gyro.getPitch() * (Math.PI / 180.0); + balanceScale = Math.sin(pitchAngleRadians) * 0.5; + } else { + balanceScale = 0; + } + } + + public void toggleAutoBalance() { + autoBalanceEnabled = !autoBalanceEnabled; } /** @@ -45,12 +79,12 @@ public void update() { * @param rightSpeed The speed of the right motors */ public void drive(double leftSpeed, double rightSpeed) { // Probably implement deadbands later - left.set(leftSpeed); - right.set(rightSpeed); + left.setSpeed(leftSpeed); + right.setSpeed(rightSpeed); } - public void setClimbMode() { - climbMode = !climbMode; + public void setClimbMode(boolean climb) { + climbMode = climb; } /** @@ -60,7 +94,8 @@ public void setClimbMode() { * @param speedInput The speed to drive */ public void arcadeDrive(double turnInput, double speedInput) { - speedInput = speedInput * (climbMode ? 0.3 : 1); + speedInput = + climbMode ? speedInput * 0.3 : speedInput + (autoBalanceEnabled ? balanceScale : 0); this.drive(speedInput - turnInput, speedInput + turnInput); } @@ -127,8 +162,8 @@ public void checkGears() { @Override public void setupLogging(Logger logger) { - // logger.addLoggable(left); - // logger.addLoggable(right); + logger.addLoggable(left); + logger.addLoggable(right); } @Override diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java index 5c74c1e..0b97d1f 100644 --- a/src/main/java/frc/robot/Manipulation.java +++ b/src/main/java/frc/robot/Manipulation.java @@ -28,9 +28,9 @@ public class Manipulation implements Loggable { * @param indexLoadID The CAN id of the spark for the index loader * */ - Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID, int indexLoadID) { + Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID) {//, int indexLoadID) { this.intakeWheel = new CANSparkMax(intakeWheelID, MotorType.kBrushless); - this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); + // this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); this.intakePneumatics = new DoubleSolenoid(PneumaticsModuleType.REVPH, pneumaticsForwardChannel, pneumaticsReverseChannel); intakeWheel.setInverted(true); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2380a4d..007cec7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,7 +39,6 @@ public class Robot extends TimedRobot { DriveModule rightModule; Climber climber; - // LoggableGyro gyro; Shooter shooter; Manipulation manipulation; CANSparkMax tempClimber; @@ -49,19 +48,20 @@ public class Robot extends TimedRobot { LoggablePowerDistribution pdp; LoggableCompressor compressor; - // ClimberGates climberGates; + ClimberGates climberGates; LoggableGyro gyro; boolean drivetrainEnabled = true; - boolean tankDriveEnabled = false; boolean climberEnabled = false; + boolean tempClimberEnalbed = true; boolean manipulationEnabled = false; + boolean shooterEnabled = false; double shootSpeed = 0.40; // private JsonAutonomous auto; private static final double DEADBAND_LIMIT = 0.01; - private static final double SPEED_CAP = 0.45; + private static final double SPEED_CAP = 0.5; InputScaler joystickDeadband = new Deadband(DEADBAND_LIMIT); InputScaler joystickSquared = new SquaredInput(DEADBAND_LIMIT); BoostInput boost = new BoostInput(SPEED_CAP); @@ -91,49 +91,61 @@ public void robotInit() { if (this.climberEnabled) { System.out.println("Initializing climber..."); - // Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); - // Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); - // Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); - // Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); + Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); + Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); + Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); + Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); - // // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); - // climberGates = new ClimberGates(4, 5, 6, 7); - // climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - // climberSolenoidC, climberGates);// ,gyro, climberSensors); + // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); + climberGates = new ClimberGates(4, 5, 6, 7); + climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, + climberSolenoidC, climberGates);// ,gyro, climberSensors); - // // logger.addLoggable(climberSensors); - // logger.addLoggable(climberGates); - // logger.addLoggable(climber); + // logger.addLoggable(climberSensors); + logger.addLoggable(climberGates); + logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); } if (this.drivetrainEnabled) { System.out.println("Initializing drivetrain..."); - leftModule = new DriveModule("LeftDriveModule", 2, 3); // 2, 3 + leftModule = new DriveModule("LeftDriveModule", 2, 3); leftModule.setEncoder(2, 3, false); - rightModule = new DriveModule("RightDriveModule", 4, 5); // 4, 5 + rightModule = new DriveModule("RightDriveModule", 4, 5); rightModule.setEncoder(0, 1, true); - drive = new Drivetrain(leftModule, rightModule, 6); + drive = new Drivetrain(leftModule, rightModule, 6, gyro); - logger.addLoggable(drive); + // logger.addLoggable(drive); + logger.addLoggable(leftModule); + logger.addLoggable(rightModule); } else { System.out.println("Drivetrain initialization disabled."); } - // shooter = new Shooter(new TalonFX(6)); + if (shooterEnabled) { + System.out.println("Initializing shooter..."); + shooter = new Shooter(new TalonFX(6)); + } else { + System.out.println("Shooter initialization disabled."); + } if (manipulationEnabled) { System.out.println("Initializing manipulation..."); - manipulation = new Manipulation(0, 1, 7, 8); + manipulation = new Manipulation(0, 1, 7); } else { System.out.println("Manipulation initialization disabled."); } - tempClimber = new CANSparkMax(8, MotorType.kBrushless); - tempClimber.setIdleMode(IdleMode.kBrake); + if (tempClimberEnalbed) { + System.out.println("Initializing tempory climber..."); + tempClimber = new CANSparkMax(8, MotorType.kBrushless); + tempClimber.setIdleMode(IdleMode.kBrake); + } else { + System.out.println("Temporary climber initialization disabled."); + } System.out.print("Initializing compressor..."); compressor = new LoggableCompressor(PneumaticsModuleType.REVPH); @@ -142,13 +154,18 @@ public void robotInit() { logger.addLoggable(driver); logger.addLoggable(operator); logger.addLoggable(compressor); + logger.addLoggable(gyro); } @Override public void robotPeriodic() { // Robot code goes here - drive.update(); - // climber.update(); + if (drivetrainEnabled) { + drive.update(); + } + if (climberEnabled) { + climber.update(); + } } @Override @@ -192,71 +209,61 @@ public void teleopInit() { public void teleopPeriodic() { // Robot code goes here if (this.drivetrainEnabled) { - // if (tankDriveEnabled) { - // double leftInput = deadband(-driver.getLeftY()); - // double rightInput = deadband(-driver.getRightY()); - // drive.tankDrive(leftInput, rightInput); - // } else { double turnInput = deadband(driver.getRightX()) * -0.3; double speedInput = deadband(driver.getLeftY()); boost.setScale(driver.getRightTriggerAxis()); drive.arcadeDrive(turnInput, boost.scale(speedInput)); - // } - if (driver.getXButtonPressed()) { - tankDriveEnabled = !tankDriveEnabled; - } // if (driver.getAButtonPressed()) { // drive.setShifter(!drive.getShifter()); // } - if (driver.getAButtonPressed()) { - drive.setClimbMode(); + + drive.setClimbMode(driver.getAButton()); + if (driver.getBButtonPressed()) { + drive.toggleAutoBalance(); } leftModule.updateCurrent(); rightModule.updateCurrent(); } - // if (this.climberEnabled) { - // double climberInput = deadband(operator.getLeftY()); - // climber.setMotors(climberInput); - // double tempClimberInput = deadband(operator.getRightY()); - // tempClimber.set(tempClimberInput); - // if (operator.getLeftBumperPressed()) { - // climber.setClimbingState(climber.getNextClimbingState()); - // } - // // climber.checkClimbingState(operator.getAButtonPressed()); + if (this.climberEnabled) { + double climberInput = deadband(operator.getLeftY()); + climber.setMotors(climberInput); + double tempClimberInput = deadband(operator.getRightY()); + tempClimber.set(tempClimberInput); + if (operator.getLeftBumperPressed()) { + climber.setClimbingState(climber.getNextClimbingState()); + } - // // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro - // if (operator.getRightBumperPressed()) { - // climber.setMotorState( - // climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC - // : MotorStates.ACTIVE); - // } + if (operator.getRightBumperPressed()) { + climber.setMotorState( + climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC + : MotorStates.ACTIVE); + } - // if (operator.getBButtonPressed() && climber.getClimberStateId() != 0) { - // climber.setClimbingState(climber.getPreviousClimbingState()); - // } - // // TODO: Enable this when we're ready to test the climber - // } + if (operator.getBButtonPressed() && climber.getClimberStateId() != 0) { + climber.setClimbingState(climber.getPreviousClimbingState()); + } + } double tempClimberInput = deadband(operator.getRightY()); tempClimber.set(tempClimberInput); // System.out.println((climberGates.getB1() ? "B1: true" : "B1: false") + " " // + (climberGates.getC() ? "C: true" : "C: false")); - - // if (operator.getYButtonPressed()) { - // shootSpeed += 0.01; - // System.out.println(shootSpeed); - // } else if (operator.getAButtonPressed()) { - // shootSpeed -= 0.01; - // System.out.println(shootSpeed); - // } else if (operator.getBButtonPressed()) { - // shootSpeed = 0; - // } - - // shooter.setPower(operator.getRightTriggerAxis()); - // shooter.setPower(operator.getRightTriggerAxis() > 0.5 ? shootSpeed : 0); + if (shooterEnabled) { + // if (operator.getYButtonPressed()) { + // shootSpeed += 0.01; + // System.out.println(shootSpeed); + // } else if (operator.getAButtonPressed()) { + // shootSpeed -= 0.01; + // System.out.println(shootSpeed); + // } else if (operator.getBButtonPressed()) { + // shootSpeed = 0; + // } + // shooter.setPower(operator.getRightTriggerAxis()); + shooter.setPower(operator.getRightTriggerAxis() > 0.5 ? shootSpeed : 0); + } if (this.manipulationEnabled) { if (driver.getRightBumperPressed()) { @@ -265,7 +272,7 @@ public void teleopPeriodic() { manipulation.setIntakeExtend(false); } manipulation.setIntakeSpin(operator.getYButton()); - manipulation.setIndexLoad(operator.getLeftTriggerAxis() > 0.5); + // manipulation.setIndexLoad(operator.getLeftTriggerAxis() > 0.5); } logger.log(); From 4ced9664a6aa93cd39d6be12d987ab3f6e50c80d Mon Sep 17 00:00:00 2001 From: theblindbandet Date: Mon, 11 Apr 2022 19:47:11 -0400 Subject: [PATCH 62/66] Remove Shooter code --- src/main/deploy/autos/shooter-test.json | 17 ---------- src/main/java/frc/robot/Shooter.java | 41 ------------------------- 2 files changed, 58 deletions(-) delete mode 100644 src/main/deploy/autos/shooter-test.json delete mode 100644 src/main/java/frc/robot/Shooter.java diff --git a/src/main/deploy/autos/shooter-test.json b/src/main/deploy/autos/shooter-test.json deleted file mode 100644 index 18204e8..0000000 --- a/src/main/deploy/autos/shooter-test.json +++ /dev/null @@ -1,17 +0,0 @@ -{ - "auto": [ - { - "type": "shoot", - "unit": "SPEED", - "args": [ - 1000 - ] - }, - { - "type": "wait", - "args": [ - 5 - ] - } - ] -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java deleted file mode 100644 index 05ac672..0000000 --- a/src/main/java/frc/robot/Shooter.java +++ /dev/null @@ -1,41 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.TalonFX; - -import frc.robot.logging.Loggable; -import frc.robot.logging.Logger; - -public class Shooter implements Loggable { - TalonFX shooterMotor; - - public Shooter(TalonFX s) { - this.shooterMotor = s; - - this.shooterMotor.setNeutralMode(NeutralMode.Coast); - } - - public void setSpeed(double speed) { - this.shooterMotor.set(ControlMode.Velocity, speed); - } - - public void setPower(double power) { - this.shooterMotor.set(ControlMode.PercentOutput, power); - } - - public double getSpeed() { - return this.shooterMotor.getSelectedSensorVelocity(); - } - - @Override - public void setupLogging(Logger logger) { - logger.addAttribute("Shooter/speed"); - } - - @Override - public void log(Logger logger) { - logger.log("Shooter/speed", this.getSpeed()); - } -} From 6d5ed0adfa3943da5e5594c846c952a826a75939 Mon Sep 17 00:00:00 2001 From: theblindbandet Date: Mon, 11 Apr 2022 19:49:00 -0400 Subject: [PATCH 63/66] Remove Climber from Robot.java --- src/main/java/frc/robot/Robot.java | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 95258bd..921ffda 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,7 +39,6 @@ public class Robot extends TimedRobot { DriveModule rightModule; Climber climber; - Shooter shooter; Manipulation manipulation; CANSparkMax tempClimber; @@ -56,8 +55,6 @@ public class Robot extends TimedRobot { boolean climberEnabled = true; boolean tempClimberEnalbed = false; boolean manipulationEnabled = false; - boolean shooterEnabled = false; - double shootSpeed = 0.40; private static final double DEADBAND_LIMIT = 0.01; private static final double SPEED_CAP = 0.5; @@ -122,12 +119,6 @@ public void robotInit() { System.out.println("Drivetrain initialization disabled."); } - if (shooterEnabled) { - System.out.println("Initializing shooter..."); - shooter = new Shooter(new TalonFX(6)); - } else { - System.out.println("Shooter initialization disabled."); - } if (manipulationEnabled) { System.out.println("Initializing manipulation..."); @@ -171,7 +162,6 @@ public void autonomousInit() { timer.reset(); // auto = new JsonAutonomous("/home/lvuser/deploy/autos/autonomous.json", gyro, drive, - // shooter, // manipulation); // System.out.println("Auto Initialized"); // logger.addLoggable(auto); @@ -248,9 +238,6 @@ public void teleopPeriodic() { // System.out.println((climberGates.getB1() ? "B1: true" : "B1: false") + " " // + (climberGates.getC() ? "C: true" : "C: false")); - if (shooterEnabled) { - shooter.setPower(operator.getRightTriggerAxis() > 0.5 ? shootSpeed : 0); - } if (this.manipulationEnabled) { if (driver.getRightBumperPressed()) { From 3572b0f2e42e392c983f066244037f6f231ebc8a Mon Sep 17 00:00:00 2001 From: theblindbandet Date: Mon, 11 Apr 2022 21:08:08 -0400 Subject: [PATCH 64/66] Start implimenting reworked climber execution plan --- src/main/java/frc/robot/Climber.java | 56 +++++++++++++++------ src/main/java/frc/robot/ClimberSensors.java | 18 +------ src/main/java/frc/robot/Robot.java | 24 ++++----- 3 files changed, 56 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index 3079208..eacd9de 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -3,6 +3,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.wpilibj.Solenoid; @@ -16,6 +17,7 @@ public class Climber implements Loggable { enum ClimbingStates { RESTING(0, "Default resting"), // PRE_STAGE(5, "Rotate climber and set pre-stage pin position (button)"), // + PRE_STAGE_MANUAL(6, "Pre-stage w/ manual control override"), // TOUCH_A(10, "Pin A (button/sensor)"), // ROTATE_B(15, "Rotate to B bar (photogate)"), // TOUCH_AB(20, "Pin B (high current/sensor)"), // @@ -49,6 +51,8 @@ enum MotorStates { public static double NEXT_BC_STATE_CURRENT = 45.0; public static int FILTER_FRAME_RANGE = 10; + public static double ENCODER_DEADZONE = 100; + public static double TOUCH_A_POSITION = 144000; // TBD public static double SWING_AB_POSITION = 43000; // TBD public static double SWING_B_POSITION = 900; // TBD @@ -57,6 +61,8 @@ enum MotorStates { private double motorSpeed; + private double previousTime; + TalonFX climbingMotor; TalonFX secondaryClimbingMotor; @@ -77,10 +83,8 @@ enum MotorStates { LoggableFirstOrderFilter rightFilter; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC // ) { - , ClimberGates gates) { - // , LoggableGyro gyro) { - // , ClimberSensors touch) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, + ClimberSensors touch) { this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -90,8 +94,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climberSolenoidB2 = climberSolenoidB2; this.climberSolenoidC = climberSolenoidC; - // this.touch = touch; - this.gates = gates; + this.touch = touch; this.climbingMotor.setNeutralMode(NeutralMode.Coast); this.secondaryClimbingMotor.setNeutralMode(NeutralMode.Coast); @@ -148,12 +151,21 @@ public void update() { climberSolenoidB1.set(true); climberSolenoidB2.set(false); climberSolenoidC.set(true); - // TODO: set motor target here - setSpeed(motorSpeed); + climbingMotor.set(ControlMode.Position, TOUCH_A_POSITION); + + if (Math.abs(climbingMotor.getSelectedSensorPosition() + - TOUCH_A_POSITION) < ENCODER_DEADZONE) { + setClimbingState(ClimbingStates.PRE_STAGE_MANUAL); + } + break; + // 06 PRE_STAGE_MANUAL: Manual control of climber position + case PRE_STAGE_MANUAL: + currentMotorState = MotorStates.ACTIVE; break; // 10 TOUCH_A: Pin A (button/sensor) case TOUCH_A: + currentMotorState = MotorStates.STATIC; setSpeed(0); climberSolenoidA.set(false); climberSolenoidB1.set(true); @@ -179,6 +191,11 @@ public void update() { // setClimbingState(ClimbingStates.TOUCH_AB); // this.setSpeed(0); // } + + if (touch.getB()) { + setClimbingState(ClimbingStates.TOUCH_AB); + previousTime = timer.get(); + } break; // 20 TOUCH_AB: Pin B (high current/sensor) @@ -188,9 +205,12 @@ public void update() { climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); - // if (gates.getB1()) { - // setClimbingState(ClimbingStates.ROTATE_AB_DOWN); - // } + if (!gates.getB1()) { + setClimbingState(ClimbingStates.ROTATE_B); + } + if (timer.get() - previousTime > 0.5) { + setClimbingState(ClimbingStates.ROTATE_AB_DOWN); + } break; // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) @@ -212,7 +232,7 @@ public void update() { climberSolenoidC.set(true); // TODO: set motor target here // if (!gates.getA()) { - setClimbingState(ClimbingStates.ROTATE_B_DOWN);; + setClimbingState(ClimbingStates.ROTATE_B_DOWN); // } break; @@ -222,8 +242,8 @@ public void update() { this.setPower(0); if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 1000 && Math.abs( - climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {// Determine - // tolerance + climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) { + // Determine tolerance System.out.println("DONE SWINGING!"); // setClimbingState(ClimbingStates.ROTATE_C); } @@ -437,6 +457,14 @@ public void checkMotorState() { } } + /** + * Checks whether or not the climber is climbing. + * @return true if the climber is climbing + */ + public boolean isClimbing() { + return this.currentClimberState != ClimbingStates.RESTING; + } + @Override public void setupLogging(Logger logger) { this.timer.setupLogging(logger); diff --git a/src/main/java/frc/robot/ClimberSensors.java b/src/main/java/frc/robot/ClimberSensors.java index 00f085d..3ad47d8 100644 --- a/src/main/java/frc/robot/ClimberSensors.java +++ b/src/main/java/frc/robot/ClimberSensors.java @@ -5,34 +5,20 @@ import frc.robot.logging.Logger; public class ClimberSensors implements Loggable { - DigitalInput a1, a2, b1, b2, c1, c2; + DigitalInput b1, b2; - public ClimberSensors(int a1ID, int a2ID, int b1ID, int b2ID, int c1ID, int c2ID) { - this.a1 = new DigitalInput(a1ID); - this.a2 = new DigitalInput(a2ID); + public ClimberSensors(int b1ID, int b2ID) { this.b1 = new DigitalInput(b1ID); this.b2 = new DigitalInput(b2ID); - this.c1 = new DigitalInput(c1ID); - this.c2 = new DigitalInput(c2ID); - } - - public boolean getA() { - return this.a1.get() && this.a2.get(); } public boolean getB() { return this.b1.get() && this.b2.get(); } - public boolean getC() { - return this.c1.get() && this.c2.get(); - } - @Override public void setupLogging(Logger logger) { - logger.log("TouchA", this.getA()); logger.log("TouchB", this.getB()); - logger.log("TouchC", this.getC()); } @Override diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 921ffda..424da9b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,12 +48,12 @@ public class Robot extends TimedRobot { LoggablePowerDistribution pdp; LoggableCompressor compressor; - ClimberGates climberGates; + ClimberSensors climberSensors; LoggableGyro gyro; boolean drivetrainEnabled = true; boolean climberEnabled = true; - boolean tempClimberEnalbed = false; + boolean tempClimberEnabled = false; boolean manipulationEnabled = false; private static final double DEADBAND_LIMIT = 0.01; @@ -92,13 +92,11 @@ public void robotInit() { Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); - // ClimberSensors climberSensors = new ClimberSensors(0, 1, 2, 3, 4, 5); - ClimberGates climberGates = new ClimberGates(6, 7, 8, 9, 10, 11, 12, 13); + climberSensors = new ClimberSensors(0,0); // TODO: Add sensors and input ids climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC, climberGates);// ,gyro, climberSensors); + climberSolenoidC, climberSensors); - // logger.addLoggable(climberSensors); - logger.addLoggable(climberGates); + logger.addLoggable(climberSensors); logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); @@ -127,8 +125,8 @@ public void robotInit() { System.out.println("Manipulation initialization disabled."); } - if (tempClimberEnalbed) { - System.out.println("Initializing tempory climber..."); + if (tempClimberEnabled) { + System.out.println("Initializing temporary climber..."); tempClimber = new CANSparkMax(8, MotorType.kBrushless); tempClimber.setIdleMode(IdleMode.kBrake); } else { @@ -219,7 +217,6 @@ public void teleopPeriodic() { if (operator.getLeftBumperPressed()) { climber.setClimbingState(climber.getNextClimbingState()); } - // climber.checkClimbingState(operator.getAButtonPressed()); // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro if (operator.getRightBumperPressed()) { @@ -233,8 +230,11 @@ public void teleopPeriodic() { } } - double tempClimberInput = deadband(operator.getRightY()); - tempClimber.set(tempClimberInput); + if (tempClimberEnabled) { + double tempClimberInput = deadband(operator.getRightY()); + tempClimber.set(tempClimberInput); + } + // System.out.println((climberGates.getB1() ? "B1: true" : "B1: false") + " " // + (climberGates.getC() ? "C: true" : "C: false")); From c8faf9a6e966820471b4a99918528bb18ee330c6 Mon Sep 17 00:00:00 2001 From: theblindbandet Date: Tue, 12 Apr 2022 18:37:29 -0400 Subject: [PATCH 65/66] Fix left and right loggables erroring Maybe nestable loggables will be a thing someday. Doubt it though, sendables exist now --- src/main/java/frc/robot/Drivetrain.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index 291d69a..be569a0 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -162,12 +162,13 @@ public void checkGears() { @Override public void setupLogging(Logger logger) { - logger.addLoggable(left); - logger.addLoggable(right); + left.setupLogging(logger); + right.setupLogging(logger); } @Override public void log(Logger logger) { - // TODO Auto-generated method stub + left.log(logger); + right.log(logger); } } From 740aae1f6381294b4f2312696f3556443a6da038 Mon Sep 17 00:00:00 2001 From: Programmer Date: Mon, 18 Apr 2022 15:19:40 -0400 Subject: [PATCH 66/66] Push state comp code --- src/main/java/frc/robot/Climber.java | 157 +++++++++++----------- src/main/java/frc/robot/DriveModule.java | 2 +- src/main/java/frc/robot/Drivetrain.java | 27 ++-- src/main/java/frc/robot/Manipulation.java | 47 +++++-- src/main/java/frc/robot/Robot.java | 97 +++++++------ 5 files changed, 190 insertions(+), 140 deletions(-) diff --git a/src/main/java/frc/robot/Climber.java b/src/main/java/frc/robot/Climber.java index eacd9de..1fb05ff 100644 --- a/src/main/java/frc/robot/Climber.java +++ b/src/main/java/frc/robot/Climber.java @@ -17,12 +17,11 @@ public class Climber implements Loggable { enum ClimbingStates { RESTING(0, "Default resting"), // PRE_STAGE(5, "Rotate climber and set pre-stage pin position (button)"), // - PRE_STAGE_MANUAL(6, "Pre-stage w/ manual control override"), // TOUCH_A(10, "Pin A (button/sensor)"), // ROTATE_B(15, "Rotate to B bar (photogate)"), // TOUCH_AB(20, "Pin B (high current/sensor)"), // ROTATE_AB_DOWN(25, "Rotate down to plumb (photogate)"), // - RELEASE_A(30, "Unpin A (gyro/accel)"), // + // RELEASE_A(30, "Unpin A (gyro/accel)"), // ROTATE_B_DOWN(35, "Wait for swinging (photogate)"), // ROTATE_C(40, "Rotate to C bar (gyro/accel)"), // TOUCH_BC(50, "Pin C (high current/sensor)"), // @@ -83,8 +82,7 @@ enum MotorStates { LoggableFirstOrderFilter rightFilter; public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climberSolenoidA, - Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC, - ClimberSensors touch) { + Solenoid climberSolenoidB1, Solenoid climberSolenoidB2, Solenoid climberSolenoidC) { this.climbingMotor = new TalonFX(climbingMotorID); this.secondaryClimbingMotor = new TalonFX(secondaryClimbingMotorID); @@ -94,7 +92,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climberSolenoidB2 = climberSolenoidB2; this.climberSolenoidC = climberSolenoidC; - this.touch = touch; + // this.touch = touch; this.climbingMotor.setNeutralMode(NeutralMode.Coast); this.secondaryClimbingMotor.setNeutralMode(NeutralMode.Coast); @@ -106,7 +104,7 @@ public Climber(int climbingMotorID, int secondaryClimbingMotorID, Solenoid climb this.climbingMotor.config_kI(0, 0.001); this.climbingMotor.config_kD(0, 5); - climbingMotor.setInverted(InvertType.InvertMotorOutput); + secondaryClimbingMotor.setInverted(InvertType.InvertMotorOutput); secondaryClimbingMotor.follow(climbingMotor); this.timer = new LoggableTimer("Climber/Time"); @@ -121,25 +119,25 @@ public void update() { this.leftFilter.update(climbingMotor.getStatorCurrent()); this.rightFilter.update(secondaryClimbingMotor.getStatorCurrent()); - // Make sure we're not pulling too much current instantly - if (climbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT) { - setClimbingState(ClimbingStates.ERROR); - System.out.println("--------HIT MAX CURRENT--------"); - } + // // Make sure we're not pulling too much current instantly + // if (climbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT + // || secondaryClimbingMotor.getStatorCurrent() > MAX_INSTANT_CURRENT) { + // setClimbingState(ClimbingStates.ERROR); + // System.out.println("--------HIT MAX CURRENT--------"); + // } - // Make sure we're not pulling too much current over time - if (leftFilter.get() > MAX_AVERAGE_CURRENT || rightFilter.get() > MAX_AVERAGE_CURRENT) { - System.out.println("--------HIT MAX AVERAGE CURRENT--------"); - setClimbingState(ClimbingStates.ERROR); - } + // // Make sure we're not pulling too much current over time + // if (leftFilter.get() > MAX_AVERAGE_CURRENT || rightFilter.get() > MAX_AVERAGE_CURRENT) { + // System.out.println("--------HIT MAX AVERAGE CURRENT--------"); + // setClimbingState(ClimbingStates.ERROR); + // } switch (this.currentClimberState) { // 00 RESTING: Default resting case RESTING: climberSolenoidA.set(false); - climberSolenoidB1.set(false); - climberSolenoidB2.set(false); + climberSolenoidB1.set(true); + climberSolenoidB2.set(true); climberSolenoidC.set(false); break; @@ -151,22 +149,13 @@ public void update() { climberSolenoidB1.set(true); climberSolenoidB2.set(false); climberSolenoidC.set(true); - climbingMotor.set(ControlMode.Position, TOUCH_A_POSITION); - - if (Math.abs(climbingMotor.getSelectedSensorPosition() - - TOUCH_A_POSITION) < ENCODER_DEADZONE) { - setClimbingState(ClimbingStates.PRE_STAGE_MANUAL); - } - break; - // 06 PRE_STAGE_MANUAL: Manual control of climber position - case PRE_STAGE_MANUAL: - currentMotorState = MotorStates.ACTIVE; + // climbingMotor.set(ControlMode.Position, TOUCH_A_POSITION); break; // 10 TOUCH_A: Pin A (button/sensor) case TOUCH_A: - currentMotorState = MotorStates.STATIC; - setSpeed(0); + setMotorState(MotorStates.ACTIVE); + this.setMotors(0); climberSolenoidA.set(false); climberSolenoidB1.set(true); climberSolenoidB2.set(false); @@ -181,7 +170,7 @@ public void update() { // 15 ROTATE_B: Rotate to B bar (photogate) case ROTATE_B: // TODO: set motor power here - this.setSpeed(-0.4); + this.setMotors(-0.4); climberSolenoidA.set(false); climberSolenoidB1.set(true); climberSolenoidB2.set(false); @@ -192,54 +181,61 @@ public void update() { // this.setSpeed(0); // } - if (touch.getB()) { - setClimbingState(ClimbingStates.TOUCH_AB); - previousTime = timer.get(); - } + // if (touch.getB()) { + // setClimbingState(ClimbingStates.TOUCH_AB); + // previousTime = timer.get(); + // } break; // 20 TOUCH_AB: Pin B (high current/sensor) case TOUCH_AB: - this.setSpeed(-0.05); + this.setMotors(-0.05); climberSolenoidA.set(false); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); - if (!gates.getB1()) { - setClimbingState(ClimbingStates.ROTATE_B); - } - if (timer.get() - previousTime > 0.5) { - setClimbingState(ClimbingStates.ROTATE_AB_DOWN); - } + // if (!gates.getB1()) { + // setClimbingState(ClimbingStates.ROTATE_B); + // } + // if (timer.get() - previousTime > 0.5) { + // setClimbingState(ClimbingStates.ROTATE_AB_DOWN); + // } break; // 25 ROTATE_AB_DOWN: Rotate down to plumb (photogate) case ROTATE_AB_DOWN: - this.setPower(0); - // TODO: set motor target here - if (climbingMotor.getSelectedSensorPosition() > SWING_AB_POSITION) {// Was less then when broken + // this.setPower(0); + climberSolenoidA.set(false); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + if (climbingMotor.getSelectedSensorPosition() > SWING_AB_POSITION) {// Was less then + // when broken System.out.println("DONE SWINGING!"); - setClimbingState(ClimbingStates.RELEASE_A); + // setClimbingState(ClimbingStates.RELEASE_A); } break; - // 30 RELEASE_A: Unpin A (gyro/accel) - case RELEASE_A: - this.setPower(0); + // // 30 RELEASE_A: Unpin A (gyro/accel) + // case RELEASE_A: + // climberSolenoidA.set(true); + // climberSolenoidB1.set(false); + // climberSolenoidB2.set(false); + // climberSolenoidC.set(true); + // // this.setPower(0); + // // TODO: set motor target here + // // if (!gates.getA()) { + // setClimbingState(ClimbingStates.ROTATE_B_DOWN); + // // } + // break; + + // 35 ROTATE_B_DOWN: Wait for swinging (photogate) + case ROTATE_B_DOWN: climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); climberSolenoidC.set(true); - // TODO: set motor target here - // if (!gates.getA()) { - setClimbingState(ClimbingStates.ROTATE_B_DOWN); - // } - break; - - // 35 ROTATE_B_DOWN: Wait for swinging (photogate) - case ROTATE_B_DOWN: - // TODO: set motor target here - this.setPower(0); + // this.setPower(0); if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 1000 && Math.abs( climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) { @@ -251,18 +247,21 @@ public void update() { // 40 ROTATE_C: Rotate to C bar (gyro/accel) case ROTATE_C: - // TODO: set motor target here - this.setSpeed(-0.3); - if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT - || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { - setClimbingState(ClimbingStates.TOUCH_BC); - this.setSpeed(0); - } + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(true); + this.setMotors(-0.3); + // if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT + // || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) { + // setClimbingState(ClimbingStates.TOUCH_BC); + // this.setSpeed(0); + // } break; // 50 TOUCH_BC: Pin C (high current/sensor) case TOUCH_BC: - this.setSpeed(-0.05); + this.setMotors(-0.05); climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(false); @@ -275,9 +274,12 @@ public void update() { // 55 ROTATE_BC_DOWN: Rotate down to plumb (photogate) case ROTATE_BC_DOWN: - // TODO: set motor target here - this.setPower(0); - if (climbingMotor.getSelectedSensorPosition() < SWING_BC_POSITION) {// Determine tolerance + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(false); + climberSolenoidC.set(false); + if (climbingMotor.getSelectedSensorPosition() < SWING_BC_POSITION) {// Determine + // tolerance System.out.println("DONE SWINGING!"); // setClimbingState(ClimbingStates.RELEASE_B); } @@ -285,7 +287,7 @@ public void update() { // 60 RELEASE_B: Unpin B (gyro/accel) case RELEASE_B: - this.setPower(0); + // this.setPower(0); climberSolenoidA.set(true); climberSolenoidB1.set(false); climberSolenoidB2.set(true); @@ -297,8 +299,11 @@ public void update() { // 65 ROTATE_C_DOWN: Wait for swinging () case ROTATE_C_DOWN: - // TODO: set motor target here - this.setPower(0); + climberSolenoidA.set(true); + climberSolenoidB1.set(false); + climberSolenoidB2.set(true); + climberSolenoidC.set(false); + // this.setPower(0); break; // 70 DONE: Climbing is done @@ -320,6 +325,7 @@ public void update() { } public void setClimbingState(ClimbingStates climbingState) { + setMotorState(MotorStates.STATIC); this.currentClimberState = climbingState; } @@ -421,6 +427,7 @@ public MotorStates getMotorState() { public void setMotors(double value) { checkMotorState(); + motorSpeed = value; if (value != 0) { setMotorState(MotorStates.ACTIVE); } @@ -430,7 +437,6 @@ public void setMotors(double value) { break; case ACTIVE: - motorSpeed = value; setSpeed(motorSpeed); break; @@ -459,6 +465,7 @@ public void checkMotorState() { /** * Checks whether or not the climber is climbing. + * * @return true if the climber is climbing */ public boolean isClimbing() { diff --git a/src/main/java/frc/robot/DriveModule.java b/src/main/java/frc/robot/DriveModule.java index 3872dea..4fccfd2 100644 --- a/src/main/java/frc/robot/DriveModule.java +++ b/src/main/java/frc/robot/DriveModule.java @@ -46,7 +46,7 @@ public class DriveModule implements Loggable { main.config_kD(0, 0); main.configMotionCruiseVelocity(MAX_VELOCITY / 2); main.configMotionAcceleration(MAX_VELOCITY / 2); - main.configClosedloopRamp(0.3); + main.configClosedloopRamp(0.4); indexCurrent = 0; } diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java index 291d69a..dc5c5e0 100644 --- a/src/main/java/frc/robot/Drivetrain.java +++ b/src/main/java/frc/robot/Drivetrain.java @@ -11,8 +11,8 @@ public class Drivetrain implements Loggable { private final double SHIFT_CURRENT_HIGH = 80; // TODO Get actual values when we test drivetrain private final double SHIFT_CURRENT_LOW = 0; private final double SHIFT_VELOCITY = 0; // Wheel velocity - private final double OFF_BALANCE_THRESHOLD = 10; - private final double ON_BALANCE_THRESHOLD = 5; + private final double OFF_BALANCE_THRESHOLD = 15; + private final double ON_BALANCE_THRESHOLD = 7; private boolean climbMode; private boolean autoBalanceMode; @@ -44,7 +44,7 @@ public class Drivetrain implements Loggable { climbMode = false; autoBalanceMode = false; - autoBalanceEnabled = true; + autoBalanceEnabled = false; } public void update() { @@ -79,8 +79,8 @@ public void toggleAutoBalance() { * @param rightSpeed The speed of the right motors */ public void drive(double leftSpeed, double rightSpeed) { // Probably implement deadbands later - left.set(leftSpeed); - right.set(rightSpeed); + left.setSpeed(leftSpeed); + right.setSpeed(rightSpeed); } public void setClimbMode(boolean climb) { @@ -94,8 +94,11 @@ public void setClimbMode(boolean climb) { * @param speedInput The speed to drive */ public void arcadeDrive(double turnInput, double speedInput) { + if (!getShifter()) { + turnInput *= 1.45; + } speedInput = - climbMode ? speedInput * 0.3 : speedInput + (autoBalanceEnabled ? balanceScale : 0); + climbMode ? speedInput * 0.4 : speedInput + (autoBalanceEnabled ? balanceScale : 0); this.drive(speedInput - turnInput, speedInput + turnInput); } @@ -112,16 +115,16 @@ public void tankDrive(double leftDrive, double rightDrive) { /** * Changes gears for the drivetrain. * - * @param lowSpeed true if in low speed gearing, false if in high speed gearing + * @param highSpeed true if in high speed gearing, false if in low speed gearing */ - public void setShifter(boolean lowSpeed) { - shifter.set(lowSpeed); + public void setShifter(boolean highSpeed) { + shifter.set(highSpeed); } /** * Gets if the gear shift is engaged. * - * @return true if in low gear, false if in high gear + * @return true if in high gear, false if in low gear */ public boolean getShifter() { return shifter.get(); @@ -162,8 +165,8 @@ public void checkGears() { @Override public void setupLogging(Logger logger) { - logger.addLoggable(left); - logger.addLoggable(right); + // logger.addLoggable(left); + // logger.addLoggable(right); } @Override diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java index 0b97d1f..d3bf7b2 100644 --- a/src/main/java/frc/robot/Manipulation.java +++ b/src/main/java/frc/robot/Manipulation.java @@ -26,12 +26,13 @@ public class Manipulation implements Loggable { * @param pneumaticsReverseChannel The Solenoid id for the reverse channel for the intake * @param intakeWheelID The CAN id of the spark for the intake * @param indexLoadID The CAN id of the spark for the index loader - * */ - Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID) {//, int indexLoadID) { + Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID, + int indexLoadID) { this.intakeWheel = new CANSparkMax(intakeWheelID, MotorType.kBrushless); - // this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); - this.intakePneumatics = new DoubleSolenoid(PneumaticsModuleType.REVPH, pneumaticsForwardChannel, pneumaticsReverseChannel); + this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); + this.intakePneumatics = new DoubleSolenoid(PneumaticsModuleType.REVPH, + pneumaticsForwardChannel, pneumaticsReverseChannel); intakeWheel.setInverted(true); } @@ -40,10 +41,9 @@ public class Manipulation implements Loggable { * Spins the intake motor * * @param spin True if the motor should spin; false if not - * */ public void setIntakeSpin(boolean spin) { - this.speed = spin ? 0.3 : 0.0; + this.speed = spin ? 0.5 : 0.0; intakeWheel.set(speed); this.spinning = spin; } @@ -52,19 +52,48 @@ public void setIntakeSpin(boolean spin) { * Moves the intake system * * @param extend True if the pneumatics should extend; false if not - * */ public void setIntakeExtend(boolean extend) { intakePneumatics.set(extend ? Value.kForward : Value.kReverse); } + /** * Moves power cells down indexing system * * @param load True if it should load; false if not - * */ public void setIndexLoad(boolean load) { - indexLoad.set(load ? 0.5 : 0.0); + indexLoad.set(load ? -0.2 : 0.0); + } + + public void setCollect(boolean collect) { + if (intakePneumatics.get() == Value.kForward && collect) { + indexLoad.set(-0.3); + intakeWheel.set(0.6); + } else { + indexLoad.set(0); + intakeWheel.set(0); + } + } + + public void setSlowEject() { + if (intakePneumatics.get() == Value.kForward) { + indexLoad.set(0.4); + intakeWheel.set(-0.35); + } else { + indexLoad.set(0); + intakeWheel.set(0); + } + } + + public void setEject() { + if (intakePneumatics.get() == Value.kForward) { + indexLoad.set(0.4); + intakeWheel.set(-1); + } else { + indexLoad.set(0); + intakeWheel.set(0); + } } @Override diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 424da9b..dd308bd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -54,7 +54,7 @@ public class Robot extends TimedRobot { boolean drivetrainEnabled = true; boolean climberEnabled = true; boolean tempClimberEnabled = false; - boolean manipulationEnabled = false; + boolean manipulationEnabled = true; private static final double DEADBAND_LIMIT = 0.01; private static final double SPEED_CAP = 0.5; @@ -87,16 +87,16 @@ public void robotInit() { if (this.climberEnabled) { System.out.println("Initializing climber..."); - Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 2); - Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 3); - Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 4); - Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 5); + Solenoid climberSolenoidA = new Solenoid(PneumaticsModuleType.REVPH, 1); + Solenoid climberSolenoidB1 = new Solenoid(PneumaticsModuleType.REVPH, 2); + Solenoid climberSolenoidB2 = new Solenoid(PneumaticsModuleType.REVPH, 3); + Solenoid climberSolenoidC = new Solenoid(PneumaticsModuleType.REVPH, 4); - climberSensors = new ClimberSensors(0,0); // TODO: Add sensors and input ids - climber = new Climber(9, 10, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, - climberSolenoidC, climberSensors); + // climberSensors = new ClimberSensors(0, 0); // TODO: Add sensors and input ids + climber = new Climber(10, 9, climberSolenoidA, climberSolenoidB1, climberSolenoidB2, + climberSolenoidC); - logger.addLoggable(climberSensors); + // logger.addLoggable(climberSensors); logger.addLoggable(climber); } else { System.out.println("Climber initialization disabled."); @@ -104,23 +104,22 @@ public void robotInit() { if (this.drivetrainEnabled) { System.out.println("Initializing drivetrain..."); - leftModule = new DriveModule("LeftDriveModule", 2, 3); + leftModule = new DriveModule("LeftDriveModule", 3, 2); leftModule.setEncoder(2, 3, false); rightModule = new DriveModule("RightDriveModule", 4, 5); rightModule.setEncoder(0, 1, true); - drive = new Drivetrain(leftModule, rightModule, 6, gyro); + drive = new Drivetrain(leftModule, rightModule, 0, gyro); logger.addLoggable(drive); } else { System.out.println("Drivetrain initialization disabled."); } - if (manipulationEnabled) { System.out.println("Initializing manipulation..."); - manipulation = new Manipulation(0, 1, 7); + manipulation = new Manipulation(5, 6, 7, 8); } else { System.out.println("Manipulation initialization disabled."); } @@ -170,14 +169,29 @@ public void autonomousInit() { public void autonomousPeriodic() { // Robot code goes here - if (timer.get() < 2) { - drive.tankDrive(-0.3, -0.3); - } else { - drive.tankDrive(0, 0); + if (drivetrainEnabled) { + if (timer.get() < 3) { + drive.tankDrive(-0.3, -0.3); + } else { + drive.tankDrive(0, 0); + } + + leftModule.updateCurrent(); + rightModule.updateCurrent(); + } + + if (manipulationEnabled) { + if (timer.get() < 0.5) { + manipulation.setIntakeExtend(true); + } else if (timer.get() < 3) { + manipulation.setIntakeExtend(true); + manipulation.setSlowEject(); + } else { + manipulation.setIntakeExtend(false); + manipulation.setCollect(false); + } } - leftModule.updateCurrent(); - rightModule.updateCurrent(); System.out.println("running - Robot"); // auto.run(); @@ -195,24 +209,23 @@ public void teleopPeriodic() { // Robot code goes here if (this.drivetrainEnabled) { double turnInput = deadband(driver.getRightX()) * -0.3; - double speedInput = deadband(driver.getLeftY()); + double speedInput = deadband(-driver.getLeftY()); boost.setScale(driver.getRightTriggerAxis()); drive.arcadeDrive(turnInput, boost.scale(speedInput)); - // if (driver.getAButtonPressed()) { - // drive.setShifter(!drive.getShifter()); - // } + + drive.setShifter(driver.getLeftTriggerAxis() < 0.5); drive.setClimbMode(driver.getAButton()); - if (driver.getBButtonPressed()) { - drive.toggleAutoBalance(); - } + // if (driver.getBButtonPressed()) { + // drive.toggleAutoBalance(); + // } leftModule.updateCurrent(); rightModule.updateCurrent(); } if (this.climberEnabled) { - double climberInput = deadband(operator.getLeftY()); + double climberInput = deadband(operator.getLeftY() * 0.8); climber.setMotors(climberInput); if (operator.getLeftBumperPressed()) { climber.setClimbingState(climber.getNextClimbingState()); @@ -220,9 +233,7 @@ public void teleopPeriodic() { // TODO: Create a way for motors to go 'limp' when needed, reading loggableGyro if (operator.getRightBumperPressed()) { - climber.setMotorState( - climber.getMotorState() == MotorStates.ACTIVE ? MotorStates.STATIC - : MotorStates.ACTIVE); + climber.setMotorState(MotorStates.STATIC); } if (operator.getBButtonPressed() && climber.getClimberStateId() != 0) { @@ -235,18 +246,18 @@ public void teleopPeriodic() { tempClimber.set(tempClimberInput); } - - // System.out.println((climberGates.getB1() ? "B1: true" : "B1: false") + " " - // + (climberGates.getC() ? "C: true" : "C: false")); - if (this.manipulationEnabled) { - if (driver.getRightBumperPressed()) { - manipulation.setIntakeExtend(true); - } else if (driver.getLeftBumperPressed()) { - manipulation.setIntakeExtend(false); + manipulation.setIntakeExtend(driver.getLeftBumper()); + + if (operator.getYButton()) { + manipulation.setCollect(true); + } else if (operator.getXButton()) { + manipulation.setSlowEject(); + } else if (operator.getAButton()) { + manipulation.setEject(); + } else { + manipulation.setCollect(false); } - manipulation.setIntakeSpin(operator.getYButton()); - // manipulation.setIndexLoad(operator.getLeftTriggerAxis() > 0.5); } logger.log(); @@ -274,9 +285,9 @@ public void testInit() { public void testPeriodic() { // Robot code goes here // if (climberEnabled) { - // climber.setPrestage(operator.getXButtonPressed()); - // climber.setPower(operator.getRightY()); // Deadband - // climber.checkClimbingState(); + // climber.setPrestage(operator.getXButtonPressed()); + // climber.setPower(operator.getRightY()); // Deadband + // climber.checkClimbingState(); // } logger.log();