From 4457f5b8047bf8bbbf2a6e69683d7578a01702c8 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Thu, 5 Aug 2021 16:30:26 -0600 Subject: [PATCH 01/18] add a shooter class with PID constants embarrassing moment when everyone sees you didn't do much :P --- src/main/java/frc/robot/Shooter.java | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 src/main/java/frc/robot/Shooter.java diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java new file mode 100644 index 0000000..55c4fad --- /dev/null +++ b/src/main/java/frc/robot/Shooter.java @@ -0,0 +1,18 @@ +package frc.robot; + +// import phoenix to control motors +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +// I assume the phoenix stuff has to do with the motors involved in the shooter? +// import com.ctre.phoenix.motorcontrol.ControlMode; +// anyway phoenix imports were here in the original + +public class Shooter { + // FPID ramps up motor speeds over time + // the fancy math terms basically mean it calculates how to get + // from the current speed to the desired speed + public static double F = 0.045; // constant added + public static double P = .6 // proportional (fraction multiplied) + public static double I = 0.0001; // integral (area under the curve) + public static double D = 10; // derivative (slope of the curve) +} \ No newline at end of file From 9cf19a44ad702084b732b715f221d63fc418dd48 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Thu, 12 Aug 2021 07:39:43 -0600 Subject: [PATCH 02/18] 0.6 instead of .6 --- src/main/java/frc/robot/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 55c4fad..17db975 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -12,7 +12,7 @@ public class Shooter { // the fancy math terms basically mean it calculates how to get // from the current speed to the desired speed public static double F = 0.045; // constant added - public static double P = .6 // proportional (fraction multiplied) + public static double P = 0.6 // proportional (fraction multiplied) public static double I = 0.0001; // integral (area under the curve) public static double D = 10; // derivative (slope of the curve) } \ No newline at end of file From bfaf38358e33ab9ab5ada5a4cc000d397ad78a9a Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Thu, 12 Aug 2021 15:34:06 -0600 Subject: [PATCH 03/18] update shooter mode comments --- src/main/java/frc/robot/Shooter.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 17db975..0e5de78 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -8,9 +8,12 @@ // anyway phoenix imports were here in the original public class Shooter { - // FPID ramps up motor speeds over time - // the fancy math terms basically mean it calculates how to get - // from the current speed to the desired speed + /* + There are two "ShooterModes": + PID (proportional integral derivative) changes speed over time + VOLTAGE gives a straight voltage amount to the motors. + */ + public static double F = 0.045; // constant added public static double P = 0.6 // proportional (fraction multiplied) public static double I = 0.0001; // integral (area under the curve) From b265649b5906e528a688f1372b1bd397d7fc52f2 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Thu, 12 Aug 2021 15:39:03 -0600 Subject: [PATCH 04/18] copy voltage constants totally not commit padding :P --- src/main/java/frc/robot/Shooter.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 0e5de78..5216041 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -14,8 +14,17 @@ public class Shooter { VOLTAGE gives a straight voltage amount to the motors. */ + // PID constants public static double F = 0.045; // constant added public static double P = 0.6 // proportional (fraction multiplied) public static double I = 0.0001; // integral (area under the curve) public static double D = 10; // derivative (slope of the curve) + + // Voltage constants + public static double VOLTAGE_TO_VELOCITY = 20480; // need to figure out units + public static double INITIAL_DESIRED_VELOCITY = INITIAL_SHOOTER_SPEED * VOLTAGE_TO_VELOCITY; + public static double VOLTAGE_INITIAL_VELOCITY_THRESHOLD = 5;//250 + public static double PID_INITIAL_VELOCITY_THRESHOLD = 5; + public static double VOLTAGE_VELOCITY_THRESHOLD = 130;//50 + public static double PID_VELOCITY_THRESHOLD = 70; } \ No newline at end of file From 02b7276963b7d09ea25067e35962404782a5f1eb Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Mon, 16 Aug 2021 21:16:53 -0600 Subject: [PATCH 05/18] voltage mode is more or less dead reckoning (I think) --- src/main/java/frc/robot/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 5216041..7890eb4 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -11,7 +11,7 @@ public class Shooter { /* There are two "ShooterModes": PID (proportional integral derivative) changes speed over time - VOLTAGE gives a straight voltage amount to the motors. + VOLTAGE gives a straight voltage amount to the motors (dead reckoning) */ // PID constants From 931b9d2a03e0019ac30ac4efe6c772c9440acb50 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 05:37:19 -0600 Subject: [PATCH 06/18] comment alignment --- src/main/java/frc/robot/Shooter.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 7890eb4..4bd248a 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -15,10 +15,10 @@ VOLTAGE gives a straight voltage amount to the motors (dead reckoning) */ // PID constants - public static double F = 0.045; // constant added - public static double P = 0.6 // proportional (fraction multiplied) + public static double F = 0.045; // constant added + public static double P = 0.6; // proportional (fraction multiplied) public static double I = 0.0001; // integral (area under the curve) - public static double D = 10; // derivative (slope of the curve) + public static double D = 10; // derivative (slope of the curve) // Voltage constants public static double VOLTAGE_TO_VELOCITY = 20480; // need to figure out units From c81cb3bd81ca08d288d7fbd59280d187ddd29b72 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 06:09:57 -0600 Subject: [PATCH 07/18] imports and comments --- src/main/java/frc/robot/Shooter.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 4bd248a..77b5722 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -1,7 +1,14 @@ package frc.robot; -// import phoenix to control motors +// Phoenix is used to control and configure the Falcon 500 motor(s) import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// https://www.ctr-electronics.com/downloads/api/java/html/enumcom_1_1ctre_1_1phoenix_1_1motorcontrol_1_1_control_mode.html +import com.ctre.phoenix.motorcontrol.ControlMode; +// for letting the shooter motor coast instead of brake in neutral mode +import com.ctre.phoenix.motorcontrol.NeutralMode; +// manage sensors +import com.ctre.phoenix.motorcontrol.TalonFXSensorCollection; + // I assume the phoenix stuff has to do with the motors involved in the shooter? // import com.ctre.phoenix.motorcontrol.ControlMode; From bcfb9fa1daf5181832275ea14816d2f64810fff6 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 06:12:36 -0600 Subject: [PATCH 08/18] define more constants --- src/main/java/frc/robot/Shooter.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 77b5722..a39015d 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -15,6 +15,9 @@ // anyway phoenix imports were here in the original public class Shooter { + // how fast the shooter motor spins (0 to 1) + public static double INITIAL_SHOOTER_SPEED = 1.00; + /* There are two "ShooterModes": PID (proportional integral derivative) changes speed over time @@ -26,12 +29,17 @@ VOLTAGE gives a straight voltage amount to the motors (dead reckoning) public static double P = 0.6; // proportional (fraction multiplied) public static double I = 0.0001; // integral (area under the curve) public static double D = 10; // derivative (slope of the curve) + // how fast we have to be going to start motors when on PID + public static double PID_INITIAL_VELOCITY_THRESHOLD = 5; + // how close we have to be to the real value (acceptable error) when on PID + public static double PID_VELOCITY_THRESHOLD = 70; // Voltage constants public static double VOLTAGE_TO_VELOCITY = 20480; // need to figure out units public static double INITIAL_DESIRED_VELOCITY = INITIAL_SHOOTER_SPEED * VOLTAGE_TO_VELOCITY; - public static double VOLTAGE_INITIAL_VELOCITY_THRESHOLD = 5;//250 - public static double PID_INITIAL_VELOCITY_THRESHOLD = 5; - public static double VOLTAGE_VELOCITY_THRESHOLD = 130;//50 - public static double PID_VELOCITY_THRESHOLD = 70; + // how fast we have to be going to start motors when on voltage + public static double VOLTAGE_INITIAL_VELOCITY_THRESHOLD = 5; + // how close we have to be to the real value (acceptable error) when on voltage + public static double VOLTAGE_VELOCITY_THRESHOLD = 130; + } \ No newline at end of file From 3a0944287fa09e26bb96cb9c2e93e098625fca53 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 15:56:04 -0600 Subject: [PATCH 09/18] remove old comment --- src/main/java/frc/robot/Shooter.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index a39015d..e56191a 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -9,11 +9,6 @@ // manage sensors import com.ctre.phoenix.motorcontrol.TalonFXSensorCollection; - -// I assume the phoenix stuff has to do with the motors involved in the shooter? -// import com.ctre.phoenix.motorcontrol.ControlMode; -// anyway phoenix imports were here in the original - public class Shooter { // how fast the shooter motor spins (0 to 1) public static double INITIAL_SHOOTER_SPEED = 1.00; From 7c0e664416083ed46d915e9e443720a7059a1bdf Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 16:39:01 -0600 Subject: [PATCH 10/18] speed range correction: .set() takes from -1 to 1 --- src/main/java/frc/robot/Shooter.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index e56191a..a324b6d 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -10,8 +10,7 @@ import com.ctre.phoenix.motorcontrol.TalonFXSensorCollection; public class Shooter { - // how fast the shooter motor spins (0 to 1) - public static double INITIAL_SHOOTER_SPEED = 1.00; + public static double INITIAL_SHOOTER_SPEED = 1.00; // -1 to 1 /* There are two "ShooterModes": From dfd1a50541f8757cebb871e3d49166ca7a0c78e8 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 16:42:03 -0600 Subject: [PATCH 11/18] modes and states enums --- src/main/java/frc/robot/Shooter.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index a324b6d..2995f78 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -17,6 +17,15 @@ public class Shooter { PID (proportional integral derivative) changes speed over time VOLTAGE gives a straight voltage amount to the motors (dead reckoning) */ + public enum ShooterModes { + VOLTAGE, PID + } + ShooterModes currentShooterMode; + + public enum ShooterStates { + NOT_MOVING, SHOOTING + } + private ShooterStates currentShooterState; // not sure why this is private // PID constants public static double F = 0.045; // constant added From c6b8c9a11ff50ace8789d9e9eaba64c6d813947c Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 16:43:22 -0600 Subject: [PATCH 12/18] add the mysterious integral zone --- src/main/java/frc/robot/Shooter.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 2995f78..4fef1f8 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -31,7 +31,9 @@ public enum ShooterStates { public static double F = 0.045; // constant added public static double P = 0.6; // proportional (fraction multiplied) public static double I = 0.0001; // integral (area under the curve) + public static int IZONE = 1400; // related to PID but I don't know how yet public static double D = 10; // derivative (slope of the curve) + // how fast we have to be going to start motors when on PID public static double PID_INITIAL_VELOCITY_THRESHOLD = 5; // how close we have to be to the real value (acceptable error) when on PID From ed1354503f631c85dd389921b363b43d0063a546 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 16:43:54 -0600 Subject: [PATCH 13/18] un-inline particular comment for consistency --- src/main/java/frc/robot/Shooter.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 4fef1f8..8ed8d60 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -40,7 +40,8 @@ public enum ShooterStates { public static double PID_VELOCITY_THRESHOLD = 70; // Voltage constants - public static double VOLTAGE_TO_VELOCITY = 20480; // need to figure out units + // need to figure out units + public static double VOLTAGE_TO_VELOCITY = 20480; public static double INITIAL_DESIRED_VELOCITY = INITIAL_SHOOTER_SPEED * VOLTAGE_TO_VELOCITY; // how fast we have to be going to start motors when on voltage public static double VOLTAGE_INITIAL_VELOCITY_THRESHOLD = 5; From da9e20102cc2b34576838b8ad54251499ab1ff2e Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 16:44:50 -0600 Subject: [PATCH 14/18] add enough for initilization --- src/main/java/frc/robot/Shooter.java | 37 +++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 8ed8d60..be291d3 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -2,7 +2,7 @@ // Phoenix is used to control and configure the Falcon 500 motor(s) import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -// https://www.ctr-electronics.com/downloads/api/java/html/enumcom_1_1ctre_1_1phoenix_1_1motorcontrol_1_1_control_mode.html +// control modes specify velocity profiles, motion profiling? import com.ctre.phoenix.motorcontrol.ControlMode; // for letting the shooter motor coast instead of brake in neutral mode import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -48,4 +48,39 @@ public enum ShooterStates { // how close we have to be to the real value (acceptable error) when on voltage public static double VOLTAGE_VELOCITY_THRESHOLD = 130; + private WPI_TalonFX shooterMotor; + private TalonFXSensorCollection sensors; + + private double shooterSpeed; + private double desiredVelocity; + + public Shooter() { + shooterMotor = new WPI_TalonFX(Constants.ShooterConstants.SHOOTER_MOTOR_ID); + sensors = new TalonFXSensorCollection(shooterMotor); + initialize(); + } + + public void initialize() { + // change motor spin direction (may be removed depending on the robot) + shooterMotor.setInverted(true); + // coast (as opposed to brake) when not giving voltage to the motor + shooterMotor.setNeutralMode(NeutralMode.Coast); + + // set up PID + // args for these functions are: slot, value, timeout in ms + shooterMotor.config_kF(0, F, 10); + shooterMotor.config_kP(0, P, 10); + shooterMotor.config_kI(0, I, 10); + shooterMotor.config_IntegralZone(0, IZONE, 10); + shooterMotor.config_kD(0, D, 10); + + setNotMoving(); + shooterSpeed = INITIAL_SHOOTER_SPEED; + desiredVelocity = INITIAL_DESIRED_VELOCITY; + } + + public void setNotMoving() { + shooterMotor.set(0); + currentShooterState = ShooterStates.NOT_MOVING; + } } \ No newline at end of file From 7d74e106bc23488777cd58cd17e920127163b3db Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Tue, 17 Aug 2021 20:17:07 -0600 Subject: [PATCH 15/18] add Shooter constants that I forgot to earlier --- src/main/java/frc/robot/Constants.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0d4a766..a680e4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,4 +21,9 @@ public class ClimberConstants{ public static final int CLIMBER_LEFT_MOTOR_ID = 1; public static final int CLIMBER_RIGHT_MOTOR_ID = 2; } + + // Shooter constants here + public class ShooterConstants{ + public static final int SHOOTER_MOTOR_ID = 1; + } } From d7ca325ee10ab095fecedee0a6a6b60fdb03d792 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Wed, 18 Aug 2021 05:10:18 -0600 Subject: [PATCH 16/18] whitespace and comments need to commit before rebasing onto edge --- src/main/java/frc/robot/Shooter.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index be291d3..011196f 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -2,7 +2,7 @@ // Phoenix is used to control and configure the Falcon 500 motor(s) import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -// control modes specify velocity profiles, motion profiling? +// PercentOutput/Current/Velocity/Position/Follower import com.ctre.phoenix.motorcontrol.ControlMode; // for letting the shooter motor coast instead of brake in neutral mode import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -40,8 +40,7 @@ public enum ShooterStates { public static double PID_VELOCITY_THRESHOLD = 70; // Voltage constants - // need to figure out units - public static double VOLTAGE_TO_VELOCITY = 20480; + public static double VOLTAGE_TO_VELOCITY = 20480; // position change per 100 ms public static double INITIAL_DESIRED_VELOCITY = INITIAL_SHOOTER_SPEED * VOLTAGE_TO_VELOCITY; // how fast we have to be going to start motors when on voltage public static double VOLTAGE_INITIAL_VELOCITY_THRESHOLD = 5; @@ -54,6 +53,7 @@ public enum ShooterStates { private double shooterSpeed; private double desiredVelocity; + public Shooter() { shooterMotor = new WPI_TalonFX(Constants.ShooterConstants.SHOOTER_MOTOR_ID); sensors = new TalonFXSensorCollection(shooterMotor); @@ -75,6 +75,7 @@ public void initialize() { shooterMotor.config_kD(0, D, 10); setNotMoving(); + shooterSpeed = INITIAL_SHOOTER_SPEED; desiredVelocity = INITIAL_DESIRED_VELOCITY; } From 44f19a73ca8107903b4bd0da7a5f4e24ea181026 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Wed, 18 Aug 2021 05:31:12 -0600 Subject: [PATCH 17/18] add setVoltage and setPID methods This commit also includes some velocity threshold setting to prepare for atDesiredVelocity and atInitialDesiredVelocity --- src/main/java/frc/robot/Shooter.java | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 011196f..86c6981 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -42,6 +42,7 @@ public enum ShooterStates { // Voltage constants public static double VOLTAGE_TO_VELOCITY = 20480; // position change per 100 ms public static double INITIAL_DESIRED_VELOCITY = INITIAL_SHOOTER_SPEED * VOLTAGE_TO_VELOCITY; + // how fast we have to be going to start motors when on voltage public static double VOLTAGE_INITIAL_VELOCITY_THRESHOLD = 5; // how close we have to be to the real value (acceptable error) when on voltage @@ -53,6 +54,10 @@ public enum ShooterStates { private double shooterSpeed; private double desiredVelocity; + // used to calculate if the (initial) desired velocity has been reached + private double initialVelocityThreshold; + private double velocityThreshold; + public Shooter() { shooterMotor = new WPI_TalonFX(Constants.ShooterConstants.SHOOTER_MOTOR_ID); @@ -61,6 +66,9 @@ public Shooter() { } public void initialize() { + // start in voltage mode + setVoltage(); + // change motor spin direction (may be removed depending on the robot) shooterMotor.setInverted(true); // coast (as opposed to brake) when not giving voltage to the motor @@ -80,8 +88,25 @@ public void initialize() { desiredVelocity = INITIAL_DESIRED_VELOCITY; } + // change to voltage mode + public void setVoltage() { + setVelocityThresholds(VOLTAGE_INITIAL_VELOCITY_THRESHOLD, VOLTAGE_VELOCITY_THRESHOLD); + currentShooterMode = ShooterModes.VOLTAGE; + } + + // change to PID mode + public void setPID() { + setVelocityThresholds(PID_INITIAL_VELOCITY_THRESHOLD, PID_VELOCITY_THRESHOLD); + currentShooterMode = ShooterModes.PID; + } + public void setNotMoving() { shooterMotor.set(0); currentShooterState = ShooterStates.NOT_MOVING; } + + public void setVelocityThresholds(double initialVelocityThreshold, double velocityThreshold) { + this.initialVelocityThreshold = initialVelocityThreshold; + this.velocityThreshold = velocityThreshold; + } } \ No newline at end of file From 90f1c9511864e1ad8236962903b4bdb17b777202 Mon Sep 17 00:00:00 2001 From: nonoesimposible Date: Wed, 18 Aug 2021 17:16:13 -0600 Subject: [PATCH 18/18] add control function and other stuff it needs (frantically) --- src/main/java/frc/robot/Shooter.java | 70 ++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/src/main/java/frc/robot/Shooter.java b/src/main/java/frc/robot/Shooter.java index 86c6981..01504f3 100644 --- a/src/main/java/frc/robot/Shooter.java +++ b/src/main/java/frc/robot/Shooter.java @@ -51,6 +51,8 @@ public enum ShooterStates { private WPI_TalonFX shooterMotor; private TalonFXSensorCollection sensors; + private boolean shooterTriggerBool; + private double shooterSpeed; private double desiredVelocity; @@ -58,6 +60,9 @@ public enum ShooterStates { private double initialVelocityThreshold; private double velocityThreshold; + private double velocity; + private double error; + public Shooter() { shooterMotor = new WPI_TalonFX(Constants.ShooterConstants.SHOOTER_MOTOR_ID); @@ -109,4 +114,69 @@ public void setVelocityThresholds(double initialVelocityThreshold, double veloci this.initialVelocityThreshold = initialVelocityThreshold; this.velocityThreshold = velocityThreshold; } + + public void controlShooter(double shooterTrigger, boolean shooterToggle) { + updateVelocity(); + checkIfShooting(shooterTrigger); + + // switch mode if appropriate + if (shooterToggle) { + switch (currentShooterMode) { + case VOLTAGE: + setPID(); + break; + case PID: + setVoltage(); + break; + } + } + } + + public void checkIfShooting(double shooterTrigger) { + shooterTriggerBool = (shooterTrigger >= Constants.ShooterConstants.DEADZONE); + + switch (currentShooterState) { + case NOT_MOVING: + if (shooterTriggerBool + && !Robot.arms.isArmClimbingPosition() + && !Robot.intake.isIntaking()) { + switch (currentShooterMode) { + case VOLTAGE: + setVoltageShooting(); + break; + case PID: + setPIDShooting(); + break; + } + } + break; + + case SHOOTING: + if (currentShooterMode == ShooterModes.VOLTAGE) { + setPIDShooting(); + } + if (!shooterTriggerBool + || Robot.arms.isArmClimbingPosition() + || Robot.intake.isIntaking()) { + setNotMoving(); + } + break; + } + } + + public void setVoltageShooting() { + updateVelocity(); + shooterMotor.set(shooterSpeed); + currentShooterState = ShooterStates.SHOOTING; + } + + public void setPIDShooting() { + shooterMotor.set(ControlMode.Velocity, desiredVelocity); + currentShooterState = ShooterStates.SHOOTING; + } + + public void updateVelocity() { + velocity = -sensors.getIntegratedSensorVelocity(); + error = desiredVelocity - velocity; + } } \ No newline at end of file