From eaef198defd78cf5d79ee61b82feef58ba094d4f Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Tue, 18 Feb 2020 12:28:11 -0800 Subject: [PATCH 01/21] add motor data --- .../team4159/frc/robot/RobotContainer.java | 4 ---- .../lib/hardware/motors/MotorData.java | 19 +++++++++++++++++++ .../team4159/lib/hardware/motors/Motors.java | 19 +++++++++++++++++++ 3 files changed, 38 insertions(+), 4 deletions(-) create mode 100644 src/main/java/org/team4159/lib/hardware/motors/MotorData.java create mode 100644 src/main/java/org/team4159/lib/hardware/motors/Motors.java diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index f7daef3..cca2b14 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -40,10 +40,6 @@ private void configureCamera() { } private void configureButtonBindings() { - new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.BUTTON_IDS.ENABLE_SHOOTER) - .whenPressed(() -> shooter.setTargetSpeed(SmartDashboard.getNumber("target_shooter_speed", 0))) - .whenReleased(new InstantCommand(shooter::stop, shooter)); - new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_ALL_INTAKE_SUBSYSTEMS) .whenPressed(new ParallelCommandGroup( new InstantCommand(intake::intake, intake), diff --git a/src/main/java/org/team4159/lib/hardware/motors/MotorData.java b/src/main/java/org/team4159/lib/hardware/motors/MotorData.java new file mode 100644 index 0000000..1c7f69d --- /dev/null +++ b/src/main/java/org/team4159/lib/hardware/motors/MotorData.java @@ -0,0 +1,19 @@ +package org.team4159.lib.hardware.motors; + +public class MotorData { + public String name; + public double free_speed, + free_current, + max_power, + stall_torque, + stall_current; + public MotorData(String name, double free_speed, double free_current, + double max_power, double stall_torque, double stall_current) { + this.name = name; + this.free_speed = free_speed; + this.free_current = free_current; + this.max_power = max_power; + this.stall_torque = stall_torque; + this.stall_current = stall_current; + } +} diff --git a/src/main/java/org/team4159/lib/hardware/motors/Motors.java b/src/main/java/org/team4159/lib/hardware/motors/Motors.java new file mode 100644 index 0000000..0f28fa3 --- /dev/null +++ b/src/main/java/org/team4159/lib/hardware/motors/Motors.java @@ -0,0 +1,19 @@ +package org.team4159.lib.hardware.motors; + +public class Motors { + // From https://motors.vex.com/ + public static final MotorData Falcon_500 = new MotorData("Falcon 500", 6380, 1.5, 783, 4.69, 257); + public static final MotorData NEO = new MotorData("NEO", 5880, 1.3, 516, 3.36, 166); + public static final MotorData CIM = new MotorData("CIM", 5330, 2.7, 337, 2.41, 131); + public static final MotorData MiniCIM = new MotorData("Mini CIM", 5840, 3, 215, 1.41, 89); + public static final MotorData BAG = new MotorData("BAG", 13180, 1.8, 149, 0.43, 53); + public static final MotorData _775pro = new MotorData("775pro", 18730, 0.7, 347, 0.71, 134); + public static final MotorData AndyMark_9015 = new MotorData("AndyMark 9015", 14270, 3.7, 134, 0.36, 71); + public static final MotorData AndyMark_NeveRest = new MotorData("AndyMark NeveRest", 5480, 0.4, 25, 0.17, 10); + public static final MotorData AndyMark_RS775_125 = new MotorData("AndyMark RS775-125", 5800, 1.6, 43, 0.28, 18); + public static final MotorData BaneBots_RS775_18V = new MotorData("BaneBots RS-775 18V", 13050, 2.7, 246, 0.72, 97); + public static final MotorData BaneBots_RS_550 = new MotorData("BaneBots RS-550", 19000, 0.4, 190, 0.38, 84); + + // From http://www.revrobotics.com/content/docs/REV-21-1651-DS.pdf + public static final MotorData NEO_550 = new MotorData("NEO 550", 1100, 1.4, 279, 0.97, 100); +} From e38397b2bac609baa1076f9240fab5d2976c0834 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Tue, 18 Feb 2020 17:14:39 -0800 Subject: [PATCH 02/21] change TrajectoryController to not return from update --- .../team4159/frc/robot/controllers/TrajectoryController.java | 4 ++-- .../java/org/team4159/frc/robot/subsystems/Drivetrain.java | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java index e9cc30e..4af20d8 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java @@ -40,7 +40,7 @@ public TrajectoryController(Drivetrain drivetrain) { this.drivetrain = drivetrain; } - public DriveSignal update() { + public void update() { DriveSignal signal = DriveSignal.NEUTRAL; switch (state) { @@ -81,7 +81,7 @@ public DriveSignal update() { break; } - return signal; + drivetrain.drive(signal); } public boolean isIdle() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index 2f87057..f3e0d31 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -95,7 +95,7 @@ public void periodic() { switch (state) { case PATH_FOLLOWING: - drive_signal = trajectory_controller.update(); + trajectory_controller.update(); if (trajectory_controller.isIdle()) { state = State.OPEN_LOOP; } @@ -111,7 +111,6 @@ public void periodic() { } public void drive(DriveSignal drive_signal) { - state = State.OPEN_LOOP; this.drive_signal = drive_signal; } From 35bf366c339122e79ee6e1772bd21cdb92805441 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Tue, 18 Feb 2020 18:46:02 -0800 Subject: [PATCH 03/21] move stuff around --- .../org/team4159/frc/robot/Constants.java | 4 +- .../frc/robot/subsystems/Drivetrain.java | 1 - .../org/team4159/lib/hardware/Gearing.java | 4 +- .../hardware/controller/ctre/CardinalFX.java | 2 - .../lib/hardware/motors/MotorData.java | 19 --------- .../team4159/lib/hardware/motors/Motors.java | 19 --------- .../lib/math/{Baba.java => Cardinal.java} | 4 +- .../team4159/lib/physics/DCMotorModel.java | 41 +++++++++++++++++++ .../simulation/SimulationConfiguration.java | 6 +++ .../lib/simulation/motors/Motors.java | 23 +++++++++++ .../lib/control/signal/TestLowPassFilter.java | 4 +- 11 files changed, 79 insertions(+), 48 deletions(-) delete mode 100644 src/main/java/org/team4159/lib/hardware/motors/MotorData.java delete mode 100644 src/main/java/org/team4159/lib/hardware/motors/Motors.java rename src/main/java/org/team4159/lib/math/{Baba.java => Cardinal.java} (55%) create mode 100644 src/main/java/org/team4159/lib/physics/DCMotorModel.java create mode 100644 src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java create mode 100644 src/main/java/org/team4159/lib/simulation/motors/Motors.java diff --git a/src/main/java/org/team4159/frc/robot/Constants.java b/src/main/java/org/team4159/frc/robot/Constants.java index 5b0b5a7..4b27354 100644 --- a/src/main/java/org/team4159/frc/robot/Constants.java +++ b/src/main/java/org/team4159/frc/robot/Constants.java @@ -8,7 +8,7 @@ import org.team4159.lib.hardware.Gearing; import org.team4159.lib.hardware.joystick.T16000M; -import org.team4159.lib.math.Baba; +import org.team4159.lib.math.Cardinal; public final class Constants { public static final class ENCODERS { @@ -74,7 +74,7 @@ public static final class CAN_IDS { public static final class DRIVE_CONSTANTS { public static final Gearing GEARING = new Gearing(8.48, ENCODERS.FALCON_CPR); public static final double WHEEL_RADIUS = Units.inchesToMeters(3.0); - public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * Baba.kTau; + public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * Cardinal.kTau; public static final double METERS_PER_COUNT = WHEEL_CIRCUMFERENCE / GEARING.COUNTS_PER_REV; diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index f3e0d31..51785b3 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; diff --git a/src/main/java/org/team4159/lib/hardware/Gearing.java b/src/main/java/org/team4159/lib/hardware/Gearing.java index 648f636..af9d6c6 100644 --- a/src/main/java/org/team4159/lib/hardware/Gearing.java +++ b/src/main/java/org/team4159/lib/hardware/Gearing.java @@ -1,6 +1,6 @@ package org.team4159.lib.hardware; -import org.team4159.lib.math.Baba; +import org.team4159.lib.math.Cardinal; public class Gearing { public int COUNTS_PER_REV; @@ -10,6 +10,6 @@ public class Gearing { public Gearing(double gear_ratio, int encoder_cpr) { COUNTS_PER_REV = (int) (gear_ratio * encoder_cpr); COUNTS_PER_DEGREE = COUNTS_PER_REV / 360.0; - COUNTS_PER_RADIAN = (int) (COUNTS_PER_REV / Baba.kTau); + COUNTS_PER_RADIAN = (int) (COUNTS_PER_REV / Cardinal.kTau); } } diff --git a/src/main/java/org/team4159/lib/hardware/controller/ctre/CardinalFX.java b/src/main/java/org/team4159/lib/hardware/controller/ctre/CardinalFX.java index 3b03ac1..c4f91b4 100644 --- a/src/main/java/org/team4159/lib/hardware/controller/ctre/CardinalFX.java +++ b/src/main/java/org/team4159/lib/hardware/controller/ctre/CardinalFX.java @@ -20,8 +20,6 @@ public CardinalFX(int id, NeutralMode mode) { @Override public void set(ControlMode control_mode, double setpoint) { - System.out.println("SET:" + setpoint); - if (setpoint != last_setpoint || control_mode != last_control_mode) { super.set(control_mode, setpoint); diff --git a/src/main/java/org/team4159/lib/hardware/motors/MotorData.java b/src/main/java/org/team4159/lib/hardware/motors/MotorData.java deleted file mode 100644 index 1c7f69d..0000000 --- a/src/main/java/org/team4159/lib/hardware/motors/MotorData.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team4159.lib.hardware.motors; - -public class MotorData { - public String name; - public double free_speed, - free_current, - max_power, - stall_torque, - stall_current; - public MotorData(String name, double free_speed, double free_current, - double max_power, double stall_torque, double stall_current) { - this.name = name; - this.free_speed = free_speed; - this.free_current = free_current; - this.max_power = max_power; - this.stall_torque = stall_torque; - this.stall_current = stall_current; - } -} diff --git a/src/main/java/org/team4159/lib/hardware/motors/Motors.java b/src/main/java/org/team4159/lib/hardware/motors/Motors.java deleted file mode 100644 index 0f28fa3..0000000 --- a/src/main/java/org/team4159/lib/hardware/motors/Motors.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team4159.lib.hardware.motors; - -public class Motors { - // From https://motors.vex.com/ - public static final MotorData Falcon_500 = new MotorData("Falcon 500", 6380, 1.5, 783, 4.69, 257); - public static final MotorData NEO = new MotorData("NEO", 5880, 1.3, 516, 3.36, 166); - public static final MotorData CIM = new MotorData("CIM", 5330, 2.7, 337, 2.41, 131); - public static final MotorData MiniCIM = new MotorData("Mini CIM", 5840, 3, 215, 1.41, 89); - public static final MotorData BAG = new MotorData("BAG", 13180, 1.8, 149, 0.43, 53); - public static final MotorData _775pro = new MotorData("775pro", 18730, 0.7, 347, 0.71, 134); - public static final MotorData AndyMark_9015 = new MotorData("AndyMark 9015", 14270, 3.7, 134, 0.36, 71); - public static final MotorData AndyMark_NeveRest = new MotorData("AndyMark NeveRest", 5480, 0.4, 25, 0.17, 10); - public static final MotorData AndyMark_RS775_125 = new MotorData("AndyMark RS775-125", 5800, 1.6, 43, 0.28, 18); - public static final MotorData BaneBots_RS775_18V = new MotorData("BaneBots RS-775 18V", 13050, 2.7, 246, 0.72, 97); - public static final MotorData BaneBots_RS_550 = new MotorData("BaneBots RS-550", 19000, 0.4, 190, 0.38, 84); - - // From http://www.revrobotics.com/content/docs/REV-21-1651-DS.pdf - public static final MotorData NEO_550 = new MotorData("NEO 550", 1100, 1.4, 279, 0.97, 100); -} diff --git a/src/main/java/org/team4159/lib/math/Baba.java b/src/main/java/org/team4159/lib/math/Cardinal.java similarity index 55% rename from src/main/java/org/team4159/lib/math/Baba.java rename to src/main/java/org/team4159/lib/math/Cardinal.java index 06efd2b..5b6bcf7 100644 --- a/src/main/java/org/team4159/lib/math/Baba.java +++ b/src/main/java/org/team4159/lib/math/Cardinal.java @@ -1,6 +1,8 @@ package org.team4159.lib.math; -public class Baba { +// car·​di·​nal /ˈkärd-nəl/ adjective 1. of basic importance + +public class Cardinal { public static double kEpsilon = 1E-9; public static double kTau = Math.PI * 2; } diff --git a/src/main/java/org/team4159/lib/physics/DCMotorModel.java b/src/main/java/org/team4159/lib/physics/DCMotorModel.java new file mode 100644 index 0000000..e7c51d5 --- /dev/null +++ b/src/main/java/org/team4159/lib/physics/DCMotorModel.java @@ -0,0 +1,41 @@ +package org.team4159.lib.physics; + +// model of a DC motor + +public class DCMotorModel { + public final String name; + public final double + free_speed, // rpm + free_current, // amps + stall_torque, // newton-meters + stall_current, // amps + test_voltage; // volts + + public DCMotorModel(String name, + double free_speed, double free_current, + double stall_torque, double stall_current, + double test_voltage) { + this.name = name; + this.free_speed = free_speed; + this.free_current = free_current; + this.stall_torque = stall_torque; + this.stall_current = stall_current; + this.test_voltage = test_voltage; + } + + public double resistance() { + return test_voltage / stall_current; // V = IR -> R = V/I + } + + public double voltage_constant() { + return free_speed / (test_voltage - free_current * resistance()); // voltage dropped at free speed = free_current * resistance + } + + public double back_emf_constant() { + return 1 / voltage_constant(); + } + + public double torque_constant() { + return stall_torque / stall_current; + } +} diff --git a/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java b/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java new file mode 100644 index 0000000..b754181 --- /dev/null +++ b/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java @@ -0,0 +1,6 @@ +package org.team4159.lib.simulation; + +public class SimulationConfiguration { + public static double SIMULATION_BATTERY_VOLTAGE = 12; + public static double SIMULATION_TIMESTEP = 0.001; // seconds +} diff --git a/src/main/java/org/team4159/lib/simulation/motors/Motors.java b/src/main/java/org/team4159/lib/simulation/motors/Motors.java new file mode 100644 index 0000000..ef85840 --- /dev/null +++ b/src/main/java/org/team4159/lib/simulation/motors/Motors.java @@ -0,0 +1,23 @@ +package org.team4159.lib.simulation.motors; + +import org.team4159.lib.physics.DCMotorModel; + +public class Motors { + public static final double TEST_VOLTAGE = 12; + + // From https://motors.vex.com/ + public static final DCMotorModel Falcon_500 = new DCMotorModel("Falcon 500", 6380, 1.5, 783, 4.69, 257, TEST_VOLTAGE); + public static final DCMotorModel NEO = new DCMotorModel("NEO", 5880, 1.3, 516, 3.36, 166, TEST_VOLTAGE); + public static final DCMotorModel CIM = new DCMotorModel("CIM", 5330, 2.7, 337, 2.41, 131, TEST_VOLTAGE); + public static final DCMotorModel MiniCIM = new DCMotorModel("Mini CIM", 5840, 3, 215, 1.41, 89, TEST_VOLTAGE); + public static final DCMotorModel BAG = new DCMotorModel("BAG", 13180, 1.8, 149, 0.43, 53, TEST_VOLTAGE); + public static final DCMotorModel _775pro = new DCMotorModel("775pro", 18730, 0.7, 347, 0.71, 134, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_9015 = new DCMotorModel("AndyMark 9015", 14270, 3.7, 134, 0.36, 71, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_NeveRest = new DCMotorModel("AndyMark NeveRest", 5480, 0.4, 25, 0.17, 10, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_RS775_125 = new DCMotorModel("AndyMark RS775-125", 5800, 1.6, 43, 0.28, 18, TEST_VOLTAGE); + public static final DCMotorModel BaneBots_RS775_18V = new DCMotorModel("BaneBots RS-775 18V", 13050, 2.7, 246, 0.72, 97, TEST_VOLTAGE); + public static final DCMotorModel BaneBots_RS_550 = new DCMotorModel("BaneBots RS-550", 19000, 0.4, 190, 0.38, 84, TEST_VOLTAGE); + + // From http://www.revrobotics.com/content/docs/REV-21-1651-DS.pdf + public static final DCMotorModel NEO_550 = new DCMotorModel("NEO 550", 1100, 1.4, 279, 0.97, 100, TEST_VOLTAGE); +} diff --git a/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java b/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java index 72b3731..9319526 100644 --- a/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java +++ b/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java @@ -10,7 +10,7 @@ import org.team4159.lib.control.signal.filters.LowPassFilter; import org.team4159.lib.logging.CSVWriter; -import org.team4159.lib.math.Baba; +import org.team4159.lib.math.Cardinal; public class TestLowPassFilter { @Test @@ -18,7 +18,7 @@ public void TestDoesNothingWhenSmoothingIs1() { LowPassFilter filter = new LowPassFilter(0, 1, 1); for (int i = 0; i < 10; i++) { - Assert.assertEquals(i, filter.calculate(i), Baba.kEpsilon); + Assert.assertEquals(i, filter.calculate(i), Cardinal.kEpsilon); } } From cd48b9afaa291002a7259ffc3306008782d6ff9c Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Tue, 18 Feb 2020 19:03:01 -0800 Subject: [PATCH 04/21] add methods to model --- .../team4159/lib/physics/DCMotorModel.java | 10 +++++++ .../lib/simulation/motors/Motors.java | 28 ++++++++++--------- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/team4159/lib/physics/DCMotorModel.java b/src/main/java/org/team4159/lib/physics/DCMotorModel.java index e7c51d5..af4fdc4 100644 --- a/src/main/java/org/team4159/lib/physics/DCMotorModel.java +++ b/src/main/java/org/team4159/lib/physics/DCMotorModel.java @@ -23,6 +23,8 @@ public DCMotorModel(String name, this.test_voltage = test_voltage; } + // derived constants + public double resistance() { return test_voltage / stall_current; // V = IR -> R = V/I } @@ -38,4 +40,12 @@ public double back_emf_constant() { public double torque_constant() { return stall_torque / stall_current; } + + public double get_current_for_speed(final double rpm, final double voltage) { + return (voltage - rpm / voltage_constant()) / resistance(); + } + + public double get_torque_for_speed(final double rpm, final double voltage) { + return torque_constant() * get_current_for_speed(rpm, voltage); + } } diff --git a/src/main/java/org/team4159/lib/simulation/motors/Motors.java b/src/main/java/org/team4159/lib/simulation/motors/Motors.java index ef85840..c553b97 100644 --- a/src/main/java/org/team4159/lib/simulation/motors/Motors.java +++ b/src/main/java/org/team4159/lib/simulation/motors/Motors.java @@ -2,22 +2,24 @@ import org.team4159.lib.physics.DCMotorModel; +// models of common FRC motors + public class Motors { - public static final double TEST_VOLTAGE = 12; + private static final double TEST_VOLTAGE = 12; // From https://motors.vex.com/ - public static final DCMotorModel Falcon_500 = new DCMotorModel("Falcon 500", 6380, 1.5, 783, 4.69, 257, TEST_VOLTAGE); - public static final DCMotorModel NEO = new DCMotorModel("NEO", 5880, 1.3, 516, 3.36, 166, TEST_VOLTAGE); - public static final DCMotorModel CIM = new DCMotorModel("CIM", 5330, 2.7, 337, 2.41, 131, TEST_VOLTAGE); - public static final DCMotorModel MiniCIM = new DCMotorModel("Mini CIM", 5840, 3, 215, 1.41, 89, TEST_VOLTAGE); - public static final DCMotorModel BAG = new DCMotorModel("BAG", 13180, 1.8, 149, 0.43, 53, TEST_VOLTAGE); - public static final DCMotorModel _775pro = new DCMotorModel("775pro", 18730, 0.7, 347, 0.71, 134, TEST_VOLTAGE); - public static final DCMotorModel AndyMark_9015 = new DCMotorModel("AndyMark 9015", 14270, 3.7, 134, 0.36, 71, TEST_VOLTAGE); - public static final DCMotorModel AndyMark_NeveRest = new DCMotorModel("AndyMark NeveRest", 5480, 0.4, 25, 0.17, 10, TEST_VOLTAGE); - public static final DCMotorModel AndyMark_RS775_125 = new DCMotorModel("AndyMark RS775-125", 5800, 1.6, 43, 0.28, 18, TEST_VOLTAGE); - public static final DCMotorModel BaneBots_RS775_18V = new DCMotorModel("BaneBots RS-775 18V", 13050, 2.7, 246, 0.72, 97, TEST_VOLTAGE); - public static final DCMotorModel BaneBots_RS_550 = new DCMotorModel("BaneBots RS-550", 19000, 0.4, 190, 0.38, 84, TEST_VOLTAGE); + public static final DCMotorModel Falcon_500 = new DCMotorModel("Falcon 500", 6380, 1.5, 4.69, 257, TEST_VOLTAGE); + public static final DCMotorModel NEO = new DCMotorModel("NEO", 5880, 1.3, 3.36, 166, TEST_VOLTAGE); + public static final DCMotorModel CIM = new DCMotorModel("CIM", 5330, 2.7, 2.41, 131, TEST_VOLTAGE); + public static final DCMotorModel MiniCIM = new DCMotorModel("Mini CIM", 5840, 3, 1.41, 89, TEST_VOLTAGE); + public static final DCMotorModel BAG = new DCMotorModel("BAG", 13180, 1.8, 0.43, 53, TEST_VOLTAGE); + public static final DCMotorModel _775pro = new DCMotorModel("775pro", 18730, 0.7, 0.71, 134, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_9015 = new DCMotorModel("AndyMark 9015", 14270, 3.7, 0.36, 71, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_NeveRest = new DCMotorModel("AndyMark NeveRest", 5480, 0.4, 0.17, 10, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_RS775_125 = new DCMotorModel("AndyMark RS775-125", 5800, 1.6, 0.28, 18, TEST_VOLTAGE); + public static final DCMotorModel BaneBots_RS775_18V = new DCMotorModel("BaneBots RS-775 18V", 13050, 2.7, 0.72, 97, TEST_VOLTAGE); + public static final DCMotorModel BaneBots_RS_550 = new DCMotorModel("BaneBots RS-550", 19000, 0.4, 0.38, 84, TEST_VOLTAGE); // From http://www.revrobotics.com/content/docs/REV-21-1651-DS.pdf - public static final DCMotorModel NEO_550 = new DCMotorModel("NEO 550", 1100, 1.4, 279, 0.97, 100, TEST_VOLTAGE); + public static final DCMotorModel NEO_550 = new DCMotorModel("NEO 550", 11000, 1.4, 0.97, 100, TEST_VOLTAGE); } From 008e5f423046dfbe0aa6fef300c214610c3175e8 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Tue, 18 Feb 2020 21:35:30 -0800 Subject: [PATCH 05/21] aaaaaaaahhh been trying for an hour and it still does not work --- .../org/team4159/frc/robot/AutoSelector.java | 8 -- .../org/team4159/frc/robot/Constants.java | 1 + .../team4159/frc/robot/RobotContainer.java | 9 +- .../frc/robot/commands/auto/BlueAuto.java | 43 ------- .../DrivetrainCharacterization.java | 73 ------------ .../commands/drivetrain/TurnDegrees.java | 39 ------- .../frc/robot/commands/feeder/RunFeeder.java | 40 ------- .../frc/robot/commands/multi/Shoot.java | 40 ------- .../robot/commands/turret/LimelightRPM.java | 45 -------- .../robot/commands/turret/LimelightSeek.java | 64 ---------- .../frc/robot/commands/turret/ZeroTurret.java | 37 ------ .../frc/robot/controllers/ArmController.java | 59 ++++++++++ .../frc/robot/controllers/AutoController.java | 4 - .../robot/controllers/ShootingController.java | 4 - .../controllers/complex/AutoController.java | 4 + .../{ => complex}/IntakeController.java | 14 +-- .../complex/ShootingController.java | 4 + .../team4159/frc/robot/subsystems/Arm.java | 59 +++------- .../org/team4159/lib/hardware/Gearing.java | 5 +- .../java/org/team4159/lib/math/Cardinal.java | 1 - .../org/team4159/lib/math/Conversions.java | 9 ++ .../java/org/team4159/lib/math/Epsilon.java | 18 +++ .../lib/{ => math}/physics/DCMotorModel.java | 4 +- .../team4159/lib/simulation/MotorModels.java | 26 +++++ .../lib/simulation/RelativeEncoderMock.java | 30 +++++ .../simulation/SimulationConfiguration.java | 2 +- .../lib/simulation/motors/Motors.java | 25 ---- .../frc/robot/subsystems/ArmTest.java | 109 ++++++++++++++++++ .../lib/control/signal/TestLowPassFilter.java | 3 +- 29 files changed, 293 insertions(+), 486 deletions(-) delete mode 100644 src/main/java/org/team4159/frc/robot/commands/auto/BlueAuto.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/characterization/DrivetrainCharacterization.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/drivetrain/TurnDegrees.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/feeder/RunFeeder.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/multi/Shoot.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/turret/LimelightRPM.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/turret/LimelightSeek.java delete mode 100644 src/main/java/org/team4159/frc/robot/commands/turret/ZeroTurret.java create mode 100644 src/main/java/org/team4159/frc/robot/controllers/ArmController.java delete mode 100644 src/main/java/org/team4159/frc/robot/controllers/AutoController.java delete mode 100644 src/main/java/org/team4159/frc/robot/controllers/ShootingController.java create mode 100644 src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java rename src/main/java/org/team4159/frc/robot/controllers/{ => complex}/IntakeController.java (77%) create mode 100644 src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java create mode 100644 src/main/java/org/team4159/lib/math/Conversions.java create mode 100644 src/main/java/org/team4159/lib/math/Epsilon.java rename src/main/java/org/team4159/lib/{ => math}/physics/DCMotorModel.java (92%) create mode 100644 src/main/java/org/team4159/lib/simulation/MotorModels.java create mode 100644 src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java delete mode 100644 src/main/java/org/team4159/lib/simulation/motors/Motors.java create mode 100644 src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java diff --git a/src/main/java/org/team4159/frc/robot/AutoSelector.java b/src/main/java/org/team4159/frc/robot/AutoSelector.java index 4a0aae4..2bdbe7a 100644 --- a/src/main/java/org/team4159/frc/robot/AutoSelector.java +++ b/src/main/java/org/team4159/frc/robot/AutoSelector.java @@ -3,13 +3,5 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import org.team4159.frc.robot.subsystems.Drivetrain; -import org.team4159.frc.robot.commands.auto.FollowTrajectory; - public class AutoSelector extends SendableChooser { - public AutoSelector(Drivetrain drivetrain) { - Command one = new FollowTrajectory(Trajectories.TEST_TRAJECTORY, drivetrain); - - setDefaultOption("1", one); - } } diff --git a/src/main/java/org/team4159/frc/robot/Constants.java b/src/main/java/org/team4159/frc/robot/Constants.java index 4b27354..4417c9f 100644 --- a/src/main/java/org/team4159/frc/robot/Constants.java +++ b/src/main/java/org/team4159/frc/robot/Constants.java @@ -11,6 +11,7 @@ import org.team4159.lib.math.Cardinal; public final class Constants { + // not sure where to put these public static final class ENCODERS { public static final int FALCON_CPR = 2048; public static final int MAG_ENCODER_CPR = 4096; diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index cca2b14..51f4fdc 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -2,11 +2,10 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.*; -import org.team4159.frc.robot.controllers.IntakeController; +import org.team4159.frc.robot.controllers.complex.IntakeController; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.hardware.Limelight; import org.team4159.frc.robot.subsystems.*; @@ -23,13 +22,13 @@ public class RobotContainer { private final Arm arm = new Arm(); private final Turret turret = new Turret(limelight); - private final IntakeController intake_controller = new IntakeController(arm, intake, feeder); + private final IntakeController intake_controller = new IntakeController(arm.getController(), intake, feeder); private final Joystick left_joy = new Joystick(CONTROLS.LEFT_JOY.USB_PORT); private final Joystick right_joy = new Joystick(CONTROLS.RIGHT_JOY.USB_PORT); private final Joystick secondary_joy = new Joystick(CONTROLS.SECONDARY_JOY.USB_PORT); - private final AutoSelector auto_selector = new AutoSelector(drivetrain); + private final AutoSelector auto_selector = new AutoSelector(); public RobotContainer() { configureCamera(); @@ -68,7 +67,7 @@ public void updateControllerInputs() { public void updateArmInputs() { if (secondary_joy.getRawButtonPressed(CONTROLS.SECONDARY_JOY.BUTTON_IDS.TOGGLE_ARM)) { - arm.setSetpoint(Math.abs(arm.getSetpoint() - ARM_CONSTANTS.RANGE_IN_COUNTS)); + arm.getController().setSetpoint(Math.abs(arm.getController().getSetpoint() - ARM_CONSTANTS.RANGE_IN_COUNTS)); } } diff --git a/src/main/java/org/team4159/frc/robot/commands/auto/BlueAuto.java b/src/main/java/org/team4159/frc/robot/commands/auto/BlueAuto.java deleted file mode 100644 index b7edfa1..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/auto/BlueAuto.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.team4159.frc.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -import edu.wpi.first.wpilibj2.command.WaitCommand; -import org.team4159.frc.robot.Constants; -import org.team4159.frc.robot.Trajectories; -import org.team4159.frc.robot.commands.arm.ToggleArm; -import org.team4159.frc.robot.commands.drivetrain.TurnDegrees; -import org.team4159.frc.robot.commands.feeder.RunFeeder; -import org.team4159.frc.robot.commands.intake.RunIntake; -import org.team4159.frc.robot.commands.multi.Shoot; -import org.team4159.frc.robot.commands.turret.LimelightSeek; -import org.team4159.frc.robot.subsystems.*; - -/* - * Placeholder until we plan out an autonomous routine - */ -public class BlueAuto extends SequentialCommandGroup { - private final double SHOOTER_TPS = 10000; - - public BlueAuto(Drivetrain drivetrain, Arm arm, Feeder feeder, Intake intake, Turret turret, Shooter shooter, Neck neck) { - addCommands( - new SequentialCommandGroup( - new ParallelCommandGroup( - new FollowTrajectory(Trajectories.SCRIMMAGE_AUTO, drivetrain), - new WaitCommand(1) - .andThen(new ToggleArm(arm)) - .andThen( - new RunIntake(5, intake) - .alongWith(new RunFeeder(5, feeder)) - ) - ), - new ParallelCommandGroup( - new Shoot(SHOOTER_TPS, shooter, neck, turret), - new LimelightSeek(turret) - ) - ) - ); - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/characterization/DrivetrainCharacterization.java b/src/main/java/org/team4159/frc/robot/commands/characterization/DrivetrainCharacterization.java deleted file mode 100644 index 36759c0..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/characterization/DrivetrainCharacterization.java +++ /dev/null @@ -1,73 +0,0 @@ -package org.team4159.frc.robot.commands.characterization; - -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.team4159.frc.robot.subsystems.Drivetrain; -import org.team4159.lib.control.signal.DriveSignal; - -/* - * http://docs.wpilib.org/en/latest/docs/software/wpilib-tools/robot-characterization - */ -public class DrivetrainCharacterization extends CommandBase { - private Drivetrain drivetrain; - - private NetworkTableEntry auto_speed_entry; - private NetworkTableEntry telemetry_entry; - private NetworkTableEntry rotate_entry; - - public DrivetrainCharacterization(Drivetrain drivetrain) { - this.drivetrain = drivetrain; - addRequirements(drivetrain); - } - - @Override - public void initialize() { - // set update rate to 10ms, needed for characterization routine - NetworkTableInstance.getDefault().setUpdateRate(0.010); - - auto_speed_entry = NetworkTableInstance.getDefault().getEntry("/robot/autospeed"); - telemetry_entry = NetworkTableInstance.getDefault().getEntry("/robot/telemetry"); - rotate_entry = NetworkTableInstance.getDefault().getEntry("/robot/rotate"); - } - - @Override - public void execute() { - // Retrieve values to send back before telling the motors to do something - double timestamp = Timer.getFPGATimestamp(); - double battery = RobotController.getBatteryVoltage(); - double drivetrain_angle = Math.toRadians(drivetrain.getDirection()); - - double left_pos = drivetrain.getLeftDistance(); - double left_rate = drivetrain.getLeftVelocity() * 10; - double right_pos = drivetrain.getRightDistance(); - double right_rate = drivetrain.getRightVelocity() * 10; - double left_volts = drivetrain.getLeftVoltage(); - double right_volts = drivetrain.getRightVoltage(); - - // Retrieve the commanded speed from NetworkTables - double autospeed = auto_speed_entry.getDouble(0); - - // command motors to do things - drivetrain.rawDrive(new DriveSignal((rotate_entry.getBoolean(false) ? 1 : -1) * autospeed, autospeed)); - - // send telemetry data array back to NT - telemetry_entry.setNumberArray(new Number[]{ - timestamp, - battery, - autospeed, - left_volts, - right_volts, - left_pos, - right_pos, - left_rate, - right_rate, - drivetrain_angle - }); - - // Provide visuals to humans - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/drivetrain/TurnDegrees.java b/src/main/java/org/team4159/frc/robot/commands/drivetrain/TurnDegrees.java deleted file mode 100644 index 0a81dde..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/drivetrain/TurnDegrees.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.team4159.frc.robot.commands.drivetrain; - -import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj2.command.CommandBase; -import org.team4159.frc.robot.subsystems.Drivetrain; - -import static org.team4159.frc.robot.Constants.*; - - -public class TurnDegrees extends CommandBase { - private Drivetrain drivetrain; - private double initial_degrees; - - private PIDController controller = new PIDController(DRIVE_CONSTANTS.kP, 0, 0); - - public TurnDegrees(double theta_degrees, Drivetrain drivetrain) { - this.drivetrain = drivetrain; - controller.setSetpoint(theta_degrees); - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - initial_degrees = drivetrain.getDirection(); - } - - @Override - public void execute() { - double output = controller.calculate(drivetrain.getDirection() - initial_degrees); - - drivetrain.voltsDrive(output, -output); - } - - @Override - public boolean isFinished() { - return controller.atSetpoint(); - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/feeder/RunFeeder.java b/src/main/java/org/team4159/frc/robot/commands/feeder/RunFeeder.java deleted file mode 100644 index 057babc..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/feeder/RunFeeder.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.team4159.frc.robot.commands.feeder; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.team4159.frc.robot.subsystems.Feeder; - -public class RunFeeder extends CommandBase { - private Feeder feeder; - - private Timer timer = new Timer(); - private double duration; - - public RunFeeder(double duration_seconds, Feeder feeder) { - this.duration = duration_seconds; - this.feeder = feeder; - - addRequirements(feeder); - } - - @Override - public void initialize() { - timer.start(); - } - - @Override - public void execute() { - feeder.feed(); - } - - @Override - public boolean isFinished() { - return timer.hasPeriodPassed(duration); - } - - @Override - public void end(boolean interrupted) { - feeder.stop(); - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/multi/Shoot.java b/src/main/java/org/team4159/frc/robot/commands/multi/Shoot.java deleted file mode 100644 index b82f99f..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/multi/Shoot.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.team4159.frc.robot.commands.multi; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.team4159.frc.robot.subsystems.Neck; -import org.team4159.frc.robot.subsystems.Shooter; -import org.team4159.frc.robot.subsystems.Turret; - -public class Shoot extends CommandBase { - private Turret turret; - private Shooter shooter; - private Neck neck; - private double target_speed; - - private boolean shooting; - - public Shoot(double target_speed, Shooter shooter, Neck neck, Turret turret) { - this.shooter = shooter; - this.neck = neck; - this.turret = turret; - - this.target_speed = target_speed; - - addRequirements(shooter, neck); - } - - @Override - public void initialize() { - shooter.setTargetSpeed(target_speed); - } - - @Override - public void execute() { - if (turret.isPointingAtTarget() && shooter.isAtTargetSpeed()) { - neck.neck(); - } else { - neck.stop(); - } - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/turret/LimelightRPM.java b/src/main/java/org/team4159/frc/robot/commands/turret/LimelightRPM.java deleted file mode 100644 index 9e12e16..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/turret/LimelightRPM.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.team4159.frc.robot.commands.turret; - -import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj2.command.CommandBase; -import org.team4159.frc.robot.Constants; -import org.team4159.frc.robot.subsystems.Drivetrain; -import org.team4159.frc.robot.subsystems.Shooter; -import org.team4159.frc.robot.subsystems.Turret; -import org.team4159.lib.hardware.Limelight; - - -public class LimelightRPM extends CommandBase { - private Limelight limelight; - private Shooter shooter; - private double distance; - private double voltage_constant= 0; - private double n = 1; - - public LimelightRPM(Shooter shooter) { - this.shooter = shooter; - limelight = shooter.getLimelight(); - addRequirements(shooter); - } - - private double calculateVoltage(double dist) { - return voltage_constant * Math.pow(dist, n); - } - - @Override - public void initialize() { - limelight.setLEDMode(Limelight.LEDMode.ForceOn); - distance = shooter.getDistanceToVisionTarget(); - } - - @Override - public void execute() { - shooter.setRawSpeed(calculateVoltage(distance)); - } - - @Override - public void end(boolean interrupted) { - limelight.setLEDMode(Limelight.LEDMode.ForceOff); - shooter.stop(); - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/turret/LimelightSeek.java b/src/main/java/org/team4159/frc/robot/commands/turret/LimelightSeek.java deleted file mode 100644 index 95c1606..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/turret/LimelightSeek.java +++ /dev/null @@ -1,64 +0,0 @@ -package org.team4159.frc.robot.commands.turret; - -import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.team4159.frc.robot.subsystems.Turret; -import org.team4159.lib.hardware.Limelight; - -import static org.team4159.frc.robot.Constants.*; - -public class LimelightSeek extends CommandBase { - private Turret turret; - private Limelight limelight; - - private boolean seeking = false; - private int seeking_range = TURRET_CONSTANTS.STARTING_SEEKING_RANGE; - private int seeking_direction; - private int starting_position; - - private PIDController pid_controller = new PIDController( - TURRET_CONSTANTS.LIMELIGHT_TURN_kP, - 0, - TURRET_CONSTANTS.LIMELIGHT_TURN_kD - ); - - public LimelightSeek(Turret turret) { - this.turret = turret; - this.limelight = turret.getLimelight(); - - addRequirements(turret); - } - - @Override - public void initialize() { - limelight.setLEDMode(Limelight.LEDMode.ForceOn); - pid_controller.reset(); - } - - @Override - public void execute() { - if (limelight.isTargetVisible()) { - seeking = false; - turret.setRawSpeed(pid_controller.calculate(limelight.getTargetHorizontalOffset())); - } else { - if (seeking) { - turret.setRawSpeed(TURRET_CONSTANTS.SEEKING_SPEED * seeking_direction); - if (Math.abs(turret.getPosition() - starting_position) > seeking_range) { - seeking_direction = -seeking_direction; - seeking_range += TURRET_CONSTANTS.SEEKING_RANGE_INCREMENT; - } - } else { - seeking = true; - starting_position = turret.getPosition(); - seeking_direction = -1; - } - } - } - - @Override - public void end(boolean interrupted) { - limelight.setLEDMode(Limelight.LEDMode.ForceOff); - turret.stop(); - } -} diff --git a/src/main/java/org/team4159/frc/robot/commands/turret/ZeroTurret.java b/src/main/java/org/team4159/frc/robot/commands/turret/ZeroTurret.java deleted file mode 100644 index 4ab9ed1..0000000 --- a/src/main/java/org/team4159/frc/robot/commands/turret/ZeroTurret.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.team4159.frc.robot.commands.turret; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.team4159.frc.robot.subsystems.Turret; - -import static org.team4159.frc.robot.Constants.*; - -public class ZeroTurret extends CommandBase { - private Turret turret; - - public ZeroTurret(Turret turret) { - this.turret = turret; - - addRequirements(turret); - } - - @Override - public void execute() { - turret.setRawSpeed(TURRET_CONSTANTS.ZEROING_SPEED); - - turret.setZeroed(false); - } - - @Override - public boolean isFinished() { - // TODO: check direction - return turret.isReverseLimitSwitchClosed(); - } - - @Override - public void end(boolean interrupted) { - turret.setZeroed(true); - - turret.stop(); - } -} diff --git a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java new file mode 100644 index 0000000..50b0301 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java @@ -0,0 +1,59 @@ +package org.team4159.frc.robot.controllers; + +import edu.wpi.first.wpilibj.controller.PIDController; + +import org.team4159.frc.robot.subsystems.Arm; + +import static org.team4159.frc.robot.Constants.*; + +public class ArmController { + private enum State { + ZEROING, + CLOSED_LOOP, + ESTOP + } + private State state = State.ZEROING; + + private Arm arm; + + private PIDController pid_controller; + + public ArmController(Arm arm) { + this.arm = arm; + + pid_controller = new PIDController( + ARM_CONSTANTS.kP, + ARM_CONSTANTS.kI, + ARM_CONSTANTS.kD + ); + pid_controller.setTolerance(ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); + } + + public void update() { + switch (state) { + case ZEROING: + arm.setRawSpeed(ARM_CONSTANTS.ZEROING_SPEED); + if (arm.isLimitSwitchClosed()) { + arm.zeroEncoder(); + state = State.CLOSED_LOOP; + } + break; + case CLOSED_LOOP: + double output = pid_controller.calculate(arm.getPosition()); + arm.setRawVoltage(output); + break; + } + } + + public boolean isAtSetpoint() { + return pid_controller.atSetpoint(); + } + + public void setSetpoint(int setpoint) { + pid_controller.setSetpoint(setpoint); + } + + public int getSetpoint() { + return (int) pid_controller.getSetpoint(); + } +} diff --git a/src/main/java/org/team4159/frc/robot/controllers/AutoController.java b/src/main/java/org/team4159/frc/robot/controllers/AutoController.java deleted file mode 100644 index 790c908..0000000 --- a/src/main/java/org/team4159/frc/robot/controllers/AutoController.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.team4159.frc.robot.controllers; - -public class AutoController { -} diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShootingController.java b/src/main/java/org/team4159/frc/robot/controllers/ShootingController.java deleted file mode 100644 index 3444e2d..0000000 --- a/src/main/java/org/team4159/frc/robot/controllers/ShootingController.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.team4159.frc.robot.controllers; - -public class ShootingController { -} diff --git a/src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java b/src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java new file mode 100644 index 0000000..20aefd8 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java @@ -0,0 +1,4 @@ +package org.team4159.frc.robot.controllers.complex; + +public class AutoController { +} diff --git a/src/main/java/org/team4159/frc/robot/controllers/IntakeController.java b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java similarity index 77% rename from src/main/java/org/team4159/frc/robot/controllers/IntakeController.java rename to src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java index 619bd63..58c68e2 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/IntakeController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java @@ -1,6 +1,6 @@ -package org.team4159.frc.robot.controllers; +package org.team4159.frc.robot.controllers.complex; -import org.team4159.frc.robot.subsystems.Arm; +import org.team4159.frc.robot.controllers.ArmController; import org.team4159.frc.robot.subsystems.Feeder; import org.team4159.frc.robot.subsystems.Intake; @@ -14,12 +14,12 @@ private enum State { } private State state; - private Arm arm; + private ArmController arm_controller; private Intake intake; private Feeder feeder; - public IntakeController(Arm arm, Intake intake, Feeder feeder) { - this.arm = arm; + public IntakeController(ArmController arm_controller, Intake intake, Feeder feeder) { + this.arm_controller = arm_controller; this.intake = intake; this.feeder = feeder; } @@ -34,7 +34,7 @@ public void update() { break; case DEPLOYING: arm_setpoint = ARM_CONSTANTS.DOWN_POSITION; - if (arm.isAtSetpoint()) { + if (arm_controller.isAtSetpoint()) { state = State.INTAKING; } break; @@ -45,7 +45,7 @@ public void update() { break; } - arm.setSetpoint(arm_setpoint); + arm_controller.setSetpoint(arm_setpoint); intake.setRawSpeed(intake_speed); feeder.setRawSpeed(feeder_speed); } diff --git a/src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java b/src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java new file mode 100644 index 0000000..d636dbb --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java @@ -0,0 +1,4 @@ +package org.team4159.frc.robot.controllers.complex; + +public class ShootingController { +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java index 5d2b46b..66198c2 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java @@ -4,30 +4,27 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.team4159.frc.robot.controllers.ArmController; import org.team4159.lib.hardware.controller.rev.CardinalMAX; import static org.team4159.frc.robot.Constants.*; public class Arm extends SubsystemBase { - private enum State { - ZEROING, - CLOSED_LOOP, - ESTOP - } - private State state = State.ZEROING; - private CANSparkMax arm_spark; private DigitalInput arm_limit_switch; private Encoder arm_encoder; - private PIDController pid_controller; + private ArmController arm_controller; public Arm() { + if (RobotBase.isSimulation()) { + return; + } + arm_spark = new CardinalMAX(CAN_IDS.ARM_SPARK, CANSparkMax.IdleMode.kBrake); arm_spark.setInverted(true); @@ -39,32 +36,12 @@ public Arm() { ARM_CONSTANTS.ENCODER_ENCODING_TYPE ); - pid_controller = new PIDController( - ARM_CONSTANTS.kP, - ARM_CONSTANTS.kI, - ARM_CONSTANTS.kD - ); - pid_controller.setTolerance(ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); + arm_controller = new ArmController(this); } @Override public void periodic() { - if (isLimitSwitchClosed()) { - zeroEncoder(); - } - - switch (state) { - case ZEROING: - setRawSpeed(ARM_CONSTANTS.ZEROING_SPEED); - if (isLimitSwitchClosed()) { - state = State.CLOSED_LOOP; - } - break; - case CLOSED_LOOP: - double output = pid_controller.calculate(arm_encoder.get()); - setRawVoltage(output); - break; - } + arm_controller.update(); } // motor setters @@ -74,28 +51,22 @@ public void setRawSpeed(double speed) { } public void setRawVoltage(double voltage) { - SmartDashboard.putNumber("arm voltage", voltage); - arm_spark.setVoltage(voltage); } - public void setSetpoint(int setpoint) { - pid_controller.setSetpoint(setpoint); - } - public void zeroEncoder() { arm_encoder.reset(); } - public boolean isLimitSwitchClosed() { - return !arm_limit_switch.get(); + public int getPosition() { + return arm_encoder.get(); } - public boolean isAtSetpoint() { - return pid_controller.atSetpoint(); + public boolean isLimitSwitchClosed() { + return !arm_limit_switch.get(); } - public int getSetpoint() { - return (int) pid_controller.getSetpoint(); + public ArmController getController() { + return arm_controller; } } diff --git a/src/main/java/org/team4159/lib/hardware/Gearing.java b/src/main/java/org/team4159/lib/hardware/Gearing.java index af9d6c6..aa9b13c 100644 --- a/src/main/java/org/team4159/lib/hardware/Gearing.java +++ b/src/main/java/org/team4159/lib/hardware/Gearing.java @@ -4,12 +4,11 @@ public class Gearing { public int COUNTS_PER_REV; - public double COUNTS_PER_DEGREE, - COUNTS_PER_RADIAN; + public double COUNTS_PER_DEGREE, COUNTS_PER_RADIAN; public Gearing(double gear_ratio, int encoder_cpr) { COUNTS_PER_REV = (int) (gear_ratio * encoder_cpr); COUNTS_PER_DEGREE = COUNTS_PER_REV / 360.0; - COUNTS_PER_RADIAN = (int) (COUNTS_PER_REV / Cardinal.kTau); + COUNTS_PER_RADIAN = COUNTS_PER_REV / Cardinal.kTau; } } diff --git a/src/main/java/org/team4159/lib/math/Cardinal.java b/src/main/java/org/team4159/lib/math/Cardinal.java index 5b6bcf7..212edb5 100644 --- a/src/main/java/org/team4159/lib/math/Cardinal.java +++ b/src/main/java/org/team4159/lib/math/Cardinal.java @@ -3,6 +3,5 @@ // car·​di·​nal /ˈkärd-nəl/ adjective 1. of basic importance public class Cardinal { - public static double kEpsilon = 1E-9; public static double kTau = Math.PI * 2; } diff --git a/src/main/java/org/team4159/lib/math/Conversions.java b/src/main/java/org/team4159/lib/math/Conversions.java new file mode 100644 index 0000000..8843896 --- /dev/null +++ b/src/main/java/org/team4159/lib/math/Conversions.java @@ -0,0 +1,9 @@ +package org.team4159.lib.math; + +public class Conversions { + private static double POUNDS_PER_KILOGRAM = 2.205; + + public static double poundsToKilograms(double pounds) { + return pounds / POUNDS_PER_KILOGRAM; + } +} diff --git a/src/main/java/org/team4159/lib/math/Epsilon.java b/src/main/java/org/team4159/lib/math/Epsilon.java new file mode 100644 index 0000000..de8d249 --- /dev/null +++ b/src/main/java/org/team4159/lib/math/Epsilon.java @@ -0,0 +1,18 @@ +package org.team4159.lib.math; + +/* +* Adapted from: Team 254 +* https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/lib/util/Util.java +*/ + +public class Epsilon { + public static double kEpsilon = 1E-12; + + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } +} diff --git a/src/main/java/org/team4159/lib/physics/DCMotorModel.java b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java similarity index 92% rename from src/main/java/org/team4159/lib/physics/DCMotorModel.java rename to src/main/java/org/team4159/lib/math/physics/DCMotorModel.java index af4fdc4..c19aa1d 100644 --- a/src/main/java/org/team4159/lib/physics/DCMotorModel.java +++ b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java @@ -1,4 +1,4 @@ -package org.team4159.lib.physics; +package org.team4159.lib.math.physics; // model of a DC motor @@ -42,7 +42,7 @@ public double torque_constant() { } public double get_current_for_speed(final double rpm, final double voltage) { - return (voltage - rpm / voltage_constant()) / resistance(); + return (voltage - rpm * back_emf_constant()) / resistance(); } public double get_torque_for_speed(final double rpm, final double voltage) { diff --git a/src/main/java/org/team4159/lib/simulation/MotorModels.java b/src/main/java/org/team4159/lib/simulation/MotorModels.java new file mode 100644 index 0000000..78895d9 --- /dev/null +++ b/src/main/java/org/team4159/lib/simulation/MotorModels.java @@ -0,0 +1,26 @@ +package org.team4159.lib.simulation; + +import edu.wpi.first.wpilibj.util.Units; +import org.team4159.lib.math.physics.DCMotorModel; + +// models of common FRC motors + +public class MotorModels { + private static final double TEST_VOLTAGE = 12; + + // From https://motors.vex.com/ + public static final DCMotorModel Falcon_500 = new DCMotorModel("Falcon 500", Units.rotationsPerMinuteToRadiansPerSecond(6380), 1.5, 4.69, 257, TEST_VOLTAGE); + public static final DCMotorModel NEO = new DCMotorModel("NEO", Units.rotationsPerMinuteToRadiansPerSecond(5880), 1.3, 3.36, 166, TEST_VOLTAGE); + public static final DCMotorModel CIM = new DCMotorModel("CIM", Units.rotationsPerMinuteToRadiansPerSecond(5330), 2.7, 2.41, 131, TEST_VOLTAGE); + public static final DCMotorModel MiniCIM = new DCMotorModel("Mini CIM", Units.rotationsPerMinuteToRadiansPerSecond(5840), 3, 1.41, 89, TEST_VOLTAGE); + public static final DCMotorModel BAG = new DCMotorModel("BAG", Units.rotationsPerMinuteToRadiansPerSecond(13180), 1.8, 0.43, 53, TEST_VOLTAGE); + public static final DCMotorModel _775pro = new DCMotorModel("775pro", Units.rotationsPerMinuteToRadiansPerSecond(18730), 0.7, 0.71, 134, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_9015 = new DCMotorModel("AndyMark 9015", Units.rotationsPerMinuteToRadiansPerSecond(14270), 3.7, 0.36, 71, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_NeveRest = new DCMotorModel("AndyMark NeveRest", Units.rotationsPerMinuteToRadiansPerSecond(5480), 0.4, 0.17, 10, TEST_VOLTAGE); + public static final DCMotorModel AndyMark_RS775_125 = new DCMotorModel("AndyMark RS775-125", Units.rotationsPerMinuteToRadiansPerSecond(5800), 1.6, 0.28, 18, TEST_VOLTAGE); + public static final DCMotorModel BaneBots_RS775_18V = new DCMotorModel("BaneBots RS-775 18V", Units.rotationsPerMinuteToRadiansPerSecond(13050), 2.7, 0.72, 97, TEST_VOLTAGE); + public static final DCMotorModel BaneBots_RS_550 = new DCMotorModel("BaneBots RS-550", Units.rotationsPerMinuteToRadiansPerSecond(19000), 0.4, 0.38, 84, TEST_VOLTAGE); + + // From http://www.revrobotics.com/content/docs/REV-21-1651-DS.pdf + public static final DCMotorModel NEO_550 = new DCMotorModel("NEO 550", Units.rotationsPerMinuteToRadiansPerSecond(11000), 1.4, 0.97, 100, TEST_VOLTAGE); +} diff --git a/src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java b/src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java new file mode 100644 index 0000000..b215635 --- /dev/null +++ b/src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java @@ -0,0 +1,30 @@ +package org.team4159.lib.simulation; + +// model of a relative encoder and the transmission's real position +// uses doubles over ints to allow for simulation over small timesteps but it's kind of iffy + +public class RelativeEncoderMock { + private double real_position = 0; + private double encoder_position = 0; + + public void move(double ticks) { + real_position += ticks; + encoder_position += ticks; + } + + public void setRealPosition(double position) { + this.real_position = position; + } + + public void setEncoderPosition(double position) { + this.encoder_position = position; + } + + public double getEncoderPosition() { + return encoder_position; + } + + public double getRealPosition() { + return real_position; + } +} diff --git a/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java b/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java index b754181..29a1907 100644 --- a/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java +++ b/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java @@ -1,6 +1,6 @@ package org.team4159.lib.simulation; public class SimulationConfiguration { - public static double SIMULATION_BATTERY_VOLTAGE = 12; public static double SIMULATION_TIMESTEP = 0.001; // seconds + public static double LOSS_COEFFICIENT = 0.8; } diff --git a/src/main/java/org/team4159/lib/simulation/motors/Motors.java b/src/main/java/org/team4159/lib/simulation/motors/Motors.java deleted file mode 100644 index c553b97..0000000 --- a/src/main/java/org/team4159/lib/simulation/motors/Motors.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.team4159.lib.simulation.motors; - -import org.team4159.lib.physics.DCMotorModel; - -// models of common FRC motors - -public class Motors { - private static final double TEST_VOLTAGE = 12; - - // From https://motors.vex.com/ - public static final DCMotorModel Falcon_500 = new DCMotorModel("Falcon 500", 6380, 1.5, 4.69, 257, TEST_VOLTAGE); - public static final DCMotorModel NEO = new DCMotorModel("NEO", 5880, 1.3, 3.36, 166, TEST_VOLTAGE); - public static final DCMotorModel CIM = new DCMotorModel("CIM", 5330, 2.7, 2.41, 131, TEST_VOLTAGE); - public static final DCMotorModel MiniCIM = new DCMotorModel("Mini CIM", 5840, 3, 1.41, 89, TEST_VOLTAGE); - public static final DCMotorModel BAG = new DCMotorModel("BAG", 13180, 1.8, 0.43, 53, TEST_VOLTAGE); - public static final DCMotorModel _775pro = new DCMotorModel("775pro", 18730, 0.7, 0.71, 134, TEST_VOLTAGE); - public static final DCMotorModel AndyMark_9015 = new DCMotorModel("AndyMark 9015", 14270, 3.7, 0.36, 71, TEST_VOLTAGE); - public static final DCMotorModel AndyMark_NeveRest = new DCMotorModel("AndyMark NeveRest", 5480, 0.4, 0.17, 10, TEST_VOLTAGE); - public static final DCMotorModel AndyMark_RS775_125 = new DCMotorModel("AndyMark RS775-125", 5800, 1.6, 0.28, 18, TEST_VOLTAGE); - public static final DCMotorModel BaneBots_RS775_18V = new DCMotorModel("BaneBots RS-775 18V", 13050, 2.7, 0.72, 97, TEST_VOLTAGE); - public static final DCMotorModel BaneBots_RS_550 = new DCMotorModel("BaneBots RS-550", 19000, 0.4, 0.38, 84, TEST_VOLTAGE); - - // From http://www.revrobotics.com/content/docs/REV-21-1651-DS.pdf - public static final DCMotorModel NEO_550 = new DCMotorModel("NEO 550", 11000, 1.4, 0.97, 100, TEST_VOLTAGE); -} diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java new file mode 100644 index 0000000..8a8aeb6 --- /dev/null +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -0,0 +1,109 @@ +package org.team4159.frc.robot.subsystems; + +import edu.wpi.first.wpilibj.TimedRobot; +import org.junit.Assert; +import org.junit.Before; +import org.junit.Test; + +import edu.wpi.first.wpilibj.util.Units; + +import org.team4159.lib.math.Conversions; +import org.team4159.lib.math.Epsilon; +import org.team4159.lib.simulation.RelativeEncoderMock; +import org.team4159.lib.simulation.MotorModels; +import org.team4159.lib.math.physics.DCMotorModel; + +import org.team4159.frc.robot.controllers.ArmController; + +import static org.team4159.lib.simulation.SimulationConfiguration.*; +import static org.team4159.frc.robot.Constants.*; + +public class ArmTest { + public class MockArm extends Arm { + private double inertia = Conversions.poundsToKilograms(3) * Math.pow(Units.inchesToMeters(8), 2); // kg m^2 + private DCMotorModel motor; + + private double applied_voltage = 0; + private double angular_velocity = 0; // rad / s + private RelativeEncoderMock encoder; + + MockArm() { + encoder = new RelativeEncoderMock(); + motor = MotorModels.NEO_550; + } + + void simulate(double dt) { + double torque = motor.get_torque_for_speed(angular_velocity, applied_voltage); + double angular_acceleration = torque / inertia; + angular_velocity += angular_acceleration * LOSS_COEFFICIENT * dt; + encoder.move(ARM_CONSTANTS.GEARING.COUNTS_PER_RADIAN * angular_velocity * dt); + } + + void setRealStartingPosition(int position) { + encoder.setRealPosition(position); + } + + double getRealPosition() { + return encoder.getRealPosition(); + } + + double getAppliedVoltage() { + return applied_voltage; + } + + double getAngularVelocity() { + return angular_velocity; + } + + @Override + public void setRawSpeed(double speed) { + setRawVoltage(speed * 12); + } + + @Override + public void setRawVoltage(double voltage) { + applied_voltage = voltage; + } + + @Override + public void zeroEncoder() { + encoder.setEncoderPosition(0); + } + + @Override + public int getPosition() { + return (int) encoder.getEncoderPosition(); + } + + @Override + public boolean isLimitSwitchClosed() { + return encoder.getRealPosition() <= 0; + } + } + + private MockArm arm; + private ArmController arm_controller; + + @Before + public void reset() { + arm = new MockArm(); + arm_controller = new ArmController(arm); + } + + private void simulateTime(double time) { + while (time > 0) { + if (Epsilon.epsilonEquals(time % TimedRobot.kDefaultPeriod, 0)) { + arm_controller.update(); + } + arm.simulate(SIMULATION_TIMESTEP); + time -= SIMULATION_TIMESTEP; + } + } + + @Test + public void Zeroes() { + arm.setRealStartingPosition(100); + simulateTime(1); + Assert.assertEquals(0, arm.getRealPosition(), Epsilon.kEpsilon); + } +} diff --git a/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java b/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java index 9319526..234a31b 100644 --- a/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java +++ b/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java @@ -11,6 +11,7 @@ import org.team4159.lib.control.signal.filters.LowPassFilter; import org.team4159.lib.logging.CSVWriter; import org.team4159.lib.math.Cardinal; +import org.team4159.lib.math.Epsilon; public class TestLowPassFilter { @Test @@ -18,7 +19,7 @@ public void TestDoesNothingWhenSmoothingIs1() { LowPassFilter filter = new LowPassFilter(0, 1, 1); for (int i = 0; i < 10; i++) { - Assert.assertEquals(i, filter.calculate(i), Cardinal.kEpsilon); + Assert.assertEquals(i, filter.calculate(i), Epsilon.kEpsilon); } } From e32dd5a080410b2758f12dc33f22b66f661f9e68 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Tue, 18 Feb 2020 22:36:18 -0800 Subject: [PATCH 06/21] something's still a little off --- .../lib/math/physics/DCMotorModel.java | 27 ++++++++++++------- ...tTrajectories.java => TrajectoryTest.java} | 2 +- .../frc/robot/subsystems/ArmTest.java | 9 ++++--- ...PassFilter.java => LowPassFilterTest.java} | 2 +- 4 files changed, 25 insertions(+), 15 deletions(-) rename src/test/java/org/team4159/frc/robot/{TestTrajectories.java => TrajectoryTest.java} (97%) rename src/test/java/org/team4159/lib/control/signal/{TestLowPassFilter.java => LowPassFilterTest.java} (98%) diff --git a/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java index c19aa1d..87d0299 100644 --- a/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java +++ b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java @@ -1,6 +1,12 @@ package org.team4159.lib.math.physics; // model of a DC motor +/* + * Adapted from: Team 254 + * https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/lib/physics/DCMotorTransmission.java + */ + +import org.team4159.lib.math.Epsilon; public class DCMotorModel { public final String name; @@ -23,29 +29,32 @@ public DCMotorModel(String name, this.test_voltage = test_voltage; } - // derived constants + // https://en.wikipedia.org/wiki/Motor_constants public double resistance() { return test_voltage / stall_current; // V = IR -> R = V/I } - public double voltage_constant() { + // "velocity constant" + public double volt_per_speed() { return free_speed / (test_voltage - free_current * resistance()); // voltage dropped at free speed = free_current * resistance } - public double back_emf_constant() { - return 1 / voltage_constant(); + // "back-EMF constant" + public double speed_per_volt() { + return 1 / volt_per_speed(); } - public double torque_constant() { + // "torque constant" + public double torque_per_amp() { return stall_torque / stall_current; } - public double get_current_for_speed(final double rpm, final double voltage) { - return (voltage - rpm * back_emf_constant()) / resistance(); + public double torque_per_volt() { + return torque_per_amp() / resistance(); } - public double get_torque_for_speed(final double rpm, final double voltage) { - return torque_constant() * get_current_for_speed(rpm, voltage); + public double getTorqueForVoltage(final double output_speed, final double voltage) { + return torque_per_volt() * (voltage - output_speed / speed_per_volt()); } } diff --git a/src/test/java/org/team4159/frc/robot/TestTrajectories.java b/src/test/java/org/team4159/frc/robot/TrajectoryTest.java similarity index 97% rename from src/test/java/org/team4159/frc/robot/TestTrajectories.java rename to src/test/java/org/team4159/frc/robot/TrajectoryTest.java index c6e0194..404ed6b 100644 --- a/src/test/java/org/team4159/frc/robot/TestTrajectories.java +++ b/src/test/java/org/team4159/frc/robot/TrajectoryTest.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.trajectory.Trajectory; import org.team4159.lib.logging.CSVWriter; -public class TestTrajectories { +public class TrajectoryTest { @Test public void TestTrajectoryGeneration() { Field[] fields = Trajectories.class.getFields(); diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index 8a8aeb6..c465567 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -33,7 +33,7 @@ public class MockArm extends Arm { } void simulate(double dt) { - double torque = motor.get_torque_for_speed(angular_velocity, applied_voltage); + double torque = motor.getTorqueForVoltage(angular_velocity, applied_voltage); double angular_acceleration = torque / inertia; angular_velocity += angular_acceleration * LOSS_COEFFICIENT * dt; encoder.move(ARM_CONSTANTS.GEARING.COUNTS_PER_RADIAN * angular_velocity * dt); @@ -92,7 +92,8 @@ public void reset() { private void simulateTime(double time) { while (time > 0) { - if (Epsilon.epsilonEquals(time % TimedRobot.kDefaultPeriod, 0)) { + if (Epsilon.epsilonEquals(time % TimedRobot.kDefaultPeriod, 0, 1E-3)) { + // System.out.println(arm.getRealPosition() + ", " + arm.getAppliedVoltage() + ", " + arm.getPosition()); arm_controller.update(); } arm.simulate(SIMULATION_TIMESTEP); @@ -103,7 +104,7 @@ private void simulateTime(double time) { @Test public void Zeroes() { arm.setRealStartingPosition(100); - simulateTime(1); - Assert.assertEquals(0, arm.getRealPosition(), Epsilon.kEpsilon); + simulateTime(5); + Assert.assertEquals(0, arm.getRealPosition(), ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); } } diff --git a/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java b/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java similarity index 98% rename from src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java rename to src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java index 234a31b..7adc24d 100644 --- a/src/test/java/org/team4159/lib/control/signal/TestLowPassFilter.java +++ b/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java @@ -13,7 +13,7 @@ import org.team4159.lib.math.Cardinal; import org.team4159.lib.math.Epsilon; -public class TestLowPassFilter { +public class LowPassFilterTest { @Test public void TestDoesNothingWhenSmoothingIs1() { LowPassFilter filter = new LowPassFilter(0, 1, 1); From 3ed0e0b9eb1536ba8d4b161413912a5dd29a6bd2 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Wed, 19 Feb 2020 00:35:07 -0800 Subject: [PATCH 07/21] minor reorganization --- .../controllers/TrajectoryController.java | 7 +++++- .../lib/control/signal/SignalUtil.java | 3 ++- .../control/signal/filters/LowPassFilter.java | 2 +- .../java/org/team4159/lib/math/Cardinal.java | 7 ------ .../java/org/team4159/lib/math/Epsilon.java | 18 -------------- src/main/java/org/team4159/lib/math/Util.java | 24 +++++++++++++++++++ 6 files changed, 33 insertions(+), 28 deletions(-) delete mode 100644 src/main/java/org/team4159/lib/math/Cardinal.java delete mode 100644 src/main/java/org/team4159/lib/math/Epsilon.java create mode 100644 src/main/java/org/team4159/lib/math/Util.java diff --git a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java index 4af20d8..34724ee 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java @@ -34,7 +34,7 @@ private enum State { private PIDController pid_right = new PIDController(Constants.DRIVE_CONSTANTS.kP, 0, Constants.DRIVE_CONSTANTS.kD); private Timer timer = new Timer(); - double prev_time = 0; + private double prev_time = 0; public TrajectoryController(Drivetrain drivetrain) { this.drivetrain = drivetrain; @@ -45,6 +45,11 @@ public void update() { switch (state) { case FOLLOWING: + if (timer.get() > trajectory_to_follow.getTotalTimeSeconds()) { + state = State.IDLE; + break; + } + double cur_time = timer.get(); double dt = cur_time - prev_time; diff --git a/src/main/java/org/team4159/lib/control/signal/SignalUtil.java b/src/main/java/org/team4159/lib/control/signal/SignalUtil.java index ac7dc1a..30819ec 100644 --- a/src/main/java/org/team4159/lib/control/signal/SignalUtil.java +++ b/src/main/java/org/team4159/lib/control/signal/SignalUtil.java @@ -1,6 +1,7 @@ package org.team4159.lib.control.signal; import edu.wpi.first.wpilibj.RobotController; +import org.team4159.lib.math.Util; public class SignalUtil { private SignalUtil() { @@ -8,7 +9,7 @@ private SignalUtil() { } public static double clampToPercent(double value) { - return Math.max(-1, Math.min(value, 1)); + return Util.clamp(-1, 1, value); } public static double squareWithSign(double n) { diff --git a/src/main/java/org/team4159/lib/control/signal/filters/LowPassFilter.java b/src/main/java/org/team4159/lib/control/signal/filters/LowPassFilter.java index c2aa964..14bf74f 100644 --- a/src/main/java/org/team4159/lib/control/signal/filters/LowPassFilter.java +++ b/src/main/java/org/team4159/lib/control/signal/filters/LowPassFilter.java @@ -1,6 +1,6 @@ package org.team4159.lib.control.signal.filters; - import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.TimedRobot; public class LowPassFilter { private double smoothed, smoothing, period; diff --git a/src/main/java/org/team4159/lib/math/Cardinal.java b/src/main/java/org/team4159/lib/math/Cardinal.java deleted file mode 100644 index 212edb5..0000000 --- a/src/main/java/org/team4159/lib/math/Cardinal.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.team4159.lib.math; - -// car·​di·​nal /ˈkärd-nəl/ adjective 1. of basic importance - -public class Cardinal { - public static double kTau = Math.PI * 2; -} diff --git a/src/main/java/org/team4159/lib/math/Epsilon.java b/src/main/java/org/team4159/lib/math/Epsilon.java deleted file mode 100644 index de8d249..0000000 --- a/src/main/java/org/team4159/lib/math/Epsilon.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.team4159.lib.math; - -/* -* Adapted from: Team 254 -* https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/lib/util/Util.java -*/ - -public class Epsilon { - public static double kEpsilon = 1E-12; - - public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); - } - - public static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, kEpsilon); - } -} diff --git a/src/main/java/org/team4159/lib/math/Util.java b/src/main/java/org/team4159/lib/math/Util.java new file mode 100644 index 0000000..5990f65 --- /dev/null +++ b/src/main/java/org/team4159/lib/math/Util.java @@ -0,0 +1,24 @@ +package org.team4159.lib.math; + +public class Util { + public static double kTau = Math.PI * 2; + + public static double clamp(double low, double high, double a) { + return Math.max(low, Math.min(a, high)); + } + + /* + * Adapted from: Team 254 + * https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/lib/util/Util.java + */ + + public static double kEpsilon = 1E-12; + + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } +} From c2c74e7744635b7ebdc37f2e94a91da426493ad2 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Wed, 19 Feb 2020 00:37:40 -0800 Subject: [PATCH 08/21] fix build --- src/main/java/org/team4159/frc/robot/Constants.java | 4 ++-- src/main/java/org/team4159/lib/hardware/Gearing.java | 4 ++-- .../java/org/team4159/lib/math/physics/DCMotorModel.java | 2 -- src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java | 4 ++-- .../org/team4159/lib/control/signal/LowPassFilterTest.java | 5 ++--- 5 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/team4159/frc/robot/Constants.java b/src/main/java/org/team4159/frc/robot/Constants.java index 4417c9f..c536054 100644 --- a/src/main/java/org/team4159/frc/robot/Constants.java +++ b/src/main/java/org/team4159/frc/robot/Constants.java @@ -8,7 +8,7 @@ import org.team4159.lib.hardware.Gearing; import org.team4159.lib.hardware.joystick.T16000M; -import org.team4159.lib.math.Cardinal; +import org.team4159.lib.math.Util; public final class Constants { // not sure where to put these @@ -75,7 +75,7 @@ public static final class CAN_IDS { public static final class DRIVE_CONSTANTS { public static final Gearing GEARING = new Gearing(8.48, ENCODERS.FALCON_CPR); public static final double WHEEL_RADIUS = Units.inchesToMeters(3.0); - public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * Cardinal.kTau; + public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * Util.kTau; public static final double METERS_PER_COUNT = WHEEL_CIRCUMFERENCE / GEARING.COUNTS_PER_REV; diff --git a/src/main/java/org/team4159/lib/hardware/Gearing.java b/src/main/java/org/team4159/lib/hardware/Gearing.java index aa9b13c..613c9c9 100644 --- a/src/main/java/org/team4159/lib/hardware/Gearing.java +++ b/src/main/java/org/team4159/lib/hardware/Gearing.java @@ -1,6 +1,6 @@ package org.team4159.lib.hardware; -import org.team4159.lib.math.Cardinal; +import org.team4159.lib.math.Util; public class Gearing { public int COUNTS_PER_REV; @@ -9,6 +9,6 @@ public class Gearing { public Gearing(double gear_ratio, int encoder_cpr) { COUNTS_PER_REV = (int) (gear_ratio * encoder_cpr); COUNTS_PER_DEGREE = COUNTS_PER_REV / 360.0; - COUNTS_PER_RADIAN = COUNTS_PER_REV / Cardinal.kTau; + COUNTS_PER_RADIAN = COUNTS_PER_REV / Util.kTau; } } diff --git a/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java index 87d0299..f9c0aeb 100644 --- a/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java +++ b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java @@ -6,8 +6,6 @@ * https://github.com/Team254/FRC-2018-Public/blob/master/src/main/java/com/team254/lib/physics/DCMotorTransmission.java */ -import org.team4159.lib.math.Epsilon; - public class DCMotorModel { public final String name; public final double diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index c465567..c81494d 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.util.Units; import org.team4159.lib.math.Conversions; -import org.team4159.lib.math.Epsilon; +import org.team4159.lib.math.Util; import org.team4159.lib.simulation.RelativeEncoderMock; import org.team4159.lib.simulation.MotorModels; import org.team4159.lib.math.physics.DCMotorModel; @@ -92,7 +92,7 @@ public void reset() { private void simulateTime(double time) { while (time > 0) { - if (Epsilon.epsilonEquals(time % TimedRobot.kDefaultPeriod, 0, 1E-3)) { + if (Util.epsilonEquals(time % TimedRobot.kDefaultPeriod, 0, 1E-3)) { // System.out.println(arm.getRealPosition() + ", " + arm.getAppliedVoltage() + ", " + arm.getPosition()); arm_controller.update(); } diff --git a/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java b/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java index 7adc24d..c9b347d 100644 --- a/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java +++ b/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java @@ -10,8 +10,7 @@ import org.team4159.lib.control.signal.filters.LowPassFilter; import org.team4159.lib.logging.CSVWriter; -import org.team4159.lib.math.Cardinal; -import org.team4159.lib.math.Epsilon; +import org.team4159.lib.math.Util; public class LowPassFilterTest { @Test @@ -19,7 +18,7 @@ public void TestDoesNothingWhenSmoothingIs1() { LowPassFilter filter = new LowPassFilter(0, 1, 1); for (int i = 0; i < 10; i++) { - Assert.assertEquals(i, filter.calculate(i), Epsilon.kEpsilon); + Assert.assertEquals(i, filter.calculate(i), Util.kEpsilon); } } From 2749b198c9bd80c60f38a2ae11750010ce222b0e Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Wed, 19 Feb 2020 00:55:30 -0800 Subject: [PATCH 09/21] make SimulationRunner --- .../org/team4159/frc/robot/Constants.java | 4 ++-- .../team4159/frc/robot/RobotContainer.java | 2 +- .../frc/robot/controllers/ArmController.java | 3 ++- .../frc/robot/controllers/IntakeManager.java | 11 ++++----- .../lib/control/signal/SignalUtil.java | 4 ++-- .../org/team4159/lib/hardware/Gearing.java | 4 ++-- .../lib/math/{Util.java => MathUtil.java} | 2 +- .../simulation/SimulationConfiguration.java | 3 +++ .../lib/simulation/SimulationRunner.java | 20 ++++++++++++++++ .../{ => mocks}/RelativeEncoderMock.java | 2 +- .../lib/simulation/mocks/SubsystemMock.java | 5 ++++ .../frc/robot/subsystems/ArmTest.java | 23 +++++-------------- .../lib/control/signal/LowPassFilterTest.java | 4 ++-- 13 files changed, 52 insertions(+), 35 deletions(-) rename src/main/java/org/team4159/lib/math/{Util.java => MathUtil.java} (96%) create mode 100644 src/main/java/org/team4159/lib/simulation/SimulationRunner.java rename src/main/java/org/team4159/lib/simulation/{ => mocks}/RelativeEncoderMock.java (94%) create mode 100644 src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java diff --git a/src/main/java/org/team4159/frc/robot/Constants.java b/src/main/java/org/team4159/frc/robot/Constants.java index 7a19bc2..db7e874 100644 --- a/src/main/java/org/team4159/frc/robot/Constants.java +++ b/src/main/java/org/team4159/frc/robot/Constants.java @@ -8,7 +8,7 @@ import org.team4159.lib.hardware.Gearing; import org.team4159.lib.hardware.joystick.T16000M; -import org.team4159.lib.math.Util; +import org.team4159.lib.math.MathUtil; public final class Constants { // not sure where to put these @@ -75,7 +75,7 @@ public static final class CAN_IDS { public static final class DRIVE_CONSTANTS { public static final Gearing GEARING = new Gearing(8.48, ENCODERS.FALCON_CPR); public static final double WHEEL_RADIUS = Units.inchesToMeters(3.0); - public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * Util.kTau; + public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * MathUtil.kTau; public static final double METERS_PER_COUNT = WHEEL_CIRCUMFERENCE / GEARING.COUNTS_PER_REV; diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index 9ed03b3..4f46eb8 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -22,7 +22,7 @@ public class RobotContainer { private final Arm arm = new Arm(); private final Turret turret = new Turret(limelight); - private final IntakeManager intake_controller = new IntakeManager(arm, intake, feeder); + private final IntakeManager intake_controller = new IntakeManager(arm.getController(), intake, feeder); private final Joystick left_joy = new Joystick(CONTROLS.LEFT_JOY.USB_PORT); private final Joystick right_joy = new Joystick(CONTROLS.RIGHT_JOY.USB_PORT); diff --git a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java index 50b0301..b93dc90 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java @@ -3,10 +3,11 @@ import edu.wpi.first.wpilibj.controller.PIDController; import org.team4159.frc.robot.subsystems.Arm; +import org.team4159.lib.control.ControlLoop; import static org.team4159.frc.robot.Constants.*; -public class ArmController { +public class ArmController implements ControlLoop { private enum State { ZEROING, CLOSED_LOOP, diff --git a/src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java b/src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java index b9e13c9..cba66a4 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java @@ -1,6 +1,5 @@ package org.team4159.frc.robot.controllers; -import org.team4159.frc.robot.subsystems.Arm; import org.team4159.frc.robot.subsystems.Feeder; import org.team4159.frc.robot.subsystems.Intake; @@ -14,12 +13,12 @@ private enum State { } private State state; - private Arm arm; + private ArmController arm_controller; private Intake intake; private Feeder feeder; - public IntakeManager(Arm arm, Intake intake, Feeder feeder) { - this.arm = arm; + public IntakeManager(ArmController arm, Intake intake, Feeder feeder) { + this.arm_controller = arm_controller; this.intake = intake; this.feeder = feeder; } @@ -34,7 +33,7 @@ public void update() { break; case DEPLOYING: arm_setpoint = ARM_CONSTANTS.DOWN_POSITION; - if (arm.isAtSetpoint()) { + if (arm_controller.isAtSetpoint()) { state = State.INTAKING; } break; @@ -45,7 +44,7 @@ public void update() { break; } - arm.setSetpoint(arm_setpoint); + arm_controller.setSetpoint(arm_setpoint); intake.setRawSpeed(intake_speed); feeder.setRawSpeed(feeder_speed); } diff --git a/src/main/java/org/team4159/lib/control/signal/SignalUtil.java b/src/main/java/org/team4159/lib/control/signal/SignalUtil.java index 30819ec..48840de 100644 --- a/src/main/java/org/team4159/lib/control/signal/SignalUtil.java +++ b/src/main/java/org/team4159/lib/control/signal/SignalUtil.java @@ -1,7 +1,7 @@ package org.team4159.lib.control.signal; import edu.wpi.first.wpilibj.RobotController; -import org.team4159.lib.math.Util; +import org.team4159.lib.math.MathUtil; public class SignalUtil { private SignalUtil() { @@ -9,7 +9,7 @@ private SignalUtil() { } public static double clampToPercent(double value) { - return Util.clamp(-1, 1, value); + return MathUtil.clamp(-1, 1, value); } public static double squareWithSign(double n) { diff --git a/src/main/java/org/team4159/lib/hardware/Gearing.java b/src/main/java/org/team4159/lib/hardware/Gearing.java index 613c9c9..465d93e 100644 --- a/src/main/java/org/team4159/lib/hardware/Gearing.java +++ b/src/main/java/org/team4159/lib/hardware/Gearing.java @@ -1,6 +1,6 @@ package org.team4159.lib.hardware; -import org.team4159.lib.math.Util; +import org.team4159.lib.math.MathUtil; public class Gearing { public int COUNTS_PER_REV; @@ -9,6 +9,6 @@ public class Gearing { public Gearing(double gear_ratio, int encoder_cpr) { COUNTS_PER_REV = (int) (gear_ratio * encoder_cpr); COUNTS_PER_DEGREE = COUNTS_PER_REV / 360.0; - COUNTS_PER_RADIAN = COUNTS_PER_REV / Util.kTau; + COUNTS_PER_RADIAN = COUNTS_PER_REV / MathUtil.kTau; } } diff --git a/src/main/java/org/team4159/lib/math/Util.java b/src/main/java/org/team4159/lib/math/MathUtil.java similarity index 96% rename from src/main/java/org/team4159/lib/math/Util.java rename to src/main/java/org/team4159/lib/math/MathUtil.java index 5990f65..7f7f908 100644 --- a/src/main/java/org/team4159/lib/math/Util.java +++ b/src/main/java/org/team4159/lib/math/MathUtil.java @@ -1,6 +1,6 @@ package org.team4159.lib.math; -public class Util { +public class MathUtil { public static double kTau = Math.PI * 2; public static double clamp(double low, double high, double a) { diff --git a/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java b/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java index 29a1907..5a7545d 100644 --- a/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java +++ b/src/main/java/org/team4159/lib/simulation/SimulationConfiguration.java @@ -1,6 +1,9 @@ package org.team4159.lib.simulation; +import edu.wpi.first.wpilibj.TimedRobot; + public class SimulationConfiguration { public static double SIMULATION_TIMESTEP = 0.001; // seconds + public static double CONTROL_LOOP_TIMESTEP = TimedRobot.kDefaultPeriod; // seconds public static double LOSS_COEFFICIENT = 0.8; } diff --git a/src/main/java/org/team4159/lib/simulation/SimulationRunner.java b/src/main/java/org/team4159/lib/simulation/SimulationRunner.java new file mode 100644 index 0000000..62e6800 --- /dev/null +++ b/src/main/java/org/team4159/lib/simulation/SimulationRunner.java @@ -0,0 +1,20 @@ +package org.team4159.lib.simulation; + +import org.team4159.lib.control.ControlLoop; +import org.team4159.lib.math.MathUtil; +import org.team4159.lib.simulation.mocks.SubsystemMock; + +import static org.team4159.lib.simulation.SimulationConfiguration.*; + +public class SimulationRunner { + // runs a simulated subsystem at a short timestep alongside a control loop at the usual timestep + public static void simulate(SubsystemMock subsystem_mock, ControlLoop control_loop, double time) { + while (time > 0) { + if (MathUtil.epsilonEquals(time % CONTROL_LOOP_TIMESTEP, 0, 1E-3)) { + control_loop.update(); + } + subsystem_mock.simulate(SIMULATION_TIMESTEP); + time -= SIMULATION_TIMESTEP; + } + } +} diff --git a/src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java b/src/main/java/org/team4159/lib/simulation/mocks/RelativeEncoderMock.java similarity index 94% rename from src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java rename to src/main/java/org/team4159/lib/simulation/mocks/RelativeEncoderMock.java index b215635..47aea08 100644 --- a/src/main/java/org/team4159/lib/simulation/RelativeEncoderMock.java +++ b/src/main/java/org/team4159/lib/simulation/mocks/RelativeEncoderMock.java @@ -1,4 +1,4 @@ -package org.team4159.lib.simulation; +package org.team4159.lib.simulation.mocks; // model of a relative encoder and the transmission's real position // uses doubles over ints to allow for simulation over small timesteps but it's kind of iffy diff --git a/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java b/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java new file mode 100644 index 0000000..e853a37 --- /dev/null +++ b/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java @@ -0,0 +1,5 @@ +package org.team4159.lib.simulation.mocks; + +public interface SubsystemMock { + void simulate(double dt); +} diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index 52111bf..4cbaac0 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -1,6 +1,5 @@ package org.team4159.frc.robot.subsystems; -import edu.wpi.first.wpilibj.TimedRobot; import org.junit.Assert; import org.junit.Before; import org.junit.Test; @@ -8,8 +7,9 @@ import edu.wpi.first.wpilibj.util.Units; import org.team4159.lib.math.Conversions; -import org.team4159.lib.math.Util; -import org.team4159.lib.simulation.RelativeEncoderMock; +import org.team4159.lib.simulation.SimulationRunner; +import org.team4159.lib.simulation.mocks.RelativeEncoderMock; +import org.team4159.lib.simulation.mocks.SubsystemMock; import org.team4159.lib.simulation.MotorModels; import org.team4159.lib.math.physics.DCMotorModel; import org.team4159.frc.robot.controllers.ArmController; @@ -18,7 +18,7 @@ import static org.team4159.frc.robot.Constants.*; public class ArmTest { - public class MockArm extends Arm { + public class MockArm extends Arm implements SubsystemMock { private double inertia = Conversions.poundsToKilograms(3) * Math.pow(Units.inchesToMeters(8), 2); // kg m^2 private DCMotorModel motor; @@ -31,7 +31,7 @@ public class MockArm extends Arm { motor = MotorModels.NEO_550; } - void simulate(double dt) { + public void simulate(double dt) { double torque = motor.getTorqueForVoltage(angular_velocity, applied_voltage); double angular_acceleration = torque / inertia; angular_velocity += angular_acceleration * LOSS_COEFFICIENT * dt; @@ -89,21 +89,10 @@ public void reset() { arm_controller = new ArmController(arm); } - private void simulateTime(double time) { - while (time > 0) { - if (Util.epsilonEquals(time % TimedRobot.kDefaultPeriod, 0, 1E-3)) { - // System.out.println(arm.getRealPosition() + ", " + arm.getAppliedVoltage() + ", " + arm.getPosition()); - arm_controller.update(); - } - arm.simulate(SIMULATION_TIMESTEP); - time -= SIMULATION_TIMESTEP; - } - } - @Test public void Zeroes() { arm.setRealStartingPosition(100); - simulateTime(5); + SimulationRunner.simulate(arm, arm_controller, 5); Assert.assertEquals(0, arm.getRealPosition(), ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); } } diff --git a/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java b/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java index c9b347d..79bc170 100644 --- a/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java +++ b/src/test/java/org/team4159/lib/control/signal/LowPassFilterTest.java @@ -10,7 +10,7 @@ import org.team4159.lib.control.signal.filters.LowPassFilter; import org.team4159.lib.logging.CSVWriter; -import org.team4159.lib.math.Util; +import org.team4159.lib.math.MathUtil; public class LowPassFilterTest { @Test @@ -18,7 +18,7 @@ public void TestDoesNothingWhenSmoothingIs1() { LowPassFilter filter = new LowPassFilter(0, 1, 1); for (int i = 0; i < 10; i++) { - Assert.assertEquals(i, filter.calculate(i), Util.kEpsilon); + Assert.assertEquals(i, filter.calculate(i), MathUtil.kEpsilon); } } From 1526808eb4162270cd94bb3aaad947121043cd25 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Wed, 19 Feb 2020 19:39:46 -0800 Subject: [PATCH 10/21] GoesToPosition test for Arm --- .../java/org/team4159/frc/robot/subsystems/ArmTest.java | 8 ++++++++ .../org/team4159/frc/robot/subsystems/ShooterTest.java | 5 +++++ 2 files changed, 13 insertions(+) create mode 100644 src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index 4cbaac0..3bacf2c 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -19,6 +19,7 @@ public class ArmTest { public class MockArm extends Arm implements SubsystemMock { + // arm model is really slow, i think the inertia / gearing are off private double inertia = Conversions.poundsToKilograms(3) * Math.pow(Units.inchesToMeters(8), 2); // kg m^2 private DCMotorModel motor; @@ -95,4 +96,11 @@ public void Zeroes() { SimulationRunner.simulate(arm, arm_controller, 5); Assert.assertEquals(0, arm.getRealPosition(), ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); } + + @Test + public void GoesToPosition() { + arm_controller.setSetpoint(ARM_CONSTANTS.DOWN_POSITION); + SimulationRunner.simulate(arm, arm_controller, 10); + Assert.assertTrue(arm_controller.isAtSetpoint()); + } } diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java new file mode 100644 index 0000000..ae25771 --- /dev/null +++ b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java @@ -0,0 +1,5 @@ +package org.team4159.frc.robot.subsystems; + +public class ShooterTest { + +} From d5dcb1e7877acc84b48a7e084da31d02bbed45a3 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Wed, 19 Feb 2020 20:16:06 -0800 Subject: [PATCH 11/21] use interfaces --- .../team4159/frc/robot/RobotContainer.java | 4 +- .../frc/robot/controllers/ArmController.java | 10 ++-- ...Manager.java => DrivetrainController.java} | 44 +++++++++----- .../robot/controllers/ShooterController.java | 10 ++-- .../robot/controllers/ShootingManager.java | 10 ++-- .../robot/controllers/TurretController.java | 10 ++-- .../team4159/frc/robot/subsystems/Arm.java | 7 +-- .../frc/robot/subsystems/Drivetrain.java | 58 ++++--------------- .../team4159/frc/robot/subsystems/Feeder.java | 2 +- .../team4159/frc/robot/subsystems/IArm.java | 9 +++ .../frc/robot/subsystems/IDrivetrain.java | 22 +++++++ .../frc/robot/subsystems/IFeeder.java | 7 +++ .../frc/robot/subsystems/IIntake.java | 7 +++ .../team4159/frc/robot/subsystems/INeck.java | 7 +++ .../frc/robot/subsystems/IShooter.java | 10 ++++ .../frc/robot/subsystems/ITurret.java | 13 +++++ .../team4159/frc/robot/subsystems/Intake.java | 5 +- .../team4159/frc/robot/subsystems/Neck.java | 5 +- .../frc/robot/subsystems/Shooter.java | 10 +--- .../team4159/frc/robot/subsystems/Turret.java | 3 +- .../{ControlLoop.java => IControlLoop.java} | 2 +- .../lib/simulation/SimulationRunner.java | 6 +- ...SubsystemMock.java => ISubsystemMock.java} | 2 +- .../frc/robot/subsystems/ArmTest.java | 9 +-- .../frc/robot/subsystems/ShooterTest.java | 28 +++++++++ 25 files changed, 181 insertions(+), 119 deletions(-) rename src/main/java/org/team4159/frc/robot/controllers/{TrajectoryManager.java => DrivetrainController.java} (73%) create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IArm.java create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IIntake.java create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/INeck.java create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IShooter.java create mode 100644 src/main/java/org/team4159/frc/robot/subsystems/ITurret.java rename src/main/java/org/team4159/lib/control/{ControlLoop.java => IControlLoop.java} (62%) rename src/main/java/org/team4159/lib/simulation/mocks/{SubsystemMock.java => ISubsystemMock.java} (68%) diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index 4f46eb8..7d08bb4 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -73,10 +73,10 @@ public void updateArmInputs() { public void updateDrivetrainInputs() { if (secondary_joy.getRawButtonPressed(CONTROLS.SECONDARY_JOY.BUTTON_IDS.FLIP_ROBOT_ORIENTATION)) { - drivetrain.flipOrientation(); + drivetrain.getController().flipDriveOrientation(); } - drivetrain.drive(new DriveSignal(left_joy.getY(), right_joy.getY())); + drivetrain.getController().setDriveSignal(new DriveSignal(left_joy.getY(), right_joy.getY())); } public void updateFeederInputs() { diff --git a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java index b93dc90..2049aef 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java @@ -2,12 +2,12 @@ import edu.wpi.first.wpilibj.controller.PIDController; -import org.team4159.frc.robot.subsystems.Arm; -import org.team4159.lib.control.ControlLoop; +import org.team4159.frc.robot.subsystems.IArm; +import org.team4159.lib.control.IControlLoop; import static org.team4159.frc.robot.Constants.*; -public class ArmController implements ControlLoop { +public class ArmController implements IControlLoop { private enum State { ZEROING, CLOSED_LOOP, @@ -15,11 +15,11 @@ private enum State { } private State state = State.ZEROING; - private Arm arm; + private IArm arm; private PIDController pid_controller; - public ArmController(Arm arm) { + public ArmController(IArm arm) { this.arm = arm; pid_controller = new PIDController( diff --git a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java b/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java similarity index 73% rename from src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java rename to src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java index c192d47..b36b99c 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java @@ -11,38 +11,40 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj.trajectory.Trajectory; -import org.team4159.frc.robot.Constants; -import org.team4159.frc.robot.subsystems.Drivetrain; -import org.team4159.lib.control.ControlLoop; +import org.team4159.frc.robot.subsystems.IDrivetrain; import org.team4159.lib.control.signal.DriveSignal; -public class TrajectoryManager { +import static org.team4159.frc.robot.Constants.*; + +public class DrivetrainController { private enum State { FOLLOWING, IDLE } private State state = State.IDLE; - private Drivetrain drivetrain; + private IDrivetrain drivetrain; private Trajectory trajectory_to_follow; + private DriveSignal demanded_signal; + private boolean is_oriented_forwards = true; private DifferentialDriveWheelSpeeds prev_speeds = new DifferentialDriveWheelSpeeds(0,0); - private DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.DRIVE_CONSTANTS.TRACK_WIDTH); - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.DRIVE_CONSTANTS.kS, Constants.DRIVE_CONSTANTS.kV, Constants.DRIVE_CONSTANTS.kA); - private RamseteController controller = new RamseteController(Constants.DRIVE_CONSTANTS.kB, Constants.DRIVE_CONSTANTS.kZeta); - private PIDController pid_left = new PIDController(Constants.DRIVE_CONSTANTS.kP, 0, Constants.DRIVE_CONSTANTS.kD); - private PIDController pid_right = new PIDController(Constants.DRIVE_CONSTANTS.kP, 0, Constants.DRIVE_CONSTANTS.kD); + private DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DRIVE_CONSTANTS.TRACK_WIDTH); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(DRIVE_CONSTANTS.kS, DRIVE_CONSTANTS.kV, DRIVE_CONSTANTS.kA); + private RamseteController controller = new RamseteController(DRIVE_CONSTANTS.kB, DRIVE_CONSTANTS.kZeta); + private PIDController pid_left = new PIDController(DRIVE_CONSTANTS.kP, 0, DRIVE_CONSTANTS.kD); + private PIDController pid_right = new PIDController(DRIVE_CONSTANTS.kP, 0, DRIVE_CONSTANTS.kD); private Timer timer = new Timer(); private double prev_time = 0; - public TrajectoryManager(Drivetrain drivetrain) { + public DrivetrainController(IDrivetrain drivetrain) { this.drivetrain = drivetrain; } public void update() { - DriveSignal signal = DriveSignal.NEUTRAL; + DriveSignal filtered_signal = DriveSignal.NEUTRAL; switch (state) { case FOLLOWING: @@ -80,20 +82,32 @@ public void update() { prev_time = cur_time; prev_speeds = target_wheel_speeds; - signal = DriveSignal.fromVolts(left_output, right_output); + filtered_signal = DriveSignal.fromVolts(left_output, right_output); break; case IDLE: - // shouldn't be getting called + filtered_signal = demanded_signal; break; } - drivetrain.drive(signal); + if (!is_oriented_forwards) { + filtered_signal = filtered_signal.invert(); + } + + drivetrain.drive(filtered_signal); } public boolean isIdle() { return state == State.IDLE; } + public void setDriveSignal(DriveSignal signal) { + this.demanded_signal = signal; + } + + public void flipDriveOrientation() { + is_oriented_forwards = !is_oriented_forwards; + } + public void setTrajectory(Trajectory trajectory) { state = State.FOLLOWING; diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java index 9bc72b5..7e53542 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java @@ -3,17 +3,17 @@ import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.team4159.frc.robot.Constants; -import org.team4159.frc.robot.subsystems.Shooter; -import org.team4159.lib.control.ControlLoop; +import org.team4159.frc.robot.subsystems.IShooter; +import org.team4159.lib.control.IControlLoop; import org.team4159.lib.hardware.Limelight; -public class ShooterController implements ControlLoop { +public class ShooterController implements IControlLoop { public enum State { IDLE, CLOSED_LOOP } - private Shooter shooter; + private IShooter shooter; private Limelight limelight; private State state = State.IDLE; @@ -23,7 +23,7 @@ public enum State { Constants.SHOOTER_CONSTANTS.kD ); - public ShooterController(Shooter shooter) { + public ShooterController(IShooter shooter) { this.shooter = shooter; this.limelight = shooter.getLimelight(); diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java b/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java index 53de402..8599f0a 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java @@ -1,22 +1,22 @@ package org.team4159.frc.robot.controllers; -import org.team4159.frc.robot.subsystems.Neck; -import org.team4159.lib.control.ControlLoop; +import org.team4159.frc.robot.subsystems.INeck; +import org.team4159.lib.control.IControlLoop; -public class ShootingManager implements ControlLoop { +public class ShootingManager implements IControlLoop { private enum State { IDLE, WAITING, SHOOTING } - private Neck neck; + private INeck neck; private TurretController turret_controller; private ShooterController shooter_controller; private State state = State.IDLE; - public ShootingManager(Neck neck, TurretController turret_controller, ShooterController shooter_controller) { + public ShootingManager(INeck neck, TurretController turret_controller, ShooterController shooter_controller) { this.neck = neck; this.turret_controller = turret_controller; this.shooter_controller = shooter_controller; diff --git a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java index 0850ba5..de3e05a 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java @@ -1,13 +1,13 @@ package org.team4159.frc.robot.controllers; import edu.wpi.first.wpilibj.controller.PIDController; -import org.team4159.frc.robot.subsystems.Turret; -import org.team4159.lib.control.ControlLoop; +import org.team4159.frc.robot.subsystems.ITurret; +import org.team4159.lib.control.IControlLoop; import org.team4159.lib.hardware.Limelight; import static org.team4159.frc.robot.Constants.*; -public class TurretController implements ControlLoop { +public class TurretController implements IControlLoop { public enum State { ZEROING, SEEKING_TARGET, @@ -16,7 +16,7 @@ public enum State { OPEN_LOOP } - private Turret turret; + private ITurret turret; private Limelight limelight; private int seeking_direction, seeking_range, seeking_starting_position; @@ -30,7 +30,7 @@ public enum State { TURRET_CONSTANTS.LIMELIGHT_TURN_kD ); - public TurretController(Turret turret) { + public TurretController(ITurret turret) { this.turret = turret; this.limelight = turret.getLimelight(); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java index 66198c2..56abca6 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.frc.robot.controllers.ArmController; @@ -12,7 +11,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Arm extends SubsystemBase { +public class Arm extends SubsystemBase implements IArm { private CANSparkMax arm_spark; private DigitalInput arm_limit_switch; @@ -21,10 +20,6 @@ public class Arm extends SubsystemBase { private ArmController arm_controller; public Arm() { - if (RobotBase.isSimulation()) { - return; - } - arm_spark = new CardinalMAX(CAN_IDS.ARM_SPARK, CANSparkMax.IdleMode.kBrake); arm_spark.setInverted(true); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index 466677e..dc15a24 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -1,33 +1,25 @@ package org.team4159.frc.robot.subsystems; import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; -import org.team4159.frc.robot.controllers.TrajectoryManager; +import org.team4159.frc.robot.controllers.DrivetrainController; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.control.signal.filters.LowPassFilterSource; import org.team4159.lib.hardware.controller.ctre.CardinalFX; import static org.team4159.frc.robot.Constants.*; -public class Drivetrain extends SubsystemBase { - private enum State { - PATH_FOLLOWING, - OPEN_LOOP - } - private State state = State.OPEN_LOOP; - +public class Drivetrain extends SubsystemBase implements IDrivetrain { private CardinalFX left_front_falcon, left_rear_falcon, right_front_falcon, right_rear_falcon; private SpeedControllerGroup left_falcons; private SpeedControllerGroup right_falcons; @@ -36,9 +28,7 @@ private enum State { private PigeonIMU pigeon; private LowPassFilterSource filtered_heading; - private TrajectoryManager trajectory_controller; - private DriveSignal drive_signal; - private boolean is_oriented_forward = true; + private DrivetrainController drivetrain_controller; public Drivetrain() { left_front_falcon = new CardinalFX(CAN_IDS.LEFT_FRONT_FALCON, NeutralMode.Coast); @@ -68,15 +58,11 @@ public Drivetrain() { odometry = new DifferentialDriveOdometry(new Rotation2d(0)); filtered_heading = new LowPassFilterSource(pigeon::getFusedHeading, 10); - trajectory_controller = new TrajectoryManager(this); + drivetrain_controller = new DrivetrainController(this); zeroSensors(); } - public void flipOrientation() { - is_oriented_forward = !is_oriented_forward; - } - @Override public void periodic() { odometry.update( @@ -85,37 +71,11 @@ public void periodic() { getRightDistance() ); filtered_heading.get(); - - SmartDashboard.putNumber("X", getPose().getTranslation().getX()); - SmartDashboard.putNumber("Y", getPose().getTranslation().getY()); - SmartDashboard.putNumber("Angle", getDirection()); - SmartDashboard.putNumber("Left Encoder", getLeftDistance()); - SmartDashboard.putNumber("Right Encoder", getRightDistance()); - - switch (state) { - case PATH_FOLLOWING: - trajectory_controller.update(); - if (trajectory_controller.isIdle()) { - state = State.OPEN_LOOP; - } - case OPEN_LOOP: - DriveSignal filtered_signal = drive_signal; - if (is_oriented_forward) { - filtered_signal = drive_signal.invert(); - } - left_falcons.set(filtered_signal.left); - right_falcons.set(filtered_signal.right); - break; - } } public void drive(DriveSignal drive_signal) { - this.drive_signal = drive_signal; - } - - public void followTrajectory(Trajectory trajectory) { - state = State.PATH_FOLLOWING; - trajectory_controller.setTrajectory(trajectory); + left_falcons.set(drive_signal.left); + right_falcons.set(drive_signal.right); } public void resetEncoders() { @@ -134,8 +94,6 @@ public void zeroSensors() { new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), Rotation2d.fromDegrees(0) ); - - System.out.println(getDirection()); } public DifferentialDriveWheelSpeeds getWheelSpeeds() { @@ -175,4 +133,8 @@ public double getRightVelocity() { public Pose2d getPose() { return odometry.getPoseMeters(); } + + public DrivetrainController getController() { + return drivetrain_controller; + } } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java index 8ce0ecb..4b1d626 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java @@ -11,7 +11,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Feeder extends SubsystemBase { +public class Feeder extends SubsystemBase implements IFeeder { private SpeedControllerGroup feeder_motors; public Feeder() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IArm.java b/src/main/java/org/team4159/frc/robot/subsystems/IArm.java new file mode 100644 index 0000000..ab30d44 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/IArm.java @@ -0,0 +1,9 @@ +package org.team4159.frc.robot.subsystems; + +public interface IArm { + void setRawSpeed(double speed); + void setRawVoltage(double voltage); + void zeroEncoder(); + int getPosition(); + boolean isLimitSwitchClosed(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java new file mode 100644 index 0000000..f725939 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java @@ -0,0 +1,22 @@ +package org.team4159.frc.robot.subsystems; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; + +import org.team4159.lib.control.signal.DriveSignal; + +public interface IDrivetrain { + void drive(DriveSignal drive_signal); + void resetEncoders(); + void resetDirection(); + void zeroSensors(); + DifferentialDriveWheelSpeeds getWheelSpeeds(); + double getDirection(); + double getLeftVoltage(); + double getRightVoltage(); + double getLeftDistance(); + double getLeftVelocity(); + double getRightDistance(); + double getRightVelocity(); + Pose2d getPose(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java b/src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java new file mode 100644 index 0000000..a374d59 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java @@ -0,0 +1,7 @@ +package org.team4159.frc.robot.subsystems; + +public interface IFeeder { + void setRawSpeed(double speed); + void feed(); + void stop(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IIntake.java b/src/main/java/org/team4159/frc/robot/subsystems/IIntake.java new file mode 100644 index 0000000..61c0d12 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/IIntake.java @@ -0,0 +1,7 @@ +package org.team4159.frc.robot.subsystems; + +public interface IIntake { + void setRawSpeed(double speed); + void intake(); + void stop(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/INeck.java b/src/main/java/org/team4159/frc/robot/subsystems/INeck.java new file mode 100644 index 0000000..72f54c0 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/INeck.java @@ -0,0 +1,7 @@ +package org.team4159.frc.robot.subsystems; + +public interface INeck { + void setRawSpeed(double speed); + void neck(); + void stop(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IShooter.java b/src/main/java/org/team4159/frc/robot/subsystems/IShooter.java new file mode 100644 index 0000000..7f2ea2d --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/IShooter.java @@ -0,0 +1,10 @@ +package org.team4159.frc.robot.subsystems; + +import org.team4159.lib.hardware.Limelight; + +public interface IShooter { + void setRawSpeed(double speed); + void setRawVoltage(double voltage); + double getSpeed(); + Limelight getLimelight(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/ITurret.java b/src/main/java/org/team4159/frc/robot/subsystems/ITurret.java new file mode 100644 index 0000000..5d47682 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/subsystems/ITurret.java @@ -0,0 +1,13 @@ +package org.team4159.frc.robot.subsystems; + +import org.team4159.lib.hardware.Limelight; + +public interface ITurret { + void setRawSpeed(double speed); + void stop(); + void setEncoderPosition(int position); + int getPosition(); + boolean isForwardLimitSwitchClosed(); + boolean isReverseLimitSwitchClosed(); + Limelight getLimelight(); +} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java index e777eae..f673adc 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java @@ -1,13 +1,14 @@ package org.team4159.frc.robot.subsystems; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.lib.hardware.controller.rev.CardinalMAX; import static org.team4159.frc.robot.Constants.*; -public class Intake extends SubsystemBase { +public class Intake extends SubsystemBase implements IIntake { private CANSparkMax intake_spark; public Intake() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java index a38535b..96e01d9 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java @@ -1,13 +1,14 @@ package org.team4159.frc.robot.subsystems; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.lib.hardware.controller.rev.CardinalMAX; import static org.team4159.frc.robot.Constants.*; -public class Neck extends SubsystemBase { +public class Neck extends SubsystemBase implements INeck { private CANSparkMax intake_spark; public Neck() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index 71c24eb..70f4b14 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java @@ -2,8 +2,6 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -15,13 +13,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Shooter extends SubsystemBase { - private enum State { - CLOSED_LOOP, - IDLE - } - private State state = State.IDLE; - +public class Shooter extends SubsystemBase implements IShooter { private CardinalSRX primary_shooter_talon, shooter_talon_two; private CardinalSPX shooter_victor_one, shooter_victor_two; private SpeedControllerGroup shooter_motors; diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java index 055313d..bf513ba 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java @@ -1,6 +1,5 @@ package org.team4159.frc.robot.subsystems; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -13,7 +12,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Turret extends SubsystemBase { +public class Turret extends SubsystemBase implements ITurret { private TalonFX turret_falcon; private Limelight limelight; diff --git a/src/main/java/org/team4159/lib/control/ControlLoop.java b/src/main/java/org/team4159/lib/control/IControlLoop.java similarity index 62% rename from src/main/java/org/team4159/lib/control/ControlLoop.java rename to src/main/java/org/team4159/lib/control/IControlLoop.java index 684d81c..8ee1408 100644 --- a/src/main/java/org/team4159/lib/control/ControlLoop.java +++ b/src/main/java/org/team4159/lib/control/IControlLoop.java @@ -1,5 +1,5 @@ package org.team4159.lib.control; -public interface ControlLoop { +public interface IControlLoop { void update(); } diff --git a/src/main/java/org/team4159/lib/simulation/SimulationRunner.java b/src/main/java/org/team4159/lib/simulation/SimulationRunner.java index 62e6800..79d42a4 100644 --- a/src/main/java/org/team4159/lib/simulation/SimulationRunner.java +++ b/src/main/java/org/team4159/lib/simulation/SimulationRunner.java @@ -1,14 +1,14 @@ package org.team4159.lib.simulation; -import org.team4159.lib.control.ControlLoop; +import org.team4159.lib.control.IControlLoop; import org.team4159.lib.math.MathUtil; -import org.team4159.lib.simulation.mocks.SubsystemMock; +import org.team4159.lib.simulation.mocks.ISubsystemMock; import static org.team4159.lib.simulation.SimulationConfiguration.*; public class SimulationRunner { // runs a simulated subsystem at a short timestep alongside a control loop at the usual timestep - public static void simulate(SubsystemMock subsystem_mock, ControlLoop control_loop, double time) { + public static void simulate(ISubsystemMock subsystem_mock, IControlLoop control_loop, double time) { while (time > 0) { if (MathUtil.epsilonEquals(time % CONTROL_LOOP_TIMESTEP, 0, 1E-3)) { control_loop.update(); diff --git a/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java b/src/main/java/org/team4159/lib/simulation/mocks/ISubsystemMock.java similarity index 68% rename from src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java rename to src/main/java/org/team4159/lib/simulation/mocks/ISubsystemMock.java index e853a37..cd6031a 100644 --- a/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java +++ b/src/main/java/org/team4159/lib/simulation/mocks/ISubsystemMock.java @@ -1,5 +1,5 @@ package org.team4159.lib.simulation.mocks; -public interface SubsystemMock { +public interface ISubsystemMock { void simulate(double dt); } diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index 3bacf2c..cc0515f 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -9,7 +9,7 @@ import org.team4159.lib.math.Conversions; import org.team4159.lib.simulation.SimulationRunner; import org.team4159.lib.simulation.mocks.RelativeEncoderMock; -import org.team4159.lib.simulation.mocks.SubsystemMock; +import org.team4159.lib.simulation.mocks.ISubsystemMock; import org.team4159.lib.simulation.MotorModels; import org.team4159.lib.math.physics.DCMotorModel; import org.team4159.frc.robot.controllers.ArmController; @@ -18,7 +18,7 @@ import static org.team4159.frc.robot.Constants.*; public class ArmTest { - public class MockArm extends Arm implements SubsystemMock { + public class MockArm implements IArm, ISubsystemMock { // arm model is really slow, i think the inertia / gearing are off private double inertia = Conversions.poundsToKilograms(3) * Math.pow(Units.inchesToMeters(8), 2); // kg m^2 private DCMotorModel motor; @@ -55,27 +55,22 @@ void setRealStartingPosition(int position) { return angular_velocity; } - @Override public void setRawSpeed(double speed) { setRawVoltage(speed * 12); } - @Override public void setRawVoltage(double voltage) { applied_voltage = voltage; } - @Override public void zeroEncoder() { encoder.setEncoderPosition(0); } - @Override public int getPosition() { return (int) encoder.getEncoderPosition(); } - @Override public boolean isLimitSwitchClosed() { return encoder.getRealPosition() <= 0; } diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java index ae25771..cc38af4 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java @@ -1,5 +1,33 @@ package org.team4159.frc.robot.subsystems; +import org.team4159.lib.hardware.Limelight; +import org.team4159.lib.simulation.mocks.ISubsystemMock; + public class ShooterTest { + public class MockShooter implements IShooter, ISubsystemMock { + @Override + public void simulate(double dt) { + + } + + @Override + public void setRawSpeed(double speed) { + + } + + @Override + public void setRawVoltage(double voltage) { + + } + + @Override + public double getSpeed() { + return 0; + } + @Override + public Limelight getLimelight() { + return null; + } + } } From 26c32793a7e0bd341319e22782e6cc358e706196 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Thu, 20 Feb 2020 09:24:32 -0800 Subject: [PATCH 12/21] Revert "use interfaces" This reverts commit d5dcb1e7877acc84b48a7e084da31d02bbed45a3. --- .../team4159/frc/robot/RobotContainer.java | 4 +- .../frc/robot/controllers/ArmController.java | 10 ++-- .../robot/controllers/ShooterController.java | 10 ++-- .../robot/controllers/ShootingManager.java | 10 ++-- ...Controller.java => TrajectoryManager.java} | 44 +++++--------- .../robot/controllers/TurretController.java | 10 ++-- .../team4159/frc/robot/subsystems/Arm.java | 7 ++- .../frc/robot/subsystems/Drivetrain.java | 58 +++++++++++++++---- .../team4159/frc/robot/subsystems/Feeder.java | 2 +- .../team4159/frc/robot/subsystems/IArm.java | 9 --- .../frc/robot/subsystems/IDrivetrain.java | 22 ------- .../frc/robot/subsystems/IFeeder.java | 7 --- .../frc/robot/subsystems/IIntake.java | 7 --- .../team4159/frc/robot/subsystems/INeck.java | 7 --- .../frc/robot/subsystems/IShooter.java | 10 ---- .../frc/robot/subsystems/ITurret.java | 13 ----- .../team4159/frc/robot/subsystems/Intake.java | 5 +- .../team4159/frc/robot/subsystems/Neck.java | 5 +- .../frc/robot/subsystems/Shooter.java | 10 +++- .../team4159/frc/robot/subsystems/Turret.java | 3 +- .../{IControlLoop.java => ControlLoop.java} | 2 +- .../lib/simulation/SimulationRunner.java | 6 +- ...ISubsystemMock.java => SubsystemMock.java} | 2 +- .../frc/robot/subsystems/ArmTest.java | 9 ++- .../frc/robot/subsystems/ShooterTest.java | 28 --------- 25 files changed, 119 insertions(+), 181 deletions(-) rename src/main/java/org/team4159/frc/robot/controllers/{DrivetrainController.java => TrajectoryManager.java} (73%) delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IArm.java delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IIntake.java delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/INeck.java delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/IShooter.java delete mode 100644 src/main/java/org/team4159/frc/robot/subsystems/ITurret.java rename src/main/java/org/team4159/lib/control/{IControlLoop.java => ControlLoop.java} (62%) rename src/main/java/org/team4159/lib/simulation/mocks/{ISubsystemMock.java => SubsystemMock.java} (68%) diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index 7d08bb4..4f46eb8 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -73,10 +73,10 @@ public void updateArmInputs() { public void updateDrivetrainInputs() { if (secondary_joy.getRawButtonPressed(CONTROLS.SECONDARY_JOY.BUTTON_IDS.FLIP_ROBOT_ORIENTATION)) { - drivetrain.getController().flipDriveOrientation(); + drivetrain.flipOrientation(); } - drivetrain.getController().setDriveSignal(new DriveSignal(left_joy.getY(), right_joy.getY())); + drivetrain.drive(new DriveSignal(left_joy.getY(), right_joy.getY())); } public void updateFeederInputs() { diff --git a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java index 2049aef..b93dc90 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java @@ -2,12 +2,12 @@ import edu.wpi.first.wpilibj.controller.PIDController; -import org.team4159.frc.robot.subsystems.IArm; -import org.team4159.lib.control.IControlLoop; +import org.team4159.frc.robot.subsystems.Arm; +import org.team4159.lib.control.ControlLoop; import static org.team4159.frc.robot.Constants.*; -public class ArmController implements IControlLoop { +public class ArmController implements ControlLoop { private enum State { ZEROING, CLOSED_LOOP, @@ -15,11 +15,11 @@ private enum State { } private State state = State.ZEROING; - private IArm arm; + private Arm arm; private PIDController pid_controller; - public ArmController(IArm arm) { + public ArmController(Arm arm) { this.arm = arm; pid_controller = new PIDController( diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java index 7e53542..9bc72b5 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java @@ -3,17 +3,17 @@ import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.team4159.frc.robot.Constants; -import org.team4159.frc.robot.subsystems.IShooter; -import org.team4159.lib.control.IControlLoop; +import org.team4159.frc.robot.subsystems.Shooter; +import org.team4159.lib.control.ControlLoop; import org.team4159.lib.hardware.Limelight; -public class ShooterController implements IControlLoop { +public class ShooterController implements ControlLoop { public enum State { IDLE, CLOSED_LOOP } - private IShooter shooter; + private Shooter shooter; private Limelight limelight; private State state = State.IDLE; @@ -23,7 +23,7 @@ public enum State { Constants.SHOOTER_CONSTANTS.kD ); - public ShooterController(IShooter shooter) { + public ShooterController(Shooter shooter) { this.shooter = shooter; this.limelight = shooter.getLimelight(); diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java b/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java index 8599f0a..53de402 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java @@ -1,22 +1,22 @@ package org.team4159.frc.robot.controllers; -import org.team4159.frc.robot.subsystems.INeck; -import org.team4159.lib.control.IControlLoop; +import org.team4159.frc.robot.subsystems.Neck; +import org.team4159.lib.control.ControlLoop; -public class ShootingManager implements IControlLoop { +public class ShootingManager implements ControlLoop { private enum State { IDLE, WAITING, SHOOTING } - private INeck neck; + private Neck neck; private TurretController turret_controller; private ShooterController shooter_controller; private State state = State.IDLE; - public ShootingManager(INeck neck, TurretController turret_controller, ShooterController shooter_controller) { + public ShootingManager(Neck neck, TurretController turret_controller, ShooterController shooter_controller) { this.neck = neck; this.turret_controller = turret_controller; this.shooter_controller = shooter_controller; diff --git a/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java similarity index 73% rename from src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java rename to src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java index b36b99c..c192d47 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java @@ -11,40 +11,38 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj.trajectory.Trajectory; -import org.team4159.frc.robot.subsystems.IDrivetrain; +import org.team4159.frc.robot.Constants; +import org.team4159.frc.robot.subsystems.Drivetrain; +import org.team4159.lib.control.ControlLoop; import org.team4159.lib.control.signal.DriveSignal; -import static org.team4159.frc.robot.Constants.*; - -public class DrivetrainController { +public class TrajectoryManager { private enum State { FOLLOWING, IDLE } private State state = State.IDLE; - private IDrivetrain drivetrain; + private Drivetrain drivetrain; private Trajectory trajectory_to_follow; - private DriveSignal demanded_signal; - private boolean is_oriented_forwards = true; private DifferentialDriveWheelSpeeds prev_speeds = new DifferentialDriveWheelSpeeds(0,0); - private DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DRIVE_CONSTANTS.TRACK_WIDTH); - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(DRIVE_CONSTANTS.kS, DRIVE_CONSTANTS.kV, DRIVE_CONSTANTS.kA); - private RamseteController controller = new RamseteController(DRIVE_CONSTANTS.kB, DRIVE_CONSTANTS.kZeta); - private PIDController pid_left = new PIDController(DRIVE_CONSTANTS.kP, 0, DRIVE_CONSTANTS.kD); - private PIDController pid_right = new PIDController(DRIVE_CONSTANTS.kP, 0, DRIVE_CONSTANTS.kD); + private DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.DRIVE_CONSTANTS.TRACK_WIDTH); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.DRIVE_CONSTANTS.kS, Constants.DRIVE_CONSTANTS.kV, Constants.DRIVE_CONSTANTS.kA); + private RamseteController controller = new RamseteController(Constants.DRIVE_CONSTANTS.kB, Constants.DRIVE_CONSTANTS.kZeta); + private PIDController pid_left = new PIDController(Constants.DRIVE_CONSTANTS.kP, 0, Constants.DRIVE_CONSTANTS.kD); + private PIDController pid_right = new PIDController(Constants.DRIVE_CONSTANTS.kP, 0, Constants.DRIVE_CONSTANTS.kD); private Timer timer = new Timer(); private double prev_time = 0; - public DrivetrainController(IDrivetrain drivetrain) { + public TrajectoryManager(Drivetrain drivetrain) { this.drivetrain = drivetrain; } public void update() { - DriveSignal filtered_signal = DriveSignal.NEUTRAL; + DriveSignal signal = DriveSignal.NEUTRAL; switch (state) { case FOLLOWING: @@ -82,32 +80,20 @@ public void update() { prev_time = cur_time; prev_speeds = target_wheel_speeds; - filtered_signal = DriveSignal.fromVolts(left_output, right_output); + signal = DriveSignal.fromVolts(left_output, right_output); break; case IDLE: - filtered_signal = demanded_signal; + // shouldn't be getting called break; } - if (!is_oriented_forwards) { - filtered_signal = filtered_signal.invert(); - } - - drivetrain.drive(filtered_signal); + drivetrain.drive(signal); } public boolean isIdle() { return state == State.IDLE; } - public void setDriveSignal(DriveSignal signal) { - this.demanded_signal = signal; - } - - public void flipDriveOrientation() { - is_oriented_forwards = !is_oriented_forwards; - } - public void setTrajectory(Trajectory trajectory) { state = State.FOLLOWING; diff --git a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java index de3e05a..0850ba5 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java @@ -1,13 +1,13 @@ package org.team4159.frc.robot.controllers; import edu.wpi.first.wpilibj.controller.PIDController; -import org.team4159.frc.robot.subsystems.ITurret; -import org.team4159.lib.control.IControlLoop; +import org.team4159.frc.robot.subsystems.Turret; +import org.team4159.lib.control.ControlLoop; import org.team4159.lib.hardware.Limelight; import static org.team4159.frc.robot.Constants.*; -public class TurretController implements IControlLoop { +public class TurretController implements ControlLoop { public enum State { ZEROING, SEEKING_TARGET, @@ -16,7 +16,7 @@ public enum State { OPEN_LOOP } - private ITurret turret; + private Turret turret; private Limelight limelight; private int seeking_direction, seeking_range, seeking_starting_position; @@ -30,7 +30,7 @@ public enum State { TURRET_CONSTANTS.LIMELIGHT_TURN_kD ); - public TurretController(ITurret turret) { + public TurretController(Turret turret) { this.turret = turret; this.limelight = turret.getLimelight(); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java index 56abca6..66198c2 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Arm.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Arm.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.frc.robot.controllers.ArmController; @@ -11,7 +12,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Arm extends SubsystemBase implements IArm { +public class Arm extends SubsystemBase { private CANSparkMax arm_spark; private DigitalInput arm_limit_switch; @@ -20,6 +21,10 @@ public class Arm extends SubsystemBase implements IArm { private ArmController arm_controller; public Arm() { + if (RobotBase.isSimulation()) { + return; + } + arm_spark = new CardinalMAX(CAN_IDS.ARM_SPARK, CANSparkMax.IdleMode.kBrake); arm_spark.setInverted(true); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index dc15a24..466677e 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -1,25 +1,33 @@ package org.team4159.frc.robot.subsystems; import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; -import org.team4159.frc.robot.controllers.DrivetrainController; +import org.team4159.frc.robot.controllers.TrajectoryManager; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.control.signal.filters.LowPassFilterSource; import org.team4159.lib.hardware.controller.ctre.CardinalFX; import static org.team4159.frc.robot.Constants.*; -public class Drivetrain extends SubsystemBase implements IDrivetrain { +public class Drivetrain extends SubsystemBase { + private enum State { + PATH_FOLLOWING, + OPEN_LOOP + } + private State state = State.OPEN_LOOP; + private CardinalFX left_front_falcon, left_rear_falcon, right_front_falcon, right_rear_falcon; private SpeedControllerGroup left_falcons; private SpeedControllerGroup right_falcons; @@ -28,7 +36,9 @@ public class Drivetrain extends SubsystemBase implements IDrivetrain { private PigeonIMU pigeon; private LowPassFilterSource filtered_heading; - private DrivetrainController drivetrain_controller; + private TrajectoryManager trajectory_controller; + private DriveSignal drive_signal; + private boolean is_oriented_forward = true; public Drivetrain() { left_front_falcon = new CardinalFX(CAN_IDS.LEFT_FRONT_FALCON, NeutralMode.Coast); @@ -58,11 +68,15 @@ public Drivetrain() { odometry = new DifferentialDriveOdometry(new Rotation2d(0)); filtered_heading = new LowPassFilterSource(pigeon::getFusedHeading, 10); - drivetrain_controller = new DrivetrainController(this); + trajectory_controller = new TrajectoryManager(this); zeroSensors(); } + public void flipOrientation() { + is_oriented_forward = !is_oriented_forward; + } + @Override public void periodic() { odometry.update( @@ -71,11 +85,37 @@ public void periodic() { getRightDistance() ); filtered_heading.get(); + + SmartDashboard.putNumber("X", getPose().getTranslation().getX()); + SmartDashboard.putNumber("Y", getPose().getTranslation().getY()); + SmartDashboard.putNumber("Angle", getDirection()); + SmartDashboard.putNumber("Left Encoder", getLeftDistance()); + SmartDashboard.putNumber("Right Encoder", getRightDistance()); + + switch (state) { + case PATH_FOLLOWING: + trajectory_controller.update(); + if (trajectory_controller.isIdle()) { + state = State.OPEN_LOOP; + } + case OPEN_LOOP: + DriveSignal filtered_signal = drive_signal; + if (is_oriented_forward) { + filtered_signal = drive_signal.invert(); + } + left_falcons.set(filtered_signal.left); + right_falcons.set(filtered_signal.right); + break; + } } public void drive(DriveSignal drive_signal) { - left_falcons.set(drive_signal.left); - right_falcons.set(drive_signal.right); + this.drive_signal = drive_signal; + } + + public void followTrajectory(Trajectory trajectory) { + state = State.PATH_FOLLOWING; + trajectory_controller.setTrajectory(trajectory); } public void resetEncoders() { @@ -94,6 +134,8 @@ public void zeroSensors() { new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), Rotation2d.fromDegrees(0) ); + + System.out.println(getDirection()); } public DifferentialDriveWheelSpeeds getWheelSpeeds() { @@ -133,8 +175,4 @@ public double getRightVelocity() { public Pose2d getPose() { return odometry.getPoseMeters(); } - - public DrivetrainController getController() { - return drivetrain_controller; - } } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java index 4b1d626..8ce0ecb 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java @@ -11,7 +11,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Feeder extends SubsystemBase implements IFeeder { +public class Feeder extends SubsystemBase { private SpeedControllerGroup feeder_motors; public Feeder() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IArm.java b/src/main/java/org/team4159/frc/robot/subsystems/IArm.java deleted file mode 100644 index ab30d44..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/IArm.java +++ /dev/null @@ -1,9 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -public interface IArm { - void setRawSpeed(double speed); - void setRawVoltage(double voltage); - void zeroEncoder(); - int getPosition(); - boolean isLimitSwitchClosed(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java deleted file mode 100644 index f725939..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/IDrivetrain.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; - -import org.team4159.lib.control.signal.DriveSignal; - -public interface IDrivetrain { - void drive(DriveSignal drive_signal); - void resetEncoders(); - void resetDirection(); - void zeroSensors(); - DifferentialDriveWheelSpeeds getWheelSpeeds(); - double getDirection(); - double getLeftVoltage(); - double getRightVoltage(); - double getLeftDistance(); - double getLeftVelocity(); - double getRightDistance(); - double getRightVelocity(); - Pose2d getPose(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java b/src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java deleted file mode 100644 index a374d59..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/IFeeder.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -public interface IFeeder { - void setRawSpeed(double speed); - void feed(); - void stop(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IIntake.java b/src/main/java/org/team4159/frc/robot/subsystems/IIntake.java deleted file mode 100644 index 61c0d12..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/IIntake.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -public interface IIntake { - void setRawSpeed(double speed); - void intake(); - void stop(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/INeck.java b/src/main/java/org/team4159/frc/robot/subsystems/INeck.java deleted file mode 100644 index 72f54c0..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/INeck.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -public interface INeck { - void setRawSpeed(double speed); - void neck(); - void stop(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/IShooter.java b/src/main/java/org/team4159/frc/robot/subsystems/IShooter.java deleted file mode 100644 index 7f2ea2d..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/IShooter.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -import org.team4159.lib.hardware.Limelight; - -public interface IShooter { - void setRawSpeed(double speed); - void setRawVoltage(double voltage); - double getSpeed(); - Limelight getLimelight(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/ITurret.java b/src/main/java/org/team4159/frc/robot/subsystems/ITurret.java deleted file mode 100644 index 5d47682..0000000 --- a/src/main/java/org/team4159/frc/robot/subsystems/ITurret.java +++ /dev/null @@ -1,13 +0,0 @@ -package org.team4159.frc.robot.subsystems; - -import org.team4159.lib.hardware.Limelight; - -public interface ITurret { - void setRawSpeed(double speed); - void stop(); - void setEncoderPosition(int position); - int getPosition(); - boolean isForwardLimitSwitchClosed(); - boolean isReverseLimitSwitchClosed(); - Limelight getLimelight(); -} diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java index f673adc..e777eae 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java @@ -1,14 +1,13 @@ package org.team4159.frc.robot.subsystems; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.lib.hardware.controller.rev.CardinalMAX; import static org.team4159.frc.robot.Constants.*; -public class Intake extends SubsystemBase implements IIntake { +public class Intake extends SubsystemBase { private CANSparkMax intake_spark; public Intake() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java index 96e01d9..a38535b 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java @@ -1,14 +1,13 @@ package org.team4159.frc.robot.subsystems; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.lib.hardware.controller.rev.CardinalMAX; import static org.team4159.frc.robot.Constants.*; -public class Neck extends SubsystemBase implements INeck { +public class Neck extends SubsystemBase { private CANSparkMax intake_spark; public Neck() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index 70f4b14..71c24eb 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -13,7 +15,13 @@ import static org.team4159.frc.robot.Constants.*; -public class Shooter extends SubsystemBase implements IShooter { +public class Shooter extends SubsystemBase { + private enum State { + CLOSED_LOOP, + IDLE + } + private State state = State.IDLE; + private CardinalSRX primary_shooter_talon, shooter_talon_two; private CardinalSPX shooter_victor_one, shooter_victor_two; private SpeedControllerGroup shooter_motors; diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java index bf513ba..055313d 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java @@ -1,5 +1,6 @@ package org.team4159.frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -12,7 +13,7 @@ import static org.team4159.frc.robot.Constants.*; -public class Turret extends SubsystemBase implements ITurret { +public class Turret extends SubsystemBase { private TalonFX turret_falcon; private Limelight limelight; diff --git a/src/main/java/org/team4159/lib/control/IControlLoop.java b/src/main/java/org/team4159/lib/control/ControlLoop.java similarity index 62% rename from src/main/java/org/team4159/lib/control/IControlLoop.java rename to src/main/java/org/team4159/lib/control/ControlLoop.java index 8ee1408..684d81c 100644 --- a/src/main/java/org/team4159/lib/control/IControlLoop.java +++ b/src/main/java/org/team4159/lib/control/ControlLoop.java @@ -1,5 +1,5 @@ package org.team4159.lib.control; -public interface IControlLoop { +public interface ControlLoop { void update(); } diff --git a/src/main/java/org/team4159/lib/simulation/SimulationRunner.java b/src/main/java/org/team4159/lib/simulation/SimulationRunner.java index 79d42a4..62e6800 100644 --- a/src/main/java/org/team4159/lib/simulation/SimulationRunner.java +++ b/src/main/java/org/team4159/lib/simulation/SimulationRunner.java @@ -1,14 +1,14 @@ package org.team4159.lib.simulation; -import org.team4159.lib.control.IControlLoop; +import org.team4159.lib.control.ControlLoop; import org.team4159.lib.math.MathUtil; -import org.team4159.lib.simulation.mocks.ISubsystemMock; +import org.team4159.lib.simulation.mocks.SubsystemMock; import static org.team4159.lib.simulation.SimulationConfiguration.*; public class SimulationRunner { // runs a simulated subsystem at a short timestep alongside a control loop at the usual timestep - public static void simulate(ISubsystemMock subsystem_mock, IControlLoop control_loop, double time) { + public static void simulate(SubsystemMock subsystem_mock, ControlLoop control_loop, double time) { while (time > 0) { if (MathUtil.epsilonEquals(time % CONTROL_LOOP_TIMESTEP, 0, 1E-3)) { control_loop.update(); diff --git a/src/main/java/org/team4159/lib/simulation/mocks/ISubsystemMock.java b/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java similarity index 68% rename from src/main/java/org/team4159/lib/simulation/mocks/ISubsystemMock.java rename to src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java index cd6031a..e853a37 100644 --- a/src/main/java/org/team4159/lib/simulation/mocks/ISubsystemMock.java +++ b/src/main/java/org/team4159/lib/simulation/mocks/SubsystemMock.java @@ -1,5 +1,5 @@ package org.team4159.lib.simulation.mocks; -public interface ISubsystemMock { +public interface SubsystemMock { void simulate(double dt); } diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index cc0515f..3bacf2c 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -9,7 +9,7 @@ import org.team4159.lib.math.Conversions; import org.team4159.lib.simulation.SimulationRunner; import org.team4159.lib.simulation.mocks.RelativeEncoderMock; -import org.team4159.lib.simulation.mocks.ISubsystemMock; +import org.team4159.lib.simulation.mocks.SubsystemMock; import org.team4159.lib.simulation.MotorModels; import org.team4159.lib.math.physics.DCMotorModel; import org.team4159.frc.robot.controllers.ArmController; @@ -18,7 +18,7 @@ import static org.team4159.frc.robot.Constants.*; public class ArmTest { - public class MockArm implements IArm, ISubsystemMock { + public class MockArm extends Arm implements SubsystemMock { // arm model is really slow, i think the inertia / gearing are off private double inertia = Conversions.poundsToKilograms(3) * Math.pow(Units.inchesToMeters(8), 2); // kg m^2 private DCMotorModel motor; @@ -55,22 +55,27 @@ void setRealStartingPosition(int position) { return angular_velocity; } + @Override public void setRawSpeed(double speed) { setRawVoltage(speed * 12); } + @Override public void setRawVoltage(double voltage) { applied_voltage = voltage; } + @Override public void zeroEncoder() { encoder.setEncoderPosition(0); } + @Override public int getPosition() { return (int) encoder.getEncoderPosition(); } + @Override public boolean isLimitSwitchClosed() { return encoder.getRealPosition() <= 0; } diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java index cc38af4..ae25771 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java @@ -1,33 +1,5 @@ package org.team4159.frc.robot.subsystems; -import org.team4159.lib.hardware.Limelight; -import org.team4159.lib.simulation.mocks.ISubsystemMock; - public class ShooterTest { - public class MockShooter implements IShooter, ISubsystemMock { - @Override - public void simulate(double dt) { - - } - - @Override - public void setRawSpeed(double speed) { - - } - - @Override - public void setRawVoltage(double voltage) { - - } - - @Override - public double getSpeed() { - return 0; - } - @Override - public Limelight getLimelight() { - return null; - } - } } From 6c968f8411407672935f5ecd3fe35ee1ce758393 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Fri, 21 Feb 2020 15:38:59 -0800 Subject: [PATCH 13/21] prog --- .../robot/controllers/ShootingManager.java | 1 + .../robot/controllers/TrajectoryManager.java | 1 - .../robot/controllers/TurretController.java | 2 +- .../frc/robot/subsystems/Shooter.java | 10 --------- .../team4159/frc/robot/subsystems/Turret.java | 1 - .../org/team4159/lib/hardware/Limelight.java | 6 ++--- .../frc/robot/subsystems/ArmTest.java | 1 + .../frc/robot/subsystems/ShooterTest.java | 22 +++++++++++++++++++ 8 files changed, 28 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java b/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java index 991e578..4c797c9 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java @@ -1,6 +1,7 @@ package org.team4159.frc.robot.controllers; import edu.wpi.first.wpilibj.Timer; + import org.team4159.frc.robot.subsystems.Neck; import org.team4159.lib.control.ControlLoop; diff --git a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java index c192d47..1be689e 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java @@ -13,7 +13,6 @@ import org.team4159.frc.robot.Constants; import org.team4159.frc.robot.subsystems.Drivetrain; -import org.team4159.lib.control.ControlLoop; import org.team4159.lib.control.signal.DriveSignal; public class TrajectoryManager { diff --git a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java index 8244456..f4ffe8d 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java @@ -32,7 +32,7 @@ public enum State { public TurretController(Turret turret) { this.turret = turret; - this.limelight = Limelight.getInstance(); + this.limelight = Limelight.getDefault(); limelight.setLEDMode(Limelight.LEDMode.ForceOn); pid_controller.reset(); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index 3a94635..4695e79 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java @@ -2,27 +2,17 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.NeutralMode; import org.team4159.lib.hardware.EnhancedEncoder; -import org.team4159.lib.hardware.Limelight; import org.team4159.lib.hardware.controller.ctre.CardinalSPX; import org.team4159.lib.hardware.controller.ctre.CardinalSRX; import static org.team4159.frc.robot.Constants.*; public class Shooter extends SubsystemBase { - private enum State { - CLOSED_LOOP, - IDLE - } - - private State state = State.IDLE; - private CardinalSRX primary_shooter_talon, shooter_talon_two; private CardinalSPX shooter_victor_one, shooter_victor_two; private SpeedControllerGroup shooter_motors; diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java index bd1b792..1756f72 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java @@ -8,7 +8,6 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.NeutralMode; -import org.team4159.lib.hardware.Limelight; import org.team4159.lib.hardware.controller.ctre.CardinalFX; import static org.team4159.frc.robot.Constants.*; diff --git a/src/main/java/org/team4159/lib/hardware/Limelight.java b/src/main/java/org/team4159/lib/hardware/Limelight.java index 17e0e07..1c943a4 100644 --- a/src/main/java/org/team4159/lib/hardware/Limelight.java +++ b/src/main/java/org/team4159/lib/hardware/Limelight.java @@ -44,18 +44,18 @@ public enum StreamingMode { private static Limelight instance = null; - public static Limelight getInstance() { + public static Limelight getDefault() { if (instance == null) { instance = new Limelight(); } return instance; } - private Limelight(NetworkTableInstance nt) { + public Limelight(NetworkTableInstance nt) { limelight_table = nt.getTable("limelight"); } - private Limelight() { + public Limelight() { this(NetworkTableInstance.getDefault()); } diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index 3bacf2c..f9696a3 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -32,6 +32,7 @@ public class MockArm extends Arm implements SubsystemMock { motor = MotorModels.NEO_550; } + @Override public void simulate(double dt) { double torque = motor.getTorqueForVoltage(angular_velocity, applied_voltage); double angular_acceleration = torque / inertia; diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java index ae25771..0094f84 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java @@ -1,5 +1,27 @@ package org.team4159.frc.robot.subsystems; +import org.team4159.lib.simulation.mocks.SubsystemMock; + public class ShooterTest { + public class MockShooter extends Shooter implements SubsystemMock { + @Override + public void simulate(double dt) { + + } + + @Override + public void setRawSpeed(double speed) { + super.setRawSpeed(speed); + } + + @Override + public void setRawVoltage(double voltage) { + super.setRawVoltage(voltage); + } + @Override + public double getSpeed() { + return super.getSpeed(); + } + } } From bf6449fcf119ea383195bfb572d577471bed77ef Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 00:00:49 -0800 Subject: [PATCH 14/21] update limelight --- .../org/team4159/frc/robot/controllers/ShooterController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java index 84d95c5..93fe3e1 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java @@ -29,7 +29,7 @@ public enum State { public ShooterController(Shooter shooter) { this.shooter = shooter; - this.limelight = Limelight.getInstance(); + this.limelight = Limelight.getDefault(); pid_controller.setTolerance(SHOOTER_CONSTANTS.ACCEPTABLE_SPEED_ERROR); } From f6b6678dce2437f433c93043866d8a1bf06b2ad7 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 00:01:32 -0800 Subject: [PATCH 15/21] idk --- .travis.yml | 17 ++++++++++++++-- build.gradle | 6 ++++++ .../team4159/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/controllers/AutoManager.java | 4 ---- ...Manager.java => TrajectoryController.java} | 6 +++--- .../robot/controllers/TurretController.java | 20 +++++++++---------- .../controllers/complex/AutoController.java | 4 ++++ .../IntakeController.java} | 7 ++++--- .../ShootingController.java} | 14 +++++++------ .../frc/robot/subsystems/Drivetrain.java | 6 +++--- .../lib/math/physics/DCMotorModel.java | 6 +++--- 11 files changed, 58 insertions(+), 36 deletions(-) delete mode 100644 src/main/java/org/team4159/frc/robot/controllers/AutoManager.java rename src/main/java/org/team4159/frc/robot/controllers/{TrajectoryManager.java => TrajectoryController.java} (95%) create mode 100644 src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java rename src/main/java/org/team4159/frc/robot/controllers/{IntakeManager.java => complex/IntakeController.java} (86%) rename src/main/java/org/team4159/frc/robot/controllers/{ShootingManager.java => complex/ShootingController.java} (78%) diff --git a/.travis.yml b/.travis.yml index 8a1522b..eabc434 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,7 +1,20 @@ language: java +rust: stable -script: - - ./gradlew test +jobs: + include: + - stage: "Tests" + name: "Unit Tests" + script: ./gradlew test + - stage: "Report" + name: "Install Deepsource" + script: + - curl https://deepsource.io/cli | sh + - name: "Generate Report" + script: + - git clone https://github.com/dominikWin/badlogvis.git + - cd badlogvis + - cargo install os: - osx diff --git a/build.gradle b/build.gradle index c55363f..f09b314 100644 --- a/build.gradle +++ b/build.gradle @@ -40,9 +40,15 @@ deploy { // Set this to true to enable desktop support. def includeDesktopSupport = false +repositories { + maven { url "https://jitpack.io" } +} + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { + compile "com.github.dominikWin:badlog:v0.1.1" + implementation wpi.deps.wpilib() nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index e4fe85a..bd14ea4 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -7,7 +7,7 @@ import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.hardware.Limelight; -import org.team4159.frc.robot.controllers.IntakeManager; +import org.team4159.frc.robot.controllers.complex.IntakeController; import org.team4159.frc.robot.subsystems.*; import static org.team4159.frc.robot.Constants.*; @@ -21,7 +21,7 @@ public class RobotContainer { private final Arm arm = new Arm(); private final Turret turret = new Turret(); - private final IntakeManager intake_controller = new IntakeManager(arm.getController(), intake, feeder); + private final IntakeController intake_controller = new IntakeController(arm.getController(), intake, feeder); private final Joystick left_joy = new Joystick(CONTROLS.LEFT_JOY.USB_PORT); private final Joystick right_joy = new Joystick(CONTROLS.RIGHT_JOY.USB_PORT); diff --git a/src/main/java/org/team4159/frc/robot/controllers/AutoManager.java b/src/main/java/org/team4159/frc/robot/controllers/AutoManager.java deleted file mode 100644 index 8b3b330..0000000 --- a/src/main/java/org/team4159/frc/robot/controllers/AutoManager.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.team4159.frc.robot.controllers; - -public class AutoManager { -} diff --git a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java similarity index 95% rename from src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java rename to src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java index 1be689e..3aad75a 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java @@ -15,7 +15,7 @@ import org.team4159.frc.robot.subsystems.Drivetrain; import org.team4159.lib.control.signal.DriveSignal; -public class TrajectoryManager { +public class TrajectoryController { private enum State { FOLLOWING, IDLE @@ -36,7 +36,7 @@ private enum State { private Timer timer = new Timer(); private double prev_time = 0; - public TrajectoryManager(Drivetrain drivetrain) { + public TrajectoryController(Drivetrain drivetrain) { this.drivetrain = drivetrain; } @@ -45,7 +45,7 @@ public void update() { switch (state) { case FOLLOWING: - if (timer.get() > trajectory_to_follow.getTotalTimeSeconds()) { + if (timer.hasPeriodPassed(trajectory_to_follow.getTotalTimeSeconds())) { state = State.IDLE; break; } diff --git a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java index f4ffe8d..33fe32b 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java @@ -1,6 +1,7 @@ package org.team4159.frc.robot.controllers; import edu.wpi.first.wpilibj.controller.PIDController; + import org.team4159.frc.robot.subsystems.Turret; import org.team4159.lib.control.ControlLoop; import org.team4159.lib.hardware.Limelight; @@ -8,22 +9,21 @@ import static org.team4159.frc.robot.Constants.*; public class TurretController implements ControlLoop { - public enum State { + private enum State { ZEROING, SEEKING_TARGET, FOUND_TARGET, RECOVERING, OPEN_LOOP } + private State last_state; + private State state = State.ZEROING; private Turret turret; private Limelight limelight; private int seeking_direction, seeking_range, seeking_starting_position; - private State last_state; - private State state = State.ZEROING; - private PIDController pid_controller = new PIDController( TURRET_CONSTANTS.LIMELIGHT_TURN_kP, TURRET_CONSTANTS.LIMELIGHT_TURN_kI, @@ -94,7 +94,7 @@ public void update() { break; case OPEN_LOOP: if (isTurretOutOfSafeRange()) { - setState(State.SEEKING_TARGET); + setState(State.RECOVERING); } break; } @@ -122,7 +122,11 @@ public void startRecovering() { state = State.RECOVERING; } - public void setState(State state) { + public void idle() { + setState(State.OPEN_LOOP); + } + + private void setState(State state) { if (state == State.SEEKING_TARGET) { startSeeking(); } else if (state == State.RECOVERING) { @@ -131,8 +135,4 @@ public void setState(State state) { this.state = state; } } - - public State getState() { - return state; - } } diff --git a/src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java b/src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java new file mode 100644 index 0000000..20aefd8 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/AutoController.java @@ -0,0 +1,4 @@ +package org.team4159.frc.robot.controllers.complex; + +public class AutoController { +} diff --git a/src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java similarity index 86% rename from src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java rename to src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java index cba66a4..ffcd783 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/IntakeManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java @@ -1,11 +1,12 @@ -package org.team4159.frc.robot.controllers; +package org.team4159.frc.robot.controllers.complex; +import org.team4159.frc.robot.controllers.ArmController; import org.team4159.frc.robot.subsystems.Feeder; import org.team4159.frc.robot.subsystems.Intake; import static org.team4159.frc.robot.Constants.*; -public class IntakeManager { +public class IntakeController { private enum State { STOWED, // arm / intake is stowing / stowed DEPLOYING, // arm is deploying, intake is not being run @@ -17,7 +18,7 @@ private enum State { private Intake intake; private Feeder feeder; - public IntakeManager(ArmController arm, Intake intake, Feeder feeder) { + public IntakeController(ArmController arm, Intake intake, Feeder feeder) { this.arm_controller = arm_controller; this.intake = intake; this.feeder = feeder; diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java b/src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java similarity index 78% rename from src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java rename to src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java index 4c797c9..563777f 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShootingManager.java +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/ShootingController.java @@ -1,11 +1,13 @@ -package org.team4159.frc.robot.controllers; +package org.team4159.frc.robot.controllers.complex; import edu.wpi.first.wpilibj.Timer; +import org.team4159.frc.robot.controllers.ShooterController; +import org.team4159.frc.robot.controllers.TurretController; import org.team4159.frc.robot.subsystems.Neck; import org.team4159.lib.control.ControlLoop; -public class ShootingManager implements ControlLoop { +public class ShootingController implements ControlLoop { private enum State { IDLE, WAITING, @@ -21,7 +23,7 @@ private enum State { private State state = State.IDLE; private Timer timer = new Timer(); - public ShootingManager(Neck neck, TurretController turret_controller, ShooterController shooter_controller) { + public ShootingController(Neck neck, TurretController turret_controller, ShooterController shooter_controller) { this.neck = neck; this.turret_controller = turret_controller; this.shooter_controller = shooter_controller; @@ -42,7 +44,7 @@ public void update() { } break; case SHOOTING: - if (isShooterReadyToShoot()) { + if (!isShooterReadyToShoot()) { state = State.WAITING; } else { neck.neck(); @@ -63,14 +65,14 @@ private boolean isShooterReadyToShoot() { } public void beginShooting() { - turret_controller.setState(TurretController.State.SEEKING_TARGET); + turret_controller.startSeeking(); shooter_controller.setState(ShooterController.State.CLOSED_LOOP); state = State.WAITING; } public void stopShooting() { - turret_controller.setState(TurretController.State.OPEN_LOOP); + turret_controller.idle(); shooter_controller.setState(ShooterController.State.IDLE); state = State.IDLE; diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index 466677e..51785b3 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -14,7 +14,7 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; -import org.team4159.frc.robot.controllers.TrajectoryManager; +import org.team4159.frc.robot.controllers.TrajectoryController; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.control.signal.filters.LowPassFilterSource; import org.team4159.lib.hardware.controller.ctre.CardinalFX; @@ -36,7 +36,7 @@ private enum State { private PigeonIMU pigeon; private LowPassFilterSource filtered_heading; - private TrajectoryManager trajectory_controller; + private TrajectoryController trajectory_controller; private DriveSignal drive_signal; private boolean is_oriented_forward = true; @@ -68,7 +68,7 @@ public Drivetrain() { odometry = new DifferentialDriveOdometry(new Rotation2d(0)); filtered_heading = new LowPassFilterSource(pigeon::getFusedHeading, 10); - trajectory_controller = new TrajectoryManager(this); + trajectory_controller = new TrajectoryController(this); zeroSensors(); } diff --git a/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java index f9c0aeb..f10ed50 100644 --- a/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java +++ b/src/main/java/org/team4159/lib/math/physics/DCMotorModel.java @@ -34,13 +34,13 @@ public double resistance() { } // "velocity constant" - public double volt_per_speed() { + public double volts_per_speed() { return free_speed / (test_voltage - free_current * resistance()); // voltage dropped at free speed = free_current * resistance } // "back-EMF constant" public double speed_per_volt() { - return 1 / volt_per_speed(); + return 1 / volts_per_speed(); } // "torque constant" @@ -53,6 +53,6 @@ public double torque_per_volt() { } public double getTorqueForVoltage(final double output_speed, final double voltage) { - return torque_per_volt() * (voltage - output_speed / speed_per_volt()); + return torque_per_volt() * (voltage - output_speed * volts_per_speed()); } } From ee350b1a247ca1bfd2a22193ef15a3a054781728 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 00:33:18 -0800 Subject: [PATCH 16/21] finalize controllers --- .../team4159/frc/robot/RobotContainer.java | 5 +- .../frc/robot/controllers/ArmController.java | 1 + .../controllers/DrivetrainController.java | 66 +++++++++++++++++ .../robot/controllers/ShooterController.java | 8 +- .../controllers/TrajectoryController.java | 73 +++++++------------ .../robot/controllers/TurretController.java | 27 +++---- .../controllers/complex/IntakeController.java | 8 +- .../frc/robot/subsystems/Drivetrain.java | 57 +++------------ .../frc/robot/subsystems/Shooter.java | 14 ++++ .../team4159/frc/robot/subsystems/Turret.java | 15 +++- 10 files changed, 153 insertions(+), 121 deletions(-) create mode 100644 src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index bd14ea4..43d6dc6 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.*; import org.team4159.lib.control.signal.DriveSignal; -import org.team4159.lib.hardware.Limelight; import org.team4159.frc.robot.controllers.complex.IntakeController; import org.team4159.frc.robot.subsystems.*; @@ -72,10 +71,10 @@ public void updateArmInputs() { public void updateDrivetrainInputs() { if (secondary_joy.getRawButtonPressed(CONTROLS.SECONDARY_JOY.BUTTON_IDS.FLIP_ROBOT_ORIENTATION)) { - drivetrain.flipOrientation(); + drivetrain.getController().flipDriveOrientation(); } - drivetrain.drive(new DriveSignal(left_joy.getY(), right_joy.getY())); + drivetrain.getController().demandSignal(new DriveSignal(left_joy.getY(), right_joy.getY())); } public void updateFeederInputs() { diff --git a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java index b93dc90..51731d0 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java @@ -30,6 +30,7 @@ public ArmController(Arm arm) { pid_controller.setTolerance(ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); } + @Override public void update() { switch (state) { case ZEROING: diff --git a/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java b/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java new file mode 100644 index 0000000..a17d6e1 --- /dev/null +++ b/src/main/java/org/team4159/frc/robot/controllers/DrivetrainController.java @@ -0,0 +1,66 @@ +package org.team4159.frc.robot.controllers; + +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +import org.team4159.frc.robot.subsystems.Drivetrain; +import org.team4159.lib.control.ControlLoop; +import org.team4159.lib.control.signal.DriveSignal; + +public class DrivetrainController implements ControlLoop { + private enum State { + PATH_FOLLOWING, + OPEN_LOOP + } + private State state = State.OPEN_LOOP; + + private boolean is_drive_oriented_forwards = true; + private DriveSignal demanded_signal = DriveSignal.NEUTRAL; + + private Drivetrain drivetrain; + private TrajectoryController trajectory_controller; + + public DrivetrainController(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + this.trajectory_controller = new TrajectoryController(drivetrain); + } + + @Override + public void update() { + DriveSignal filtered_signal = DriveSignal.NEUTRAL; + + switch (state) { + case OPEN_LOOP: + if (is_drive_oriented_forwards) { + filtered_signal = demanded_signal; + } else { + filtered_signal = demanded_signal.invert(); + } + break; + case PATH_FOLLOWING: + trajectory_controller.update(); + if (trajectory_controller.isComplete()) { + state = State.OPEN_LOOP; + } + break; + } + + drivetrain.drive(filtered_signal); + } + + public void demandSignal(DriveSignal signal) { + demanded_signal = signal; + } + + public void flipDriveOrientation() { + is_drive_oriented_forwards = !is_drive_oriented_forwards; + } + + public void startFollowingTrajectory(Trajectory trajectory) { + state = State.PATH_FOLLOWING; + trajectory_controller.setTrajectory(trajectory); + } + + public void cancelPathFollowing() { + state = State.OPEN_LOOP; + } +} diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java index 93fe3e1..eabf9d9 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java @@ -14,22 +14,20 @@ public enum State { IDLE, CLOSED_LOOP } - + private State state = State.IDLE; private Shooter shooter; private Limelight limelight; - private State state = State.IDLE; - private PIDController pid_controller = new PIDController( SHOOTER_CONSTANTS.kP, SHOOTER_CONSTANTS.kI, SHOOTER_CONSTANTS.kD ); - public ShooterController(Shooter shooter) { + public ShooterController(Shooter shooter, Limelight limelight) { this.shooter = shooter; - this.limelight = Limelight.getDefault(); + this.limelight = limelight; pid_controller.setTolerance(SHOOTER_CONSTANTS.ACCEPTABLE_SPEED_ERROR); } diff --git a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java index 3aad75a..613ffce 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TrajectoryController.java @@ -13,15 +13,10 @@ import org.team4159.frc.robot.Constants; import org.team4159.frc.robot.subsystems.Drivetrain; +import org.team4159.lib.control.ControlLoop; import org.team4159.lib.control.signal.DriveSignal; -public class TrajectoryController { - private enum State { - FOLLOWING, - IDLE - } - private State state = State.IDLE; - +public class TrajectoryController implements ControlLoop { private Drivetrain drivetrain; private Trajectory trajectory_to_follow; @@ -40,62 +35,46 @@ public TrajectoryController(Drivetrain drivetrain) { this.drivetrain = drivetrain; } + @Override public void update() { - DriveSignal signal = DriveSignal.NEUTRAL; - - switch (state) { - case FOLLOWING: - if (timer.hasPeriodPassed(trajectory_to_follow.getTotalTimeSeconds())) { - state = State.IDLE; - break; - } + double cur_time = timer.get(); + double dt = cur_time - prev_time; - double cur_time = timer.get(); - double dt = cur_time - prev_time; + Pose2d drivetrain_pose = drivetrain.getPose(); + Trajectory.State trajectory_sample = trajectory_to_follow.sample(cur_time); - Pose2d drivetrain_pose = drivetrain.getPose(); - Trajectory.State trajectory_sample = trajectory_to_follow.sample(cur_time); + var target_wheel_speeds = kinematics.toWheelSpeeds( + controller.calculate(drivetrain_pose, trajectory_sample)); - var target_wheel_speeds = kinematics.toWheelSpeeds( - controller.calculate(drivetrain_pose, trajectory_sample)); + double left_speed_setpoint = target_wheel_speeds.leftMetersPerSecond; + double right_speed_setpoint = target_wheel_speeds.rightMetersPerSecond; - double left_speed_setpoint = target_wheel_speeds.leftMetersPerSecond; - double right_speed_setpoint = target_wheel_speeds.rightMetersPerSecond; + double left_feed_forward = + feedforward.calculate(left_speed_setpoint, + (left_speed_setpoint - prev_speeds.leftMetersPerSecond) / dt); + double right_feed_forward = + feedforward.calculate(right_speed_setpoint, + (right_speed_setpoint - prev_speeds.rightMetersPerSecond) / dt); - double left_feed_forward = - feedforward.calculate(left_speed_setpoint, - (left_speed_setpoint - prev_speeds.leftMetersPerSecond) / dt); - double right_feed_forward = - feedforward.calculate(right_speed_setpoint, - (right_speed_setpoint - prev_speeds.rightMetersPerSecond) / dt); + double left_PID = pid_left.calculate(drivetrain.getWheelSpeeds().leftMetersPerSecond, left_speed_setpoint); + double right_PID = pid_right.calculate(drivetrain.getWheelSpeeds().rightMetersPerSecond, right_speed_setpoint); - double left_PID = pid_left.calculate(drivetrain.getWheelSpeeds().leftMetersPerSecond, left_speed_setpoint); - double right_PID = pid_right.calculate(drivetrain.getWheelSpeeds().rightMetersPerSecond, right_speed_setpoint); + double left_output = left_feed_forward + left_PID; - double left_output = left_feed_forward + left_PID; + double right_output = right_feed_forward + right_PID; - double right_output = right_feed_forward + right_PID; - - prev_time = cur_time; - prev_speeds = target_wheel_speeds; - - signal = DriveSignal.fromVolts(left_output, right_output); - break; - case IDLE: - // shouldn't be getting called - break; - } + prev_time = cur_time; + prev_speeds = target_wheel_speeds; + DriveSignal signal = DriveSignal.fromVolts(left_output, right_output); drivetrain.drive(signal); } - public boolean isIdle() { - return state == State.IDLE; + public boolean isComplete() { + return timer.hasPeriodPassed(trajectory_to_follow.getTotalTimeSeconds()); } public void setTrajectory(Trajectory trajectory) { - state = State.FOLLOWING; - Transform2d transform = drivetrain.getPose().minus(trajectory.getInitialPose()); trajectory = trajectory.transformBy(transform); trajectory_to_follow = trajectory; diff --git a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java index 33fe32b..1fe494e 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java @@ -14,7 +14,7 @@ private enum State { SEEKING_TARGET, FOUND_TARGET, RECOVERING, - OPEN_LOOP + IDLE } private State last_state; private State state = State.ZEROING; @@ -46,18 +46,20 @@ public void update() { turret.setEncoderPosition(TURRET_CONSTANTS.REVERSE_POSITION); } + if (isTurretOutOfSafeRange() && state != State.ZEROING) { + setState(State.RECOVERING); + } + switch (state) { case ZEROING: - if (turret.isReverseLimitSwitchClosed()) { - setState(State.OPEN_LOOP); + if (turret.isForwardLimitSwitchClosed()) { + setState(State.IDLE); } else { turret.setRawSpeed(TURRET_CONSTANTS.ZEROING_SPEED); } break; case SEEKING_TARGET: - if (isTurretOutOfSafeRange()) { - setState(State.RECOVERING); - } else if (limelight.isTargetVisible()){ + if (limelight.isTargetVisible()){ setState(State.FOUND_TARGET); } else { turret.setRawSpeed(seeking_direction * TURRET_CONSTANTS.SEEKING_SPEED); @@ -75,9 +77,7 @@ public void update() { } break; case FOUND_TARGET: - if (isTurretOutOfSafeRange()) { - setState(State.RECOVERING); - } else if (!limelight.isTargetVisible()) { + if (!limelight.isTargetVisible()) { setState(State.SEEKING_TARGET); } else { turret.setRawSpeed(pid_controller.calculate(limelight.getTargetHorizontalOffset())); @@ -92,10 +92,7 @@ public void update() { setState(last_state); } break; - case OPEN_LOOP: - if (isTurretOutOfSafeRange()) { - setState(State.RECOVERING); - } + case IDLE: break; } } @@ -112,18 +109,16 @@ public void startSeeking() { seeking_direction = -1; seeking_range = TURRET_CONSTANTS.STARTING_SEEKING_RANGE; seeking_starting_position = turret.getPosition(); - state = State.SEEKING_TARGET; } public void startRecovering() { last_state = state; - state = State.RECOVERING; } public void idle() { - setState(State.OPEN_LOOP); + setState(State.IDLE); } private void setState(State state) { diff --git a/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java index ffcd783..ffa63ec 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java @@ -3,27 +3,29 @@ import org.team4159.frc.robot.controllers.ArmController; import org.team4159.frc.robot.subsystems.Feeder; import org.team4159.frc.robot.subsystems.Intake; +import org.team4159.lib.control.ControlLoop; import static org.team4159.frc.robot.Constants.*; -public class IntakeController { +public class IntakeController implements ControlLoop { private enum State { STOWED, // arm / intake is stowing / stowed DEPLOYING, // arm is deploying, intake is not being run INTAKING // arm is deployed, intake is being run } - private State state; + private State state = State.STOWED; private ArmController arm_controller; private Intake intake; private Feeder feeder; - public IntakeController(ArmController arm, Intake intake, Feeder feeder) { + public IntakeController(ArmController arm_controller, Intake intake, Feeder feeder) { this.arm_controller = arm_controller; this.intake = intake; this.feeder = feeder; } + @Override public void update() { int arm_setpoint = ARM_CONSTANTS.UP_POSITION; double intake_speed = 0, diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index 51785b3..d93d6e7 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -1,19 +1,18 @@ package org.team4159.frc.robot.subsystems; import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.motorcontrol.NeutralMode; +import org.team4159.frc.robot.controllers.DrivetrainController; import org.team4159.frc.robot.controllers.TrajectoryController; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.control.signal.filters.LowPassFilterSource; @@ -22,12 +21,6 @@ import static org.team4159.frc.robot.Constants.*; public class Drivetrain extends SubsystemBase { - private enum State { - PATH_FOLLOWING, - OPEN_LOOP - } - private State state = State.OPEN_LOOP; - private CardinalFX left_front_falcon, left_rear_falcon, right_front_falcon, right_rear_falcon; private SpeedControllerGroup left_falcons; private SpeedControllerGroup right_falcons; @@ -36,9 +29,7 @@ private enum State { private PigeonIMU pigeon; private LowPassFilterSource filtered_heading; - private TrajectoryController trajectory_controller; - private DriveSignal drive_signal; - private boolean is_oriented_forward = true; + private DrivetrainController drivetrain_controller; public Drivetrain() { left_front_falcon = new CardinalFX(CAN_IDS.LEFT_FRONT_FALCON, NeutralMode.Coast); @@ -68,15 +59,11 @@ public Drivetrain() { odometry = new DifferentialDriveOdometry(new Rotation2d(0)); filtered_heading = new LowPassFilterSource(pigeon::getFusedHeading, 10); - trajectory_controller = new TrajectoryController(this); + drivetrain_controller = new DrivetrainController(this); zeroSensors(); } - public void flipOrientation() { - is_oriented_forward = !is_oriented_forward; - } - @Override public void periodic() { odometry.update( @@ -86,36 +73,12 @@ public void periodic() { ); filtered_heading.get(); - SmartDashboard.putNumber("X", getPose().getTranslation().getX()); - SmartDashboard.putNumber("Y", getPose().getTranslation().getY()); - SmartDashboard.putNumber("Angle", getDirection()); - SmartDashboard.putNumber("Left Encoder", getLeftDistance()); - SmartDashboard.putNumber("Right Encoder", getRightDistance()); - - switch (state) { - case PATH_FOLLOWING: - trajectory_controller.update(); - if (trajectory_controller.isIdle()) { - state = State.OPEN_LOOP; - } - case OPEN_LOOP: - DriveSignal filtered_signal = drive_signal; - if (is_oriented_forward) { - filtered_signal = drive_signal.invert(); - } - left_falcons.set(filtered_signal.left); - right_falcons.set(filtered_signal.right); - break; - } + drivetrain_controller.update(); } - public void drive(DriveSignal drive_signal) { - this.drive_signal = drive_signal; - } - - public void followTrajectory(Trajectory trajectory) { - state = State.PATH_FOLLOWING; - trajectory_controller.setTrajectory(trajectory); + public void drive(DriveSignal signal) { + left_falcons.set(signal.left); + right_falcons.set(signal.right); } public void resetEncoders() { @@ -134,8 +97,6 @@ public void zeroSensors() { new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), Rotation2d.fromDegrees(0) ); - - System.out.println(getDirection()); } public DifferentialDriveWheelSpeeds getWheelSpeeds() { @@ -175,4 +136,8 @@ public double getRightVelocity() { public Pose2d getPose() { return odometry.getPoseMeters(); } + + public DrivetrainController getController() { + return drivetrain_controller; + } } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index 4695e79..b3b3533 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java @@ -6,6 +6,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; +import org.team4159.frc.robot.controllers.ShooterController; import org.team4159.lib.hardware.EnhancedEncoder; import org.team4159.lib.hardware.controller.ctre.CardinalSPX; import org.team4159.lib.hardware.controller.ctre.CardinalSRX; @@ -19,6 +20,8 @@ public class Shooter extends SubsystemBase { private EnhancedEncoder shooter_encoder; + private ShooterController shooter_controller; + public Shooter() { primary_shooter_talon = new CardinalSRX(CAN_IDS.PRIMARY_SHOOTER_TALON, NeutralMode.Coast); shooter_talon_two = new CardinalSRX(CAN_IDS.SHOOTER_TALON_TWO, NeutralMode.Coast); @@ -38,6 +41,13 @@ public Shooter() { SHOOTER_CONSTANTS.IS_ENCODER_REVERSED, SHOOTER_CONSTANTS.ENCODER_ENCODING_TYPE )); + + shooter_controller = new ShooterController(this); + } + + @Override + public void periodic() { + shooter_controller.update(); } public void setRawSpeed(double speed) { @@ -51,4 +61,8 @@ public void setRawVoltage(double voltage) { public double getSpeed() { return shooter_encoder.getVelocity(); } + + public ShooterController getController() { + return shooter_controller; + } } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java index 1756f72..6023499 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.NeutralMode; +import org.team4159.frc.robot.controllers.TurretController; import org.team4159.lib.hardware.controller.ctre.CardinalFX; import static org.team4159.frc.robot.Constants.*; @@ -15,12 +16,20 @@ public class Turret extends SubsystemBase { private TalonFX turret_falcon; + private TurretController turret_controller; + public Turret() { turret_falcon = new CardinalFX(CAN_IDS.TURRET_FALCON, NeutralMode.Brake); turret_falcon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); turret_falcon.setInverted(false); - // FIGURE OUT DIRECTION LATER + + turret_controller = new TurretController(this); + } + + @Override + public void periodic() { + turret_controller.update(); } public void setRawSpeed(double speed) { @@ -48,4 +57,8 @@ public boolean isForwardLimitSwitchClosed() { public boolean isReverseLimitSwitchClosed() { return turret_falcon.isRevLimitSwitchClosed() == 1; } + + public TurretController getController() { + return turret_controller; + } } From 2b2eb01c10be41757577662d5dea84685446aaf3 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 00:38:09 -0800 Subject: [PATCH 17/21] builds --- src/main/java/org/team4159/frc/robot/subsystems/Shooter.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index b3b3533..37e0f43 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java @@ -8,6 +8,7 @@ import org.team4159.frc.robot.controllers.ShooterController; import org.team4159.lib.hardware.EnhancedEncoder; +import org.team4159.lib.hardware.Limelight; import org.team4159.lib.hardware.controller.ctre.CardinalSPX; import org.team4159.lib.hardware.controller.ctre.CardinalSRX; @@ -42,7 +43,7 @@ public Shooter() { SHOOTER_CONSTANTS.ENCODER_ENCODING_TYPE )); - shooter_controller = new ShooterController(this); + shooter_controller = new ShooterController(this, Limelight.getDefault()); } @Override From 7d644d51480f2ad732bb072ff432d603b02afbe8 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 17:45:43 -0800 Subject: [PATCH 18/21] 2/22 --- .../org/team4159/frc/robot/Constants.java | 17 +++---- .../java/org/team4159/frc/robot/Robot.java | 10 +++++ .../team4159/frc/robot/RobotContainer.java | 45 ++++++++++--------- .../frc/robot/controllers/ArmController.java | 9 +++- .../robot/controllers/ShooterController.java | 9 +++- .../robot/controllers/TurretController.java | 10 +++-- .../controllers/complex/IntakeController.java | 4 +- .../frc/robot/subsystems/Drivetrain.java | 5 +-- .../team4159/frc/robot/subsystems/Feeder.java | 29 +++++++----- .../team4159/frc/robot/subsystems/Intake.java | 1 + .../team4159/frc/robot/subsystems/Neck.java | 7 +-- .../frc/robot/subsystems/Shooter.java | 26 ++++++----- .../team4159/frc/robot/subsystems/Turret.java | 4 +- .../frc/robot/subsystems/ShooterTest.java | 32 +++++++++++-- 14 files changed, 137 insertions(+), 71 deletions(-) diff --git a/src/main/java/org/team4159/frc/robot/Constants.java b/src/main/java/org/team4159/frc/robot/Constants.java index db7e874..64b80b5 100644 --- a/src/main/java/org/team4159/frc/robot/Constants.java +++ b/src/main/java/org/team4159/frc/robot/Constants.java @@ -38,14 +38,14 @@ public static final class BUTTON_IDS { public static final int ENABLE_SHOOTER = T16000M.TRIGGER_ID; public static final int FLIP_ROBOT_ORIENTATION = T16000M.TOP_MIDDLE_BTN_ID; public static final int TOGGLE_ARM = T16000M.TOP_RIGHT_BTN_ID; - public static final int RUN_ALL_INTAKE_SUBSYSTEMS = T16000M.PRIMARY_BOTTOM_MIDDLE_BTN_ID; public static final int LIMELIGHT_SEEK = T16000M.PRIMARY_TOP_INNER_BTN_ID; // Debug buttons public static final int RUN_INTAKE = T16000M.TOP_LEFT_BTN_ID, RUN_FEEDER = T16000M.PRIMARY_TOP_OUTER_BTN_ID, - RUN_NECK = T16000M.PRIMARY_TOP_MIDDLE_BTN_ID; + RUN_NECK = T16000M.PRIMARY_TOP_MIDDLE_BTN_ID, + RUN_SHOOTER = T16000M.PRIMARY_BOTTOM_MIDDLE_BTN_ID; } } } @@ -66,8 +66,8 @@ public static final class CAN_IDS { public static final int INTAKE_SPARK = 2; // unknown // TODO: Change! public static final int LOWER_FEEDER_TALON = 5; - public static final int UPPER_FEEDER_SPARK = 6; - public static final int NECK_SPARK = 4; + public static final int UPPER_FEEDER_SPARK = 4; + public static final int NECK_SPARK = 3; public static final int PIGEON = 0; } @@ -101,14 +101,14 @@ public static final class DRIVE_CONSTANTS { } public static final class SHOOTER_CONSTANTS { - public static final int COUNTS_PER_SECOND_TO_RPM = ENCODERS.THROUGH_BORE_ENCODER_CPR * 60; + public static final double COUNTS_PER_SECOND_TO_RPM = (1.0 / ENCODERS.THROUGH_BORE_ENCODER_CPR) * 60; @SuppressWarnings("PointlessArithmeticExpression") // 1 RPM i think public static final int ACCEPTABLE_SPEED_ERROR = 1 * ENCODERS.THROUGH_BORE_ENCODER_CPR / 60; // counts per second - public static final int ENCODER_CHANNEL_A_PORT = 2; - public static final int ENCODER_CHANNEL_B_PORT = 3; + public static final int ENCODER_CHANNEL_A_PORT = 4; + public static final int ENCODER_CHANNEL_B_PORT = 5; public static final boolean IS_ENCODER_REVERSED = true; public static final EncodingType ENCODER_ENCODING_TYPE = EncodingType.k4X; @@ -186,7 +186,8 @@ public static final class INTAKE_CONSTANTS { } public static final class FEEDER_CONSTANTS { - public static final double FEEDING_SPEED = 1; + public static final double FLOOR_FEEDING_SPEED = 1; + public static final double TOWER_FEEDING_SPEED = 0.3; } public static final class FIELD_CONSTANTS { diff --git a/src/main/java/org/team4159/frc/robot/Robot.java b/src/main/java/org/team4159/frc/robot/Robot.java index 33c9dcf..db3a7d2 100644 --- a/src/main/java/org/team4159/frc/robot/Robot.java +++ b/src/main/java/org/team4159/frc/robot/Robot.java @@ -16,6 +16,16 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); } + @Override + public void autonomousInit() { + robot_container.zeroSubsystems(); + } + + @Override + public void teleopInit() { + robot_container.zeroSubsystems(); + } + @Override public void teleopPeriodic() { robot_container.updateSubsystemInputs(); diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index 43d6dc6..7b6db20 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -2,12 +2,11 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.*; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.frc.robot.controllers.complex.IntakeController; import org.team4159.frc.robot.subsystems.*; +import org.team4159.lib.hardware.Limelight; import static org.team4159.frc.robot.Constants.*; @@ -20,43 +19,40 @@ public class RobotContainer { private final Arm arm = new Arm(); private final Turret turret = new Turret(); + private final Limelight limelight = Limelight.getDefault(); + private final IntakeController intake_controller = new IntakeController(arm.getController(), intake, feeder); private final Joystick left_joy = new Joystick(CONTROLS.LEFT_JOY.USB_PORT); private final Joystick right_joy = new Joystick(CONTROLS.RIGHT_JOY.USB_PORT); private final Joystick secondary_joy = new Joystick(CONTROLS.SECONDARY_JOY.USB_PORT); - private final AutoSelector auto_selector = new AutoSelector(); + private final AutoSelector auto_selector = new AutoSelector(); public RobotContainer() { - configureCamera(); + configureCameras(); } - private void configureCamera() { - CameraServer.getInstance().startAutomaticCapture(); + private void configureCameras() { + //CameraServer.getInstance().startAutomaticCapture(); + limelight.setLEDMode(Limelight.LEDMode.ForceOff); } - private void configureButtonBindings() { - new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_ALL_INTAKE_SUBSYSTEMS) - .whenPressed(new ParallelCommandGroup( - new InstantCommand(intake::intake, intake), - new InstantCommand(feeder::feed, feeder), - new InstantCommand(() -> shooter.setRawSpeed(1), shooter))) - .whenReleased(new ParallelCommandGroup( - new InstantCommand(intake::stop, intake), - new InstantCommand(feeder::stop, feeder), - new InstantCommand(() -> shooter.setRawSpeed(0), shooter))); + public void zeroSubsystems() { + //arm.getController().startZeroing(); + //turret.getController().startZeroing(); } public void updateSubsystemInputs() { updateDrivetrainInputs(); - updateFeederInputs(); // TODO: switch to IntakeController when done testing in isolation - updateArmInputs(); + // updateArmInputs(); updateIntakeInputs(); + updateFeederInputs(); updateNeckInputs(); + updateShooterInputs(); } public void updateControllerInputs() { @@ -78,7 +74,7 @@ public void updateDrivetrainInputs() { } public void updateFeederInputs() { - if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_FEEDER)) { + if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_SHOOTER)) { feeder.feed(); } else { feeder.stop(); @@ -94,13 +90,22 @@ public void updateIntakeInputs() { } public void updateNeckInputs() { - if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_NECK)) { + if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_SHOOTER)) { neck.neck(); } else { neck.stop(); } } + public void updateShooterInputs() { + if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_SHOOTER)) { + shooter.setRawSpeed(1); + shooter.getController().writeToSmartDashboard(); + } else { + shooter.stop(); + } + } + public void updateIntakeControllerInputs() { if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_INTAKE)) { intake_controller.intake(); diff --git a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java index 51731d0..a4c179c 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ArmController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ArmController.java @@ -9,11 +9,12 @@ public class ArmController implements ControlLoop { private enum State { + IDLE, ZEROING, CLOSED_LOOP, ESTOP } - private State state = State.ZEROING; + private State state = State.IDLE; private Arm arm; @@ -33,6 +34,8 @@ public ArmController(Arm arm) { @Override public void update() { switch (state) { + case IDLE: + break; case ZEROING: arm.setRawSpeed(ARM_CONSTANTS.ZEROING_SPEED); if (arm.isLimitSwitchClosed()) { @@ -51,6 +54,10 @@ public boolean isAtSetpoint() { return pid_controller.atSetpoint(); } + public void startZeroing() { + state = State.ZEROING; + } + public void setSetpoint(int setpoint) { pid_controller.setSetpoint(setpoint); } diff --git a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java index eabf9d9..f00f3a8 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/ShooterController.java @@ -39,6 +39,7 @@ public void update() { shooter.setRawVoltage(0); break; case CLOSED_LOOP: + /* if (limelight.isTargetVisible()) { // In theory, the velocity of the ball should scale linearly with RPM, which in turn scales linearly with horizontal distance (I think) // I'll update this math later @@ -50,6 +51,7 @@ public void update() { pid_controller.setSetpoint(target_rpm); } + */ shooter.setRawVoltage(pid_controller.calculate(shooter.getSpeed())); break; @@ -62,6 +64,10 @@ public double getDistanceToVisionTarget() { return LIMELIGHT_CONSTANTS.VISION_TARGET_HEIGHT / Math.tan(total_angle_to_target); } + public void setTargetSpeed(double speed) { + pid_controller.setSetpoint(speed); + } + public boolean isAtTargetSpeed() { return pid_controller.atSetpoint(); } @@ -76,8 +82,9 @@ public void writeToSmartDashboard() { SmartDashboard.putNumber( "current_shooter_speed_rpm", - shooter.getSpeed() * SHOOTER_CONSTANTS.COUNTS_PER_SECOND_TO_RPM + shooter.getSpeed() ); + System.out.println("current_shooter_speed_rpm: " + shooter.getSpeed() * SHOOTER_CONSTANTS.COUNTS_PER_SECOND_TO_RPM); double distance = getDistanceToVisionTarget(); diff --git a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java index 1fe494e..9471276 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/TurretController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/TurretController.java @@ -17,7 +17,7 @@ private enum State { IDLE } private State last_state; - private State state = State.ZEROING; + private State state = State.IDLE; private Turret turret; private Limelight limelight; @@ -30,9 +30,9 @@ private enum State { TURRET_CONSTANTS.LIMELIGHT_TURN_kD ); - public TurretController(Turret turret) { + public TurretController(Turret turret, Limelight limelight) { this.turret = turret; - this.limelight = Limelight.getDefault(); + this.limelight = limelight; limelight.setLEDMode(Limelight.LEDMode.ForceOn); pid_controller.reset(); @@ -105,6 +105,10 @@ public boolean isTurretOutOfSafeRange() { return Math.abs(turret.getPosition()) < TURRET_CONSTANTS.SAFE_FORWARD_POSITION; } + public void startZeroing() { + state = State.ZEROING; + } + public void startSeeking() { seeking_direction = -1; seeking_range = TURRET_CONSTANTS.STARTING_SEEKING_RANGE; diff --git a/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java index ffa63ec..f27e629 100644 --- a/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java +++ b/src/main/java/org/team4159/frc/robot/controllers/complex/IntakeController.java @@ -43,13 +43,13 @@ public void update() { case INTAKING: arm_setpoint = ARM_CONSTANTS.DOWN_POSITION; intake_speed = INTAKE_CONSTANTS.INTAKE_SPEED; - feeder_speed = FEEDER_CONSTANTS.FEEDING_SPEED; + feeder_speed = FEEDER_CONSTANTS.TOWER_FEEDING_SPEED; break; } arm_controller.setSetpoint(arm_setpoint); intake.setRawSpeed(intake_speed); - feeder.setRawSpeed(feeder_speed); + feeder.setRawTowerSpeed(feeder_speed); } public void intake() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java index d93d6e7..340f8d7 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java @@ -13,7 +13,6 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.team4159.frc.robot.controllers.DrivetrainController; -import org.team4159.frc.robot.controllers.TrajectoryController; import org.team4159.lib.control.signal.DriveSignal; import org.team4159.lib.control.signal.filters.LowPassFilterSource; import org.team4159.lib.hardware.controller.ctre.CardinalFX; @@ -51,8 +50,8 @@ public Drivetrain() { right_rear_falcon ); - left_falcons.setInverted(true); - right_falcons.setInverted(false); + left_falcons.setInverted(false); + right_falcons.setInverted(true); pigeon = new PigeonIMU(CAN_IDS.PIGEON); diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java index 8ce0ecb..4a22a46 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Feeder.java @@ -3,8 +3,9 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.CANSparkMax; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMaxLowLevel; +import com.revrobotics.CANSparkMax; import org.team4159.lib.hardware.controller.ctre.CardinalSRX; import org.team4159.lib.hardware.controller.rev.CardinalMAX; @@ -12,27 +13,31 @@ import static org.team4159.frc.robot.Constants.*; public class Feeder extends SubsystemBase { - private SpeedControllerGroup feeder_motors; + private CardinalMAX tower_spark; + private CardinalSRX floor_talon; public Feeder() { - CardinalMAX upper_feeder_spark = new CardinalMAX(CAN_IDS.UPPER_FEEDER_SPARK, CANSparkMax.IdleMode.kCoast); - upper_feeder_spark.setInverted(true); + tower_spark = new CardinalMAX(CAN_IDS.UPPER_FEEDER_SPARK, CANSparkMax.IdleMode.kCoast, 40, CANSparkMaxLowLevel.MotorType.kBrushed); + floor_talon = new CardinalSRX(CAN_IDS.LOWER_FEEDER_TALON, NeutralMode.Brake); + + tower_spark.setInverted(true); + } - feeder_motors = new SpeedControllerGroup( - new CardinalSRX(CAN_IDS.LOWER_FEEDER_TALON, NeutralMode.Brake), - upper_feeder_spark - ); + public void setRawTowerSpeed(double speed) { + tower_spark.set(speed); } - public void setRawSpeed(double speed) { - feeder_motors.set(speed); + public void setRawFloorSpeed(double speed) { + floor_talon.set(speed); } public void feed() { - setRawSpeed(FEEDER_CONSTANTS.FEEDING_SPEED); + setRawTowerSpeed(FEEDER_CONSTANTS.TOWER_FEEDING_SPEED); + setRawFloorSpeed(FEEDER_CONSTANTS.FLOOR_FEEDING_SPEED); } public void stop() { - setRawSpeed(0); + setRawTowerSpeed(0); + setRawFloorSpeed(0); } } diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java index e777eae..b236f1c 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Intake.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Intake.java @@ -12,6 +12,7 @@ public class Intake extends SubsystemBase { public Intake() { intake_spark = new CardinalMAX(CAN_IDS.INTAKE_SPARK, CANSparkMax.IdleMode.kCoast); + intake_spark.setInverted(true); } public void setRawSpeed(double speed) { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java index a38535b..2d6c966 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Neck.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Neck.java @@ -2,20 +2,21 @@ import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team4159.lib.hardware.controller.rev.CardinalMAX; import static org.team4159.frc.robot.Constants.*; public class Neck extends SubsystemBase { - private CANSparkMax intake_spark; + private CANSparkMax neck_spark; public Neck() { - intake_spark = new CardinalMAX(CAN_IDS.NECK_SPARK, CANSparkMax.IdleMode.kCoast); + neck_spark = new CardinalMAX(CAN_IDS.NECK_SPARK, CANSparkMax.IdleMode.kCoast, 40, CANSparkMaxLowLevel.MotorType.kBrushed); } public void setRawSpeed(double speed) { - intake_spark.set(speed); + neck_spark.set(speed); } public void neck() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java index 37e0f43..99e691a 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Shooter.java @@ -17,7 +17,6 @@ public class Shooter extends SubsystemBase { private CardinalSRX primary_shooter_talon, shooter_talon_two; private CardinalSPX shooter_victor_one, shooter_victor_two; - private SpeedControllerGroup shooter_motors; private EnhancedEncoder shooter_encoder; @@ -26,15 +25,14 @@ public class Shooter extends SubsystemBase { public Shooter() { primary_shooter_talon = new CardinalSRX(CAN_IDS.PRIMARY_SHOOTER_TALON, NeutralMode.Coast); shooter_talon_two = new CardinalSRX(CAN_IDS.SHOOTER_TALON_TWO, NeutralMode.Coast); - shooter_victor_one = new CardinalSPX(CAN_IDS.SHOOTER_VICTOR_ONE, NeutralMode.Coast); - shooter_victor_two = new CardinalSPX(CAN_IDS.SHOOTER_VICTOR_TWO, NeutralMode.Coast); + //shooter_victor_one = new CardinalSPX(CAN_IDS.SHOOTER_VICTOR_ONE, NeutralMode.Coast); + //shooter_victor_two = new CardinalSPX(CAN_IDS.SHOOTER_VICTOR_TWO, NeutralMode.Coast); - shooter_motors = new SpeedControllerGroup( - primary_shooter_talon, - shooter_talon_two, - shooter_victor_one, - shooter_victor_two - ); + //primary_shooter_talon.configOpenloopRamp(1); + + shooter_talon_two.follow(primary_shooter_talon); + //shooter_victor_one.follow(primary_shooter_talon); + //shooter_victor_two.follow(primary_shooter_talon); shooter_encoder = new EnhancedEncoder(new Encoder( SHOOTER_CONSTANTS.ENCODER_CHANNEL_A_PORT, @@ -48,15 +46,19 @@ public Shooter() { @Override public void periodic() { - shooter_controller.update(); + // shooter_controller.update(); } public void setRawSpeed(double speed) { - shooter_motors.set(speed); + primary_shooter_talon.set(speed); } public void setRawVoltage(double voltage) { - shooter_motors.setVoltage(voltage); + primary_shooter_talon.setVoltage(voltage); + } + + public void stop() { + setRawSpeed(0); } public double getSpeed() { diff --git a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java index 6023499..aff91be 100644 --- a/src/main/java/org/team4159/frc/robot/subsystems/Turret.java +++ b/src/main/java/org/team4159/frc/robot/subsystems/Turret.java @@ -1,6 +1,5 @@ package org.team4159.frc.robot.subsystems; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -9,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.team4159.frc.robot.controllers.TurretController; +import org.team4159.lib.hardware.Limelight; import org.team4159.lib.hardware.controller.ctre.CardinalFX; import static org.team4159.frc.robot.Constants.*; @@ -24,7 +24,7 @@ public Turret() { turret_falcon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); turret_falcon.setInverted(false); - turret_controller = new TurretController(this); + turret_controller = new TurretController(this, Limelight.getDefault()); } @Override diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java index 0094f84..9139b0a 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ShooterTest.java @@ -1,27 +1,51 @@ package org.team4159.frc.robot.subsystems; +import org.team4159.lib.math.MathUtil; +import org.team4159.lib.math.physics.DCMotorModel; +import org.team4159.lib.simulation.MotorModels; import org.team4159.lib.simulation.mocks.SubsystemMock; +import static org.team4159.lib.simulation.SimulationConfiguration.*; +import static org.team4159.frc.robot.Constants.*; + public class ShooterTest { public class MockShooter extends Shooter implements SubsystemMock { + private double inertia = 0; + private DCMotorModel[] motors; + + private double applied_voltage = 0; + private double angular_velocity = 0; // rad / s + + MockShooter() { + motors = new DCMotorModel[] { + MotorModels._775pro, + MotorModels._775pro + }; + } + @Override public void simulate(double dt) { - + double torque = 0; + for (DCMotorModel motor : motors) { + torque += motor.getTorqueForVoltage(angular_velocity, applied_voltage); + } + double angular_acceleration = torque / inertia; + angular_velocity += angular_acceleration * LOSS_COEFFICIENT * dt; } @Override public void setRawSpeed(double speed) { - super.setRawSpeed(speed); + setRawVoltage(speed * 12); } @Override public void setRawVoltage(double voltage) { - super.setRawVoltage(voltage); + applied_voltage = voltage; } @Override public double getSpeed() { - return super.getSpeed(); + return angular_velocity / MathUtil.kTau * ENCODERS.THROUGH_BORE_ENCODER_CPR; } } } From c80960071ca96c2447909c6c793baabd4cc9d89e Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 18:28:50 -0800 Subject: [PATCH 19/21] start zeroing in test --- src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java index f9696a3..fdb6b6b 100644 --- a/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java +++ b/src/test/java/org/team4159/frc/robot/subsystems/ArmTest.java @@ -94,12 +94,14 @@ public void reset() { @Test public void Zeroes() { arm.setRealStartingPosition(100); + arm_controller.startZeroing(); SimulationRunner.simulate(arm, arm_controller, 5); Assert.assertEquals(0, arm.getRealPosition(), ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS); } @Test public void GoesToPosition() { + Zeroes(); arm_controller.setSetpoint(ARM_CONSTANTS.DOWN_POSITION); SimulationRunner.simulate(arm, arm_controller, 10); Assert.assertTrue(arm_controller.isAtSetpoint()); From 5694fe8afd24381f9f29bf5281f4a228334c98c6 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 21:49:10 -0800 Subject: [PATCH 20/21] simplify travis --- .travis.yml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/.travis.yml b/.travis.yml index eabc434..17d32f2 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,15 +6,6 @@ jobs: - stage: "Tests" name: "Unit Tests" script: ./gradlew test - - stage: "Report" - name: "Install Deepsource" - script: - - curl https://deepsource.io/cli | sh - - name: "Generate Report" - script: - - git clone https://github.com/dominikWin/badlogvis.git - - cd badlogvis - - cargo install os: - osx From e014732fa659cf4656e942e37dcd812119dda1d7 Mon Sep 17 00:00:00 2001 From: Kai Chang Date: Sat, 22 Feb 2020 21:51:52 -0800 Subject: [PATCH 21/21] change buttons --- src/main/java/org/team4159/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team4159/frc/robot/RobotContainer.java b/src/main/java/org/team4159/frc/robot/RobotContainer.java index 7b6db20..fdbe19f 100644 --- a/src/main/java/org/team4159/frc/robot/RobotContainer.java +++ b/src/main/java/org/team4159/frc/robot/RobotContainer.java @@ -74,7 +74,7 @@ public void updateDrivetrainInputs() { } public void updateFeederInputs() { - if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_SHOOTER)) { + if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_FEEDER)) { feeder.feed(); } else { feeder.stop(); @@ -90,7 +90,7 @@ public void updateIntakeInputs() { } public void updateNeckInputs() { - if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_SHOOTER)) { + if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_NECK)) { neck.neck(); } else { neck.stop();