From fcb0386a9b62669521180d1b72d52d5374d81313 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sun, 16 Jul 2023 17:18:40 -0700 Subject: [PATCH 01/47] add code from approved PR by james --- .../com/team766/controllers/CSMSMPID.java | 214 ++++++++++++++++++ 1 file changed, 214 insertions(+) create mode 100644 src/main/java/com/team766/controllers/CSMSMPID.java diff --git a/src/main/java/com/team766/controllers/CSMSMPID.java b/src/main/java/com/team766/controllers/CSMSMPID.java new file mode 100644 index 00000000..0c92a795 --- /dev/null +++ b/src/main/java/com/team766/controllers/CSMSMPID.java @@ -0,0 +1,214 @@ +package com.team766.controllers; + +import com.team766.config.ConfigFileReader; +import com.team766.hal.RobotProvider; +import com.team766.library.SetValueProvider; +import com.team766.library.SettableValueProvider; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; //Todo: ? +import com.team766.logging.Logger; +import com.team766.logging.Severity; // Todo: ? + + +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import java.io.IOError; +import javax.swing.text.DefaultStyledDocument.ElementSpec; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; + + +public class CSMSMPID{ + // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder + private MotorController mc1; + private CANSparkMax csm1; + private SparkMaxPIDController pid1; + private SparkMaxAbsoluteEncoder abs1; + //PID Related Variables + private static double dz1 = 0; + private static double maxpos1 = 0; + private static double minpos1 = 0; + private static double maxvel1 = 0; + private static double maxaccel1 = 0; + private static double maxspeed1 = 0; + private static double minspeed1 = 0; + private static double currentPos = 0; + private static double combo; + + //antigrav variable + public static double antiGravK; + //enum for which state the PID is in + private enum PIDSTATE{ + PID, + OFF, + ANTIGRAV + } + //the state of the PID2 + private PIDSTATE theState = PIDSTATE.OFF; + + //constructor for the class with no absolute encoder + public CSMSMPID(String configName) throws Exception{ + //loggerCategory = Category.MECHANISMS; + + try{ + mc1 = RobotProvider.instance.getMotor(configName); + csm1 = (CANSparkMax)mc1; + pid1 = csm1.getPIDController(); + }catch (IllegalArgumentException ill){ + throw new Exception("Error instantiating the PID controller: " + ill); + } + + + } + //constructor for the class with an absolute encoder + public CSMSMPID(String configName, double absEncoderOffset) throws Exception{ + //loggerCategory = Category.MECHANISMS; + + try{ + mc1 = RobotProvider.instance.getMotor(configName); + csm1 = (CANSparkMax)mc1; + pid1 = csm1.getPIDController(); + abs1 = csm1.getAbsoluteEncoder(Type.kDutyCycle); + abs1.setZeroOffset(absEncoderOffset); + pid1.setFeedbackDevice(abs1); + }catch (IllegalArgumentException ill){ + throw new Exception("Error instantiating the CLE PID controller: " + ill); + } + + + } + //manually changing the state + public void updateState(PIDSTATE state){ + theState = state; + } + //changing all PID values at once + public void setPIDF(double p, double i, double d, double ff){ + pid1.setP(p); + pid1.setI(i); + pid1.setD(d); + pid1.setFF(ff); + } + //changing the P value + public void setP(double p){ + pid1.setP(p); + } + //changing the I value + public void setI(double i){ + pid1.setI(i); + } + //changing the D value + public void setD(double d){ + pid1.setD(d); + } + //changing the FF value + public void setFf(double ff){ + pid1.setFF(ff); + } + + //setting the antigravity constants2 + public void setAntigravConstant(double k){ + antiGravK = k; + } + + private void antigrav(){ + mc1.set(antiGravK * Math.sin(mc1.getSensorPosition())); + } + + //adding a built in closed loop error (not tested yet) + public void setSmartMotionAllowedClosedLoopError(double error){ + pid1.setSmartMotionAllowedClosedLoopError(error, 0); + } + //changing the deadzone + public void setDeadzone(double dz){ + dz1 = dz; + } + //changing the output range of the speed of the motors + public void setOutputRange(double min, double max){ + maxspeed1 = max; + minspeed1 = min; + pid1.setOutputRange(min, max); + } + //changing the neutral mode of the motor (brake/coast) + public void setMotorMode(NeutralMode mode){ + mc1.setNeutralMode(mode); + } + //setting the maximum and minimul locations that the motor can go to + public void setMinMaxLocation(double min, double max){ + maxpos1 = max; + minpos1 = min; + } + //setting the maximum velocity of the motor + public void setMaxVel(double max){ + maxvel1 = max; + pid1.setSmartMotionMaxVelocity(max, 0); + pid1.setSmartMotionMinOutputVelocity(0, 0); + } + //setting the maximum acceleration of the motor + public void setMaxAccel(double max){ + maxaccel1 = max; + pid1.setSmartMotionMaxAccel(max, 0); + } + + //go to a position using the thing that wasn't tested yet and almost broke the robot... + public void setCLEPosition(double position){ + if(position > maxpos1){ + position = maxpos1; + } else if(position < minpos1){ + position = minpos1; + } + + pid1.setReference(position, ControlType.kSmartMotion); + } + //1st step to go to a position using the normal PID, setting what you want the position to be + public void setPosition(double position){ + if(position > maxpos1){ + position = maxpos1; + } else if(position < minpos1){ + position = minpos1; + } + + currentPos = position; + } + + public void stop(){ + //Failsafe + currentPos = mc1.getSensorPosition(); + theState = PIDSTATE.OFF; + } + + //run loop that actually runs the PID using normal dz + //You need to call this function repedatly in OI + public void run(boolean enabled){ + if(enabled){ + switch(theState){ + case OFF: + break; + case ANTIGRAV: + antigrav(); + case PID: + if (mc1.getSensorPosition() <= (dz1 + mc1.getSensorPosition()) && mc1.getSensorPosition() >= (mc1.getSensorPosition() - dz1)){ + combo ++; + } else { + combo = 0; + pid1.setReference(currentPos, ControlType.kSmartMotion); // todo: testing if this is allowed + } + + if(combo >= 6){ + theState = PIDSTATE.ANTIGRAV; + } + break; + } + } else{ + //log("enabled is false"); + } + + } + + } + From 81dd9c7033451d18c4082d905081a3ff78b38f27 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sun, 16 Jul 2023 17:42:32 -0700 Subject: [PATCH 02/47] that was supposed to be here --- src/main/java/com/team766/controllers/CSMSMPID.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/controllers/CSMSMPID.java b/src/main/java/com/team766/controllers/CSMSMPID.java index 0c92a795..2ee613af 100644 --- a/src/main/java/com/team766/controllers/CSMSMPID.java +++ b/src/main/java/com/team766/controllers/CSMSMPID.java @@ -42,9 +42,9 @@ public class CSMSMPID{ private static double combo; //antigrav variable - public static double antiGravK; + private static double antiGravK; //enum for which state the PID is in - private enum PIDSTATE{ + public enum PIDSTATE{ PID, OFF, ANTIGRAV From db7eb64fb62115f8474f19d185815e9801147175 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 17 Jul 2023 04:07:06 -0700 Subject: [PATCH 03/47] Ryan's feedback. Over under 70 lint messages? --- .../com/team766/controllers/CSMSMPID.java | 138 +++++++++--------- 1 file changed, 72 insertions(+), 66 deletions(-) diff --git a/src/main/java/com/team766/controllers/CSMSMPID.java b/src/main/java/com/team766/controllers/CSMSMPID.java index 2ee613af..824393de 100644 --- a/src/main/java/com/team766/controllers/CSMSMPID.java +++ b/src/main/java/com/team766/controllers/CSMSMPID.java @@ -1,13 +1,13 @@ -package com.team766.controllers; +wpackage com.team766.controllers; import com.team766.config.ConfigFileReader; import com.team766.hal.RobotProvider; import com.team766.library.SetValueProvider; import com.team766.library.SettableValueProvider; import com.team766.library.ValueProvider; -import com.team766.logging.Category; //Todo: ? +import com.team766.logging.Category; import com.team766.logging.Logger; -import com.team766.logging.Severity; // Todo: ? +import com.team766.logging.Severity; import com.revrobotics.SparkMaxAbsoluteEncoder; @@ -24,24 +24,22 @@ import com.team766.library.RateLimiter; -public class CSMSMPID{ +public class CanSparkMaxSmartMotionPIDController{ // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder - private MotorController mc1; - private CANSparkMax csm1; - private SparkMaxPIDController pid1; - private SparkMaxAbsoluteEncoder abs1; + private MotorController mc; + private CANSparkMax csm; + private SparkMaxPIDController pid; + private SparkMaxAbsoluteEncoder abs; //PID Related Variables private static double dz1 = 0; - private static double maxpos1 = 0; - private static double minpos1 = 0; - private static double maxvel1 = 0; - private static double maxaccel1 = 0; - private static double maxspeed1 = 0; - private static double minspeed1 = 0; - private static double currentPos = 0; - private static double combo; - - //antigrav variable + private static double setPointPosition = 0; + private static double comboOfTimesInsideDeadzone = 0; + private static double minPos = 0; + private static double maxPos = 0; + + private bool isAbsoluteEncoderEnabled; + + //antigrav coefficient private static double antiGravK; //enum for which state the PID is in public enum PIDSTATE{ @@ -49,36 +47,38 @@ public enum PIDSTATE{ OFF, ANTIGRAV } - //the state of the PID2 + //the state of the PID private PIDSTATE theState = PIDSTATE.OFF; //constructor for the class with no absolute encoder - public CSMSMPID(String configName) throws Exception{ - //loggerCategory = Category.MECHANISMS; + public CanSparkMaxSmartMotionPIDController(String configName) throws RuntimeException{ + loggerCategory = Category.MECHANISMS; try{ - mc1 = RobotProvider.instance.getMotor(configName); - csm1 = (CANSparkMax)mc1; - pid1 = csm1.getPIDController(); + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax)mc; + pid = csm.getPIDController(); + isAbsoluteEncoderEnabled = true; }catch (IllegalArgumentException ill){ - throw new Exception("Error instantiating the PID controller: " + ill); + throw new RuntimeException("Error instantiating the PID controller: " + ill); } - + } //constructor for the class with an absolute encoder - public CSMSMPID(String configName, double absEncoderOffset) throws Exception{ - //loggerCategory = Category.MECHANISMS; + public CanSparkMaxSmartMotionPIDController(String configName, double absEncoderOffset) throws RuntimeException{ + loggerCategory = Category.MECHANISMS; try{ - mc1 = RobotProvider.instance.getMotor(configName); - csm1 = (CANSparkMax)mc1; - pid1 = csm1.getPIDController(); - abs1 = csm1.getAbsoluteEncoder(Type.kDutyCycle); - abs1.setZeroOffset(absEncoderOffset); - pid1.setFeedbackDevice(abs1); + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax)mc; + pid = csm.getPIDController(); + abs = csm.getAbsoluteEncoder(Type.kDutyCycle); + abs.setZeroOffset(absEncoderOffset); + pid.setFeedbackDevice(abs); + isAbsoluteEncoderEnabled = false; }catch (IllegalArgumentException ill){ - throw new Exception("Error instantiating the CLE PID controller: " + ill); + throw new RuntimeException("Error instantiating the CLE PID controller: " + ill); } @@ -89,26 +89,26 @@ public void updateState(PIDSTATE state){ } //changing all PID values at once public void setPIDF(double p, double i, double d, double ff){ - pid1.setP(p); - pid1.setI(i); - pid1.setD(d); - pid1.setFF(ff); + pid.setP(p); + pid.setI(i); + pid.setD(d); + pid.setFF(ff); } //changing the P value public void setP(double p){ - pid1.setP(p); + pid.setP(p); } //changing the I value public void setI(double i){ - pid1.setI(i); + pid.setI(i); } //changing the D value public void setD(double d){ - pid1.setD(d); + pid.setD(d); } //changing the FF value public void setFf(double ff){ - pid1.setFF(ff); + pid.setFF(ff); } //setting the antigravity constants2 @@ -117,12 +117,12 @@ public void setAntigravConstant(double k){ } private void antigrav(){ - mc1.set(antiGravK * Math.sin(mc1.getSensorPosition())); + mc.set(antiGravK * Math.sin(mc.getSensorPosition())); } //adding a built in closed loop error (not tested yet) public void setSmartMotionAllowedClosedLoopError(double error){ - pid1.setSmartMotionAllowedClosedLoopError(error, 0); + pid.setSmartMotionAllowedClosedLoopError(error, 0); } //changing the deadzone public void setDeadzone(double dz){ @@ -130,29 +130,25 @@ public void setDeadzone(double dz){ } //changing the output range of the speed of the motors public void setOutputRange(double min, double max){ - maxspeed1 = max; - minspeed1 = min; - pid1.setOutputRange(min, max); + pid.setOutputRange(min, max); } //changing the neutral mode of the motor (brake/coast) public void setMotorMode(NeutralMode mode){ - mc1.setNeutralMode(mode); + mc.setNeutralMode(mode); } - //setting the maximum and minimul locations that the motor can go to + //setting the maximum and minimum locations that the motor can go to public void setMinMaxLocation(double min, double max){ maxpos1 = max; minpos1 = min; } //setting the maximum velocity of the motor public void setMaxVel(double max){ - maxvel1 = max; - pid1.setSmartMotionMaxVelocity(max, 0); - pid1.setSmartMotionMinOutputVelocity(0, 0); + pid.setSmartMotionMaxVelocity(max, 0); + pid.setSmartMotionMinOutputVelocity(0, 0); } //setting the maximum acceleration of the motor public void setMaxAccel(double max){ - maxaccel1 = max; - pid1.setSmartMotionMaxAccel(max, 0); + pid.setSmartMotionMaxAccel(max, 0); } //go to a position using the thing that wasn't tested yet and almost broke the robot... @@ -163,22 +159,22 @@ public void setCLEPosition(double position){ position = minpos1; } - pid1.setReference(position, ControlType.kSmartMotion); + pid.setReference(position, ControlType.kSmartMotion); } //1st step to go to a position using the normal PID, setting what you want the position to be - public void setPosition(double position){ + public void setSetpoint(double position){ if(position > maxpos1){ position = maxpos1; } else if(position < minpos1){ position = minpos1; } - currentPos = position; + setPointPosition = position; } public void stop(){ //Failsafe - currentPos = mc1.getSensorPosition(); + setPointPosition = mc.getSensorPosition(); theState = PIDSTATE.OFF; } @@ -186,26 +182,36 @@ public void stop(){ //You need to call this function repedatly in OI public void run(boolean enabled){ if(enabled){ + //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 + if(isAbsoluteEncoderEnabled){ + MathUtil.clamp(minPos, 0.0, 1.0); + MathUtil.clamp(maxPos, 0.0, 1.0); + } + switch(theState){ case OFF: break; case ANTIGRAV: - antigrav(); + if (mc.getSensorPosition() <= (dz1 + mc.getSensorPosition()) && mc.getSensorPosition() >= (mc.getSensorPosition() - dz1)){ + antigrav(); + } else { + theState = PIDSTATE.PID; + } case PID: - if (mc1.getSensorPosition() <= (dz1 + mc1.getSensorPosition()) && mc1.getSensorPosition() >= (mc1.getSensorPosition() - dz1)){ - combo ++; + if (mc.getSensorPosition() <= (dz1 + mc.getSensorPosition()) && mc.getSensorPosition() >= (mc.getSensorPosition() - dz1)){ + comboOfTimesInsideDeadzone ++; } else { - combo = 0; - pid1.setReference(currentPos, ControlType.kSmartMotion); // todo: testing if this is allowed + comboOfTimesInsideDeadzone = 0; + pid.setReference(setPointPosition, ControlType.kSmartMotion); // todo: testing if this is allowed } - if(combo >= 6){ + if(comboOfTimesInsideDeadzone >= 6){ theState = PIDSTATE.ANTIGRAV; } break; } } else{ - //log("enabled is false"); + log("enabled is false on run loop PID controller"); } } From 1837647a34b6abc0bc4c0163e79cf514d6fcb1c0 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 17 Jul 2023 04:11:03 -0700 Subject: [PATCH 04/47] Rename CSMSMPID.java to CanSparkMaxSmartMotionPIDController.java --- .../{CSMSMPID.java => CanSparkMaxSmartMotionPIDController.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/com/team766/controllers/{CSMSMPID.java => CanSparkMaxSmartMotionPIDController.java} (100%) diff --git a/src/main/java/com/team766/controllers/CSMSMPID.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java similarity index 100% rename from src/main/java/com/team766/controllers/CSMSMPID.java rename to src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java From b41a5b907f3bba91c69f7ad467029fe554ca9298 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 17 Jul 2023 11:06:59 -0700 Subject: [PATCH 05/47] typo (accidental wpackage instead of package) --- .../controllers/CanSparkMaxSmartMotionPIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java index 824393de..ce741286 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java @@ -1,4 +1,4 @@ -wpackage com.team766.controllers; +package com.team766.controllers; import com.team766.config.ConfigFileReader; import com.team766.hal.RobotProvider; From b589b2279fa5a816ecb481b814a5f3b595e0bd3a Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 18 Jul 2023 06:58:20 -0700 Subject: [PATCH 06/47] Made it so it changes the state whenever you input a new setpoint --- .../controllers/CanSparkMaxSmartMotionPIDController.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java index ce741286..776041c8 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java @@ -170,6 +170,8 @@ public void setSetpoint(double position){ } setPointPosition = position; + + theState = PIDSTATE.PID; } public void stop(){ From ac72e4690cc59e30ceb5f2dbf67497bdb1f2b0ab Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sun, 30 Jul 2023 10:22:37 -0700 Subject: [PATCH 07/47] first batch fix --- .../CanSparkMaxSmartMotionPIDController.java | 40 +++++++++++++------ 1 file changed, 27 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java index 776041c8..36b7eaf2 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java @@ -31,13 +31,14 @@ public class CanSparkMaxSmartMotionPIDController{ private SparkMaxPIDController pid; private SparkMaxAbsoluteEncoder abs; //PID Related Variables - private static double dz1 = 0; + private static double deadzone = 0; private static double setPointPosition = 0; private static double comboOfTimesInsideDeadzone = 0; private static double minPos = 0; private static double maxPos = 0; private bool isAbsoluteEncoderEnabled; + private bool isMechanismRotational; //antigrav coefficient private static double antiGravK; @@ -51,14 +52,15 @@ public enum PIDSTATE{ private PIDSTATE theState = PIDSTATE.OFF; //constructor for the class with no absolute encoder - public CanSparkMaxSmartMotionPIDController(String configName) throws RuntimeException{ + public CanSparkMaxSmartMotionPIDController(String configName, boolean rotational){ loggerCategory = Category.MECHANISMS; try{ mc = RobotProvider.instance.getMotor(configName); csm = (CANSparkMax)mc; pid = csm.getPIDController(); - isAbsoluteEncoderEnabled = true; + isAbsoluteEncoderEnabled = false; + isMechanismRotational = rotational; }catch (IllegalArgumentException ill){ throw new RuntimeException("Error instantiating the PID controller: " + ill); } @@ -66,7 +68,7 @@ public CanSparkMaxSmartMotionPIDController(String configName) throws RuntimeExce } //constructor for the class with an absolute encoder - public CanSparkMaxSmartMotionPIDController(String configName, double absEncoderOffset) throws RuntimeException{ + public CanSparkMaxSmartMotionPIDController(String configName, double absEncoderOffset, boolean rotational){ loggerCategory = Category.MECHANISMS; try{ @@ -76,7 +78,8 @@ public CanSparkMaxSmartMotionPIDController(String configName, double absEncoderO abs = csm.getAbsoluteEncoder(Type.kDutyCycle); abs.setZeroOffset(absEncoderOffset); pid.setFeedbackDevice(abs); - isAbsoluteEncoderEnabled = false; + isAbsoluteEncoderEnabled = true; + isMechanismRotational = rotational; }catch (IllegalArgumentException ill){ throw new RuntimeException("Error instantiating the CLE PID controller: " + ill); } @@ -111,13 +114,23 @@ public void setFf(double ff){ pid.setFF(ff); } - //setting the antigravity constants2 + /* + * Here we set the antigrav constant + * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with + * If the mechanism isn't rotational, this is just the amount of power to apply. + * @param k the value to set according to the above condition + */ public void setAntigravConstant(double k){ antiGravK = k; } private void antigrav(){ - mc.set(antiGravK * Math.sin(mc.getSensorPosition())); + if(rotational){ + mc.set(antiGravK * Math.sin(mc.getSensorPosition())); + }else{ + mc.set(antiGravK); + } + } //adding a built in closed loop error (not tested yet) @@ -126,7 +139,7 @@ public void setSmartMotionAllowedClosedLoopError(double error){ } //changing the deadzone public void setDeadzone(double dz){ - dz1 = dz; + deadzone = dz; } //changing the output range of the speed of the motors public void setOutputRange(double min, double max){ @@ -186,21 +199,22 @@ public void run(boolean enabled){ if(enabled){ //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 if(isAbsoluteEncoderEnabled){ - MathUtil.clamp(minPos, 0.0, 1.0); - MathUtil.clamp(maxPos, 0.0, 1.0); + minPos = MathUtil.clamp(minPos, 0.0, 1.0); + maxPos = MathUtil.clamp(maxPos, 0.0, 1.0); } switch(theState){ - case OFF: + case OFF:` + mc.set(0); break; case ANTIGRAV: - if (mc.getSensorPosition() <= (dz1 + mc.getSensorPosition()) && mc.getSensorPosition() >= (mc.getSensorPosition() - dz1)){ + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ antigrav(); } else { theState = PIDSTATE.PID; } case PID: - if (mc.getSensorPosition() <= (dz1 + mc.getSensorPosition()) && mc.getSensorPosition() >= (mc.getSensorPosition() - dz1)){ + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ comboOfTimesInsideDeadzone ++; } else { comboOfTimesInsideDeadzone = 0; From 6ebaf6445e6b29a323c04ad98ae91b57ce3bdc94 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sun, 30 Jul 2023 11:34:27 -0700 Subject: [PATCH 08/47] do not merge yet --- ...MaxSmartMotionRotationalPIDController.java | 201 ++++++++++++++++++ 1 file changed, 201 insertions(+) create mode 100644 src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java new file mode 100644 index 00000000..1c67dcca --- /dev/null +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -0,0 +1,201 @@ +package com.team766.controllers; + +import com.team766.config.ConfigFileReader; +import com.team766.hal.RobotProvider; +import com.team766.library.SetValueProvider; +import com.team766.library.SettableValueProvider; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import java.io.IOError; +import javax.swing.text.DefaultStyledDocument.ElementSpec; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; + + +public class CanSparkMaxSmartMotionRotationalPIDController{ + + // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder + private MotorController mc; + private CANSparkMax csm; + private SparkMaxPIDController pid; + private SparkMaxAbsoluteEncoder abs; + + //PID Related Variables + private static double deadzone = 0; + private static double setPointPosition = 0; + private static double comboOfTimesInsideDeadzone = 0; + private static double minPos = 0; + private static double maxPos = 0; + + + //Precision variables + private double degreesToEncoderUnitsRatio = 0; + + //antigrav coefficient + private static double antiGravK; + //enum for which state the PID is in + public enum PIDSTATE{ + PID, + OFF, + ANTIGRAV + } + //the state of the PID + private PIDSTATE theState = PIDSTATE.OFF; + + //constructor for the class not using an absolute encoder for kDutyCycle + + public CanSparkMaxSmartMotionRotationalPIDController(String configName, double absEncoderOffsetToMakeZeroEncoderUnits, double ratio){ + loggerCategory = Category.MECHANISMS; + + try{ + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax)mc; + pid = csm.getPIDController(); + abs = csm.getAbsoluteEncoder(Type.kDutyCycle); + abs.setZeroOffset(absEncoderOffsetToMakeZeroEncoderUnits); + degreesToEncoderUnitsRatio = ratio; + }catch (IllegalArgumentException ill){ + throw new RuntimeException("Error instantiating the PID controller: " + ill); + } + + + } + + private double euToDegrees(double eu){ + return eu * degreesToEncoderUnitsRatio; + } + + private double degreesToEu(double degrees){ + return (1/degreesToEncoderUnitsRatio) * degrees; + } + + //manually changing the state + public void updateState(PIDSTATE state){ + theState = state; + } + //changing all PID values at once + public void setPIDF(double p, double i, double d, double ff){ + pid.setP(p); + pid.setI(i); + pid.setD(d); + pid.setFF(ff); + } + //changing the P value + public void setP(double p){ + pid.setP(p); + } + //changing the I value + public void setI(double i){ + pid.setI(i); + } + //changing the D value + public void setD(double d){ + pid.setD(d); + } + //changing the FF value + public void setFf(double ff){ + pid.setFF(ff); + } + + /* + * Here we set the antigrav constant + * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with + * If the mechanism isn't rotational, this is just the amount of power to apply. + * @param k the value to set according to the above condition + */ + public void setAntigravConstant(double k){ + antiGravK = k; + } + + private void antigrav(){ + mc.set(antiGravK * Math.sin(euToDegrees(mc.getSensorPosition()))); + } + + //changing the deadzone + public void setDeadzone(double dz){ + deadzone = dz; + } + //changing the output range of the speed of the motors + public void setOutputRange(double min, double max){ + pid.setOutputRange(min, max); + } + //changing the neutral mode of the motor (brake/coast) + public void setMotorMode(NeutralMode mode){ + mc.setNeutralMode(mode); + } + //setting the maximum and minimum locations that the motor can go to + public void setMinMaxLocation(double min, double max){ + maxPos = max; + minPos = min; + } + //setting the maximum velocity of the motor + public void setMaxVel(double max){ + pid.setSmartMotionMaxVelocity(max, 0); + pid.setSmartMotionMinOutputVelocity(0, 0); + } + //setting the maximum acceleration of the motor + public void setMaxAccel(double max){ + pid.setSmartMotionMaxAccel(max, 0); + } + + //1st step to go to a position using the normal PID, setting what you want the position to be + public void setSetpoint(double positionInDegrees){ + setPointPosition = MathUtil.clamp(degreesToEu(positionInDegrees), minPos, maxPos); + theState = PIDSTATE.PID; + } + + //Failsafe + public void stop(){ + setPointPosition = mc.getSensorPosition(); + theState = PIDSTATE.OFF; + } + + //run loop that actually runs the PID + //You need to call this function repedatly in OI as often as possible + public void run(boolean enabled){ + if(enabled){ + //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 + + switch(theState){ + case OFF: + mc.set(0); + break; + case ANTIGRAV: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ + antigrav(); + } else { + theState = PIDSTATE.PID; + } + case PID: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ + comboOfTimesInsideDeadzone ++; + } else { + comboOfTimesInsideDeadzone = 0; + pid.setReference(setPointPosition, ControlType.kSmartMotion); + } + + if(comboOfTimesInsideDeadzone >= 6){ + theState = PIDSTATE.ANTIGRAV; + } + break; + } + } else{ + log("enabled is false on run loop PID controller"); + } + + } + + } + + From e3965f4d56c061adcb4aa7a7a3e69d05914deeb8 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 31 Jul 2023 20:15:08 -0700 Subject: [PATCH 09/47] do not merge: also where do we store this --- .../com/team766/controllers/OffsetPoint.java | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 src/main/java/com/team766/controllers/OffsetPoint.java diff --git a/src/main/java/com/team766/controllers/OffsetPoint.java b/src/main/java/com/team766/controllers/OffsetPoint.java new file mode 100644 index 00000000..ac86b239 --- /dev/null +++ b/src/main/java/com/team766/controllers/OffsetPoint.java @@ -0,0 +1,19 @@ +package com.team766.controllers; + +public class OffsetPoint { + + private double absoluteValue, motorValue; + + public OffsetPoint(double absoluteEncoderValue, double motorEncoderValue){ + absoluteValue = absoluteEncoderValue; + motorValue = motorEncoderValue; + } + + public double getAbsoluteValue(){ + return absoluteValue; + } + + public double getMotorEncoderValue(){ + return motorValue; + } +} From 48dd06a27ca0aaaa0efa4fa62ce943fe94ff5b63 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 31 Jul 2023 20:33:28 -0700 Subject: [PATCH 10/47] ready for new review now --- ...MaxSmartMotionRotationalPIDController.java | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 1c67dcca..0d957064 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -41,6 +41,7 @@ public class CanSparkMaxSmartMotionRotationalPIDController{ //Precision variables private double degreesToEncoderUnitsRatio = 0; + private double encoderUnitsPerOneAbsolute = 0; //antigrav coefficient private static double antiGravK; @@ -55,7 +56,7 @@ public enum PIDSTATE{ //constructor for the class not using an absolute encoder for kDutyCycle - public CanSparkMaxSmartMotionRotationalPIDController(String configName, double absEncoderOffsetToMakeZeroEncoderUnits, double ratio){ + public CanSparkMaxSmartMotionRotationalPIDController(String configName, double absEncoderOffset, double absEncoderOffsetForZeroEncoderUnits, OffsetPoint first, OffsetPoint second, double ratio){ loggerCategory = Category.MECHANISMS; try{ @@ -63,8 +64,20 @@ public CanSparkMaxSmartMotionRotationalPIDController(String configName, double a csm = (CANSparkMax)mc; pid = csm.getPIDController(); abs = csm.getAbsoluteEncoder(Type.kDutyCycle); - abs.setZeroOffset(absEncoderOffsetToMakeZeroEncoderUnits); + abs.setZeroOffset(absEncoderOffset); degreesToEncoderUnitsRatio = ratio; + + + double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); + double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); + + encoderUnitsPerOneAbsolute = motorEncoderDiference/absoluteDifference; + + double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); + + mc.setSensorPosition(relEncoder); + + }catch (IllegalArgumentException ill){ throw new RuntimeException("Error instantiating the PID controller: " + ill); } @@ -72,6 +85,10 @@ public CanSparkMaxSmartMotionRotationalPIDController(String configName, double a } + private double absToEu(double abs){ + return encoderUnitsPerOneAbsolute / (1/abs); + } + private double euToDegrees(double eu){ return eu * degreesToEncoderUnitsRatio; } From fa1d671f3173b0f6e6c2fb506c2fd72b8d326101 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 31 Jul 2023 20:41:37 -0700 Subject: [PATCH 11/47] Delete csmsmpidcontroller to make room for others --- .../CanSparkMaxSmartMotionPIDController.java | 236 ------------------ 1 file changed, 236 deletions(-) delete mode 100644 src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java deleted file mode 100644 index 36b7eaf2..00000000 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionPIDController.java +++ /dev/null @@ -1,236 +0,0 @@ -package com.team766.controllers; - -import com.team766.config.ConfigFileReader; -import com.team766.hal.RobotProvider; -import com.team766.library.SetValueProvider; -import com.team766.library.SettableValueProvider; -import com.team766.library.ValueProvider; -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.logging.Severity; - - -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import java.io.IOError; -import javax.swing.text.DefaultStyledDocument.ElementSpec; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.revrobotics.CANSparkMax; -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; - - -public class CanSparkMaxSmartMotionPIDController{ - // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder - private MotorController mc; - private CANSparkMax csm; - private SparkMaxPIDController pid; - private SparkMaxAbsoluteEncoder abs; - //PID Related Variables - private static double deadzone = 0; - private static double setPointPosition = 0; - private static double comboOfTimesInsideDeadzone = 0; - private static double minPos = 0; - private static double maxPos = 0; - - private bool isAbsoluteEncoderEnabled; - private bool isMechanismRotational; - - //antigrav coefficient - private static double antiGravK; - //enum for which state the PID is in - public enum PIDSTATE{ - PID, - OFF, - ANTIGRAV - } - //the state of the PID - private PIDSTATE theState = PIDSTATE.OFF; - - //constructor for the class with no absolute encoder - public CanSparkMaxSmartMotionPIDController(String configName, boolean rotational){ - loggerCategory = Category.MECHANISMS; - - try{ - mc = RobotProvider.instance.getMotor(configName); - csm = (CANSparkMax)mc; - pid = csm.getPIDController(); - isAbsoluteEncoderEnabled = false; - isMechanismRotational = rotational; - }catch (IllegalArgumentException ill){ - throw new RuntimeException("Error instantiating the PID controller: " + ill); - } - - - } - //constructor for the class with an absolute encoder - public CanSparkMaxSmartMotionPIDController(String configName, double absEncoderOffset, boolean rotational){ - loggerCategory = Category.MECHANISMS; - - try{ - mc = RobotProvider.instance.getMotor(configName); - csm = (CANSparkMax)mc; - pid = csm.getPIDController(); - abs = csm.getAbsoluteEncoder(Type.kDutyCycle); - abs.setZeroOffset(absEncoderOffset); - pid.setFeedbackDevice(abs); - isAbsoluteEncoderEnabled = true; - isMechanismRotational = rotational; - }catch (IllegalArgumentException ill){ - throw new RuntimeException("Error instantiating the CLE PID controller: " + ill); - } - - - } - //manually changing the state - public void updateState(PIDSTATE state){ - theState = state; - } - //changing all PID values at once - public void setPIDF(double p, double i, double d, double ff){ - pid.setP(p); - pid.setI(i); - pid.setD(d); - pid.setFF(ff); - } - //changing the P value - public void setP(double p){ - pid.setP(p); - } - //changing the I value - public void setI(double i){ - pid.setI(i); - } - //changing the D value - public void setD(double d){ - pid.setD(d); - } - //changing the FF value - public void setFf(double ff){ - pid.setFF(ff); - } - - /* - * Here we set the antigrav constant - * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with - * If the mechanism isn't rotational, this is just the amount of power to apply. - * @param k the value to set according to the above condition - */ - public void setAntigravConstant(double k){ - antiGravK = k; - } - - private void antigrav(){ - if(rotational){ - mc.set(antiGravK * Math.sin(mc.getSensorPosition())); - }else{ - mc.set(antiGravK); - } - - } - - //adding a built in closed loop error (not tested yet) - public void setSmartMotionAllowedClosedLoopError(double error){ - pid.setSmartMotionAllowedClosedLoopError(error, 0); - } - //changing the deadzone - public void setDeadzone(double dz){ - deadzone = dz; - } - //changing the output range of the speed of the motors - public void setOutputRange(double min, double max){ - pid.setOutputRange(min, max); - } - //changing the neutral mode of the motor (brake/coast) - public void setMotorMode(NeutralMode mode){ - mc.setNeutralMode(mode); - } - //setting the maximum and minimum locations that the motor can go to - public void setMinMaxLocation(double min, double max){ - maxpos1 = max; - minpos1 = min; - } - //setting the maximum velocity of the motor - public void setMaxVel(double max){ - pid.setSmartMotionMaxVelocity(max, 0); - pid.setSmartMotionMinOutputVelocity(0, 0); - } - //setting the maximum acceleration of the motor - public void setMaxAccel(double max){ - pid.setSmartMotionMaxAccel(max, 0); - } - - //go to a position using the thing that wasn't tested yet and almost broke the robot... - public void setCLEPosition(double position){ - if(position > maxpos1){ - position = maxpos1; - } else if(position < minpos1){ - position = minpos1; - } - - pid.setReference(position, ControlType.kSmartMotion); - } - //1st step to go to a position using the normal PID, setting what you want the position to be - public void setSetpoint(double position){ - if(position > maxpos1){ - position = maxpos1; - } else if(position < minpos1){ - position = minpos1; - } - - setPointPosition = position; - - theState = PIDSTATE.PID; - } - - public void stop(){ - //Failsafe - setPointPosition = mc.getSensorPosition(); - theState = PIDSTATE.OFF; - } - - //run loop that actually runs the PID using normal dz - //You need to call this function repedatly in OI - public void run(boolean enabled){ - if(enabled){ - //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 - if(isAbsoluteEncoderEnabled){ - minPos = MathUtil.clamp(minPos, 0.0, 1.0); - maxPos = MathUtil.clamp(maxPos, 0.0, 1.0); - } - - switch(theState){ - case OFF:` - mc.set(0); - break; - case ANTIGRAV: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ - antigrav(); - } else { - theState = PIDSTATE.PID; - } - case PID: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ - comboOfTimesInsideDeadzone ++; - } else { - comboOfTimesInsideDeadzone = 0; - pid.setReference(setPointPosition, ControlType.kSmartMotion); // todo: testing if this is allowed - } - - if(comboOfTimesInsideDeadzone >= 6){ - theState = PIDSTATE.ANTIGRAV; - } - break; - } - } else{ - log("enabled is false on run loop PID controller"); - } - - } - - } - From 21b05d063495fb300c603952f242d1ec2cff21c0 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 31 Jul 2023 20:42:25 -0700 Subject: [PATCH 12/47] Create CanSparkMaxSmartMotionNonRotationalPIDController.java --- ...SmartMotionNonRotationalPIDController.java | 216 ++++++++++++++++++ 1 file changed, 216 insertions(+) create mode 100644 src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java new file mode 100644 index 00000000..ad7d2672 --- /dev/null +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java @@ -0,0 +1,216 @@ + +package com.team766.controllers; + +import com.team766.config.ConfigFileReader; +import com.team766.hal.RobotProvider; +import com.team766.library.SetValueProvider; +import com.team766.library.SettableValueProvider; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; +import edu.wpi.first.math.MathUtil; +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import java.io.IOError; +import javax.swing.text.DefaultStyledDocument.ElementSpec; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; + + +public class CanSparkMaxSmartMotionNonRotationalPIDController{ + + // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder + private MotorController mc; + private CANSparkMax csm; + private SparkMaxPIDController pid; + private SparkMaxAbsoluteEncoder abs; + + //PID Related Variables + private static double deadzone = 0; + private static double setPointPosition = 0; + private static double comboOfTimesInsideDeadzone = 0; + private static double minPos = 0; + private static double maxPos = 0; + + + //Precision variables + private double degreesToEncoderUnitsRatio = 0; + private double encoderUnitsPerOneAbsolute = 0; + + //antigrav coefficient + private static double antigravPower; + //enum for which state the PID is in + public enum PIDSTATE{ + PID, + OFF, + ANTIGRAV + } + //the state of the PID + private PIDSTATE theState = PIDSTATE.OFF; + + //constructor for the class not using an absolute encoder for kDutyCycle + + public CanSparkMaxSmartMotionNonRotationalPIDController(String configName, double absEncoderOffset, double absEncoderOffsetForZeroEncoderUnits, OffsetPoint first, OffsetPoint second, double ratio){ + loggerCategory = Category.MECHANISMS; + + try{ + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax)mc; + pid = csm.getPIDController(); + abs = csm.getAbsoluteEncoder(Type.kDutyCycle); + abs.setZeroOffset(absEncoderOffset); + degreesToEncoderUnitsRatio = ratio; + + + double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); + double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); + + encoderUnitsPerOneAbsolute = motorEncoderDiference/absoluteDifference; + + double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); + + mc.setSensorPosition(relEncoder); + + + }catch (IllegalArgumentException ill){ + throw new RuntimeException("Error instantiating the PID controller: " + ill); + } + + + } + + private double absToEu(double abs){ + return encoderUnitsPerOneAbsolute / (1/abs); + } + + + private double degreesToEu(double degrees){ + return (1/degreesToEncoderUnitsRatio) * degrees; + } + + //manually changing the state + public void updateState(PIDSTATE state){ + theState = state; + } + //changing all PID values at once + public void setPIDF(double p, double i, double d, double ff){ + pid.setP(p); + pid.setI(i); + pid.setD(d); + pid.setFF(ff); + } + //changing the P value + public void setP(double p){ + pid.setP(p); + } + //changing the I value + public void setI(double i){ + pid.setI(i); + } + //changing the D value + public void setD(double d){ + pid.setD(d); + } + //changing the FF value + public void setFf(double ff){ + pid.setFF(ff); + } + + /* + * Here we set the antigrav constant + * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with + * If the mechanism isn't rotational, this is just the amount of power to apply. + * @param k the value to set according to the above condition + */ + public void setAntigravPower(double power){ + antigravPower = power; + } + + private void antigrav(){ + mc.set(antigravPower); + } + + //changing the deadzone + public void setDeadzone(double dz){ + deadzone = dz; + } + //changing the output range of the speed of the motors + public void setOutputRange(double min, double max){ + pid.setOutputRange(min, max); + } + //changing the neutral mode of the motor (brake/coast) + public void setMotorMode(NeutralMode mode){ + mc.setNeutralMode(mode); + } + //setting the maximum and minimum locations that the motor can go to + public void setMinMaxLocation(double min, double max){ + maxPos = max; + minPos = min; + } + //setting the maximum velocity of the motor + public void setMaxVel(double max){ + pid.setSmartMotionMaxVelocity(max, 0); + pid.setSmartMotionMinOutputVelocity(0, 0); + } + //setting the maximum acceleration of the motor + public void setMaxAccel(double max){ + pid.setSmartMotionMaxAccel(max, 0); + } + + //1st step to go to a position using the normal PID, setting what you want the position to be + public void setSetpoint(double positionInDegrees){ + setPointPosition = MathUtil.clamp(degreesToEu(positionInDegrees), minPos, maxPos); + theState = PIDSTATE.PID; + } + + //Failsafe + public void stop(){ + setPointPosition = mc.getSensorPosition(); + theState = PIDSTATE.OFF; + } + + //run loop that actually runs the PID + //You need to call this function repedatly in OI as often as possible + public void run(boolean enabled){ + if(enabled){ + //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 + + switch(theState){ + case OFF: + mc.set(0); + break; + case ANTIGRAV: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ + antigrav(); + } else { + theState = PIDSTATE.PID; + } + case PID: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ + comboOfTimesInsideDeadzone ++; + } else { + comboOfTimesInsideDeadzone = 0; + pid.setReference(setPointPosition, ControlType.kSmartMotion); + } + + if(comboOfTimesInsideDeadzone >= 6){ + theState = PIDSTATE.ANTIGRAV; + } + break; + } + } else{ + log("enabled is false on run loop PID controller"); + } + + } + + } + + From 7ce2fbac057783ea27b3fd0022e97c912e8968e2 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Wed, 16 Aug 2023 09:28:48 -0700 Subject: [PATCH 13/47] Update CanSparkMaxSmartMotionRotationalPIDController.java --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 0d957064..e592a94c 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -86,7 +86,7 @@ public CanSparkMaxSmartMotionRotationalPIDController(String configName, double a } private double absToEu(double abs){ - return encoderUnitsPerOneAbsolute / (1/abs); + return encoderUnitsPerOneAbsolute * abs; } private double euToDegrees(double eu){ From 14db6536ee57b2c99db5f0bfd786b60d04ef4e53 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Wed, 16 Aug 2023 09:31:52 -0700 Subject: [PATCH 14/47] fixed run loop --- ...parkMaxSmartMotionNonRotationalPIDController.java | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java index ad7d2672..d5112cf7 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java @@ -58,7 +58,7 @@ public enum PIDSTATE{ //constructor for the class not using an absolute encoder for kDutyCycle public CanSparkMaxSmartMotionNonRotationalPIDController(String configName, double absEncoderOffset, double absEncoderOffsetForZeroEncoderUnits, OffsetPoint first, OffsetPoint second, double ratio){ - loggerCategory = Category.MECHANISMS; + try{ mc = RobotProvider.instance.getMotor(configName); @@ -178,9 +178,9 @@ public void stop(){ //run loop that actually runs the PID //You need to call this function repedatly in OI as often as possible - public void run(boolean enabled){ - if(enabled){ - //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 + public void run(){ + + switch(theState){ case OFF: @@ -205,9 +205,7 @@ public void run(boolean enabled){ } break; } - } else{ - log("enabled is false on run loop PID controller"); - } + } From c6f6d3655ca2cfd2da5d879d2ce237e27a5245f8 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Wed, 16 Aug 2023 09:33:16 -0700 Subject: [PATCH 15/47] fix run loop and logging errors --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index e592a94c..05ce0703 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -57,7 +57,7 @@ public enum PIDSTATE{ //constructor for the class not using an absolute encoder for kDutyCycle public CanSparkMaxSmartMotionRotationalPIDController(String configName, double absEncoderOffset, double absEncoderOffsetForZeroEncoderUnits, OffsetPoint first, OffsetPoint second, double ratio){ - loggerCategory = Category.MECHANISMS; + try{ mc = RobotProvider.instance.getMotor(configName); @@ -180,9 +180,7 @@ public void stop(){ //run loop that actually runs the PID //You need to call this function repedatly in OI as often as possible - public void run(boolean enabled){ - if(enabled){ - //Checking if Abs encoder is enabled, and if so we wouldn't want positions above 1 and below 0 + public void run(){ switch(theState){ case OFF: @@ -207,9 +205,7 @@ public void run(boolean enabled){ } break; } - } else{ - log("enabled is false on run loop PID controller"); - } + } From 5bbdf08d343863bda28a3d712997997530e7364a Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 17 Aug 2023 09:41:33 -0700 Subject: [PATCH 16/47] Update CanSparkMaxSmartMotionRotationalPIDController.java degrees to eu --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 05ce0703..f9e9dab0 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -94,7 +94,7 @@ private double euToDegrees(double eu){ } private double degreesToEu(double degrees){ - return (1/degreesToEncoderUnitsRatio) * degrees; + return degrees / degreesToEncoderUnitsRatio; } //manually changing the state From 4de9cf4c049e586f22f540e83e722138884ccd9c Mon Sep 17 00:00:00 2001 From: JayAgra <69493224+JayAgra@users.noreply.github.com> Date: Fri, 18 Aug 2023 12:11:07 -0700 Subject: [PATCH 17/47] "Please lint my branch" he said so i tried --- ...SmartMotionNonRotationalPIDController.java | 189 +++++++++--------- ...MaxSmartMotionRotationalPIDController.java | 183 +++++++++-------- .../com/team766/controllers/OffsetPoint.java | 6 +- 3 files changed, 199 insertions(+), 179 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java index d5112cf7..dff70813 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java @@ -1,4 +1,3 @@ - package com.team766.controllers; import com.team766.config.ConfigFileReader; @@ -8,6 +7,7 @@ import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; +import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; import edu.wpi.first.math.MathUtil; import com.revrobotics.SparkMaxAbsoluteEncoder; @@ -24,191 +24,200 @@ import com.team766.library.RateLimiter; -public class CanSparkMaxSmartMotionNonRotationalPIDController{ - +public class CanSparkMaxSmartMotionNonRotationalPIDController { + // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder private MotorController mc; + private CANSparkMax csm; + private SparkMaxPIDController pid; + private SparkMaxAbsoluteEncoder abs; - + //PID Related Variables - private static double deadzone = 0; + private static double deadzone = 0; + private static double setPointPosition = 0; + private static double comboOfTimesInsideDeadzone = 0; + private static double minPos = 0; - private static double maxPos = 0; + private static double maxPos = 0; //Precision variables private double degreesToEncoderUnitsRatio = 0; + private double encoderUnitsPerOneAbsolute = 0; //antigrav coefficient private static double antigravPower; //enum for which state the PID is in - public enum PIDSTATE{ + public enum PIDSTATE { PID, OFF, ANTIGRAV } //the state of the PID private PIDSTATE theState = PIDSTATE.OFF; - - //constructor for the class not using an absolute encoder for kDutyCycle - - public CanSparkMaxSmartMotionNonRotationalPIDController(String configName, double absEncoderOffset, double absEncoderOffsetForZeroEncoderUnits, OffsetPoint first, OffsetPoint second, double ratio){ - - try{ - mc = RobotProvider.instance.getMotor(configName); - csm = (CANSparkMax)mc; - pid = csm.getPIDController(); - abs = csm.getAbsoluteEncoder(Type.kDutyCycle); - abs.setZeroOffset(absEncoderOffset); - degreesToEncoderUnitsRatio = ratio; + //constructor for the class not using an absolute encoder for kDutyCycle + public CanSparkMaxSmartMotionNonRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double ratio) { + try { + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax) mc; + pid = csm.getPIDController(); + abs = csm.getAbsoluteEncoder(Type.kDutyCycle); + abs.setZeroOffset(absEncoderOffset); + degreesToEncoderUnitsRatio = ratio; - double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); - double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); + double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); + double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); - encoderUnitsPerOneAbsolute = motorEncoderDiference/absoluteDifference; + encoderUnitsPerOneAbsolute = motorEncoderDiference / absoluteDifference; - double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); + double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); - mc.setSensorPosition(relEncoder); - + mc.setSensorPosition(relEncoder); - }catch (IllegalArgumentException ill){ - throw new RuntimeException("Error instantiating the PID controller: " + ill); - } + } catch (IllegalArgumentException ill) { + throw new RuntimeException("Error instantiating the PID controller: " + ill); + } - } - private double absToEu(double abs){ - return encoderUnitsPerOneAbsolute / (1/abs); + private double absToEu(final double abs) { + return encoderUnitsPerOneAbsolute / (1 / abs); } - - private double degreesToEu(double degrees){ - return (1/degreesToEncoderUnitsRatio) * degrees; + private double degreesToEu(final double degrees) { + return (1 / degreesToEncoderUnitsRatio) * degrees; } //manually changing the state - public void updateState(PIDSTATE state){ + public void updateState(final PIDSTATE state) { theState = state; } + //changing all PID values at once - public void setPIDF(double p, double i, double d, double ff){ + public void setPIDF(final double p, final double i, final double d, final double ff) { pid.setP(p); pid.setI(i); pid.setD(d); pid.setFF(ff); } + //changing the P value - public void setP(double p){ + public void setP(final double p) { pid.setP(p); } + //changing the I value - public void setI(double i){ + public void setI(final double i) { pid.setI(i); } + //changing the D value - public void setD(double d){ + public void setD(final double d) { pid.setD(d); } + //changing the FF value - public void setFf(double ff){ + public void setFf(final double ff) { pid.setFF(ff); } - + /* - * Here we set the antigrav constant - * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with - * If the mechanism isn't rotational, this is just the amount of power to apply. - * @param k the value to set according to the above condition - */ - public void setAntigravPower(double power){ + * Here we set the antigrav constant + * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with + * If the mechanism isn't rotational, this is just the amount of power to apply. + * @param k the value to set according to the above condition + */ + + public void setAntigravPower(final double power) { antigravPower = power; } - private void antigrav(){ + private void antigrav() { mc.set(antigravPower); } //changing the deadzone - public void setDeadzone(double dz){ + public void setDeadzone(final double dz) { deadzone = dz; } + //changing the output range of the speed of the motors - public void setOutputRange(double min, double max){ + public void setOutputRange(final double min, final double max) { pid.setOutputRange(min, max); } + //changing the neutral mode of the motor (brake/coast) - public void setMotorMode(NeutralMode mode){ + public void setMotorMode(final NeutralMode mode) { mc.setNeutralMode(mode); } + //setting the maximum and minimum locations that the motor can go to - public void setMinMaxLocation(double min, double max){ + public void setMinMaxLocation(final double min, final double max) { maxPos = max; minPos = min; } + //setting the maximum velocity of the motor - public void setMaxVel(double max){ + public void setMaxVel(final double max) { pid.setSmartMotionMaxVelocity(max, 0); pid.setSmartMotionMinOutputVelocity(0, 0); } + //setting the maximum acceleration of the motor - public void setMaxAccel(double max){ + public void setMaxAccel(final double max) { pid.setSmartMotionMaxAccel(max, 0); } //1st step to go to a position using the normal PID, setting what you want the position to be - public void setSetpoint(double positionInDegrees){ + public void setSetpoint(final double positionInDegrees) { setPointPosition = MathUtil.clamp(degreesToEu(positionInDegrees), minPos, maxPos); theState = PIDSTATE.PID; } //Failsafe - public void stop(){ + public void stop() { setPointPosition = mc.getSensorPosition(); theState = PIDSTATE.OFF; } //run loop that actually runs the PID //You need to call this function repedatly in OI as often as possible - public void run(){ - - - - switch(theState){ - case OFF: - mc.set(0); - break; - case ANTIGRAV: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ - antigrav(); - } else { - theState = PIDSTATE.PID; - } - case PID: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ - comboOfTimesInsideDeadzone ++; - } else { - comboOfTimesInsideDeadzone = 0; - pid.setReference(setPointPosition, ControlType.kSmartMotion); - } - - if(comboOfTimesInsideDeadzone >= 6){ - theState = PIDSTATE.ANTIGRAV; - } - break; - } - - - } - - } - - + public void run() { + switch (theState) { + case OFF: + mc.set(0); + break; + case ANTIGRAV: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { + antigrav(); + } else { + theState = PIDSTATE.PID; + } + + case PID: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { + comboOfTimesInsideDeadzone++; + } else { + comboOfTimesInsideDeadzone = 0; + pid.setReference(setPointPosition, ControlType.kSmartMotion); + } + + if (comboOfTimesInsideDeadzone >= 6) { + theState = PIDSTATE.ANTIGRAV; + } + + break; + default: + LoggerExceptionUtils.logException(new IllegalArgumentException("Unknown state. Provided value: " + theState)); + break; + } + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index f9e9dab0..aa935893 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -7,6 +7,7 @@ import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; +import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; import edu.wpi.first.math.MathUtil; import com.revrobotics.SparkMaxAbsoluteEncoder; @@ -23,192 +24,202 @@ import com.team766.library.RateLimiter; -public class CanSparkMaxSmartMotionRotationalPIDController{ - +public class CanSparkMaxSmartMotionRotationalPIDController { + // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder private MotorController mc; + private CANSparkMax csm; + private SparkMaxPIDController pid; + private SparkMaxAbsoluteEncoder abs; - + //PID Related Variables - private static double deadzone = 0; + private static double deadzone = 0; + private static double setPointPosition = 0; + private static double comboOfTimesInsideDeadzone = 0; + private static double minPos = 0; - private static double maxPos = 0; + private static double maxPos = 0; //Precision variables private double degreesToEncoderUnitsRatio = 0; + private double encoderUnitsPerOneAbsolute = 0; //antigrav coefficient private static double antiGravK; //enum for which state the PID is in - public enum PIDSTATE{ + public enum PIDSTATE { PID, OFF, ANTIGRAV } //the state of the PID private PIDSTATE theState = PIDSTATE.OFF; - - //constructor for the class not using an absolute encoder for kDutyCycle - - public CanSparkMaxSmartMotionRotationalPIDController(String configName, double absEncoderOffset, double absEncoderOffsetForZeroEncoderUnits, OffsetPoint first, OffsetPoint second, double ratio){ - - try{ - mc = RobotProvider.instance.getMotor(configName); - csm = (CANSparkMax)mc; - pid = csm.getPIDController(); - abs = csm.getAbsoluteEncoder(Type.kDutyCycle); - abs.setZeroOffset(absEncoderOffset); - degreesToEncoderUnitsRatio = ratio; + //constructor for the class not using an absolute encoder for kDutyCycle + public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double ratio) { + try { + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax) mc; + pid = csm.getPIDController(); + abs = csm.getAbsoluteEncoder(Type.kDutyCycle); + abs.setZeroOffset(absEncoderOffset); + degreesToEncoderUnitsRatio = ratio; - double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); - double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); + double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); + double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); - encoderUnitsPerOneAbsolute = motorEncoderDiference/absoluteDifference; + encoderUnitsPerOneAbsolute = motorEncoderDiference / absoluteDifference; - double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); + double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); - mc.setSensorPosition(relEncoder); - + mc.setSensorPosition(relEncoder); - }catch (IllegalArgumentException ill){ - throw new RuntimeException("Error instantiating the PID controller: " + ill); - } + } catch (IllegalArgumentException ill) { + throw new RuntimeException("Error instantiating the PID controller: " + ill); + } - } - private double absToEu(double abs){ - return encoderUnitsPerOneAbsolute * abs; + private double absToEu(final double abs) { + return encoderUnitsPerOneAbsolute * abs; } - private double euToDegrees(double eu){ + private double euToDegrees(final double eu) { return eu * degreesToEncoderUnitsRatio; } - private double degreesToEu(double degrees){ + private double degreesToEu(final double degrees) { return degrees / degreesToEncoderUnitsRatio; } //manually changing the state - public void updateState(PIDSTATE state){ + public void updateState(final PIDSTATE state) { theState = state; } + //changing all PID values at once - public void setPIDF(double p, double i, double d, double ff){ + public void setPIDF(final double p, final double i, final double d, final double ff) { pid.setP(p); pid.setI(i); pid.setD(d); pid.setFF(ff); } + //changing the P value - public void setP(double p){ + public void setP(final double p) { pid.setP(p); } + //changing the I value - public void setI(double i){ + public void setI(final double i) { pid.setI(i); } + //changing the D value - public void setD(double d){ + public void setD(final double d) { pid.setD(d); } + //changing the FF value - public void setFf(double ff){ + public void setFf(final double ff) { pid.setFF(ff); } - + /* - * Here we set the antigrav constant - * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with - * If the mechanism isn't rotational, this is just the amount of power to apply. - * @param k the value to set according to the above condition - */ - public void setAntigravConstant(double k){ + * Here we set the antigrav constant + * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with + * If the mechanism isn't rotational, this is just the amount of power to apply. + * @param k the value to set according to the above condition + */ + + public void setAntigravConstant(final double k) { antiGravK = k; } - private void antigrav(){ + private void antigrav() { mc.set(antiGravK * Math.sin(euToDegrees(mc.getSensorPosition()))); } //changing the deadzone - public void setDeadzone(double dz){ + public void setDeadzone(final double dz) { deadzone = dz; } + //changing the output range of the speed of the motors - public void setOutputRange(double min, double max){ + public void setOutputRange(final double min, final double max) { pid.setOutputRange(min, max); } + //changing the neutral mode of the motor (brake/coast) - public void setMotorMode(NeutralMode mode){ + public void setMotorMode(final NeutralMode mode) { mc.setNeutralMode(mode); } + //setting the maximum and minimum locations that the motor can go to - public void setMinMaxLocation(double min, double max){ + public void setMinMaxLocation(final double min, final double max) { maxPos = max; minPos = min; } + //setting the maximum velocity of the motor - public void setMaxVel(double max){ + public void setMaxVel(final double max) { pid.setSmartMotionMaxVelocity(max, 0); pid.setSmartMotionMinOutputVelocity(0, 0); } + //setting the maximum acceleration of the motor - public void setMaxAccel(double max){ + public void setMaxAccel(final double max) { pid.setSmartMotionMaxAccel(max, 0); } //1st step to go to a position using the normal PID, setting what you want the position to be - public void setSetpoint(double positionInDegrees){ + public void setSetpoint(final double positionInDegrees) { setPointPosition = MathUtil.clamp(degreesToEu(positionInDegrees), minPos, maxPos); theState = PIDSTATE.PID; } //Failsafe - public void stop(){ + public void stop() { setPointPosition = mc.getSensorPosition(); theState = PIDSTATE.OFF; } //run loop that actually runs the PID //You need to call this function repedatly in OI as often as possible - public void run(){ - - switch(theState){ - case OFF: - mc.set(0); - break; - case ANTIGRAV: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ - antigrav(); - } else { - theState = PIDSTATE.PID; - } - case PID: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)){ - comboOfTimesInsideDeadzone ++; - } else { - comboOfTimesInsideDeadzone = 0; - pid.setReference(setPointPosition, ControlType.kSmartMotion); - } - - if(comboOfTimesInsideDeadzone >= 6){ - theState = PIDSTATE.ANTIGRAV; - } - break; - } - - - } - - } - - + public void run() { + switch (theState) { + case OFF: + mc.set(0); + break; + case ANTIGRAV: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { + antigrav(); + } else { + theState = PIDSTATE.PID; + } + case PID: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { + comboOfTimesInsideDeadzone++; + } else { + comboOfTimesInsideDeadzone = 0; + pid.setReference(setPointPosition, ControlType.kSmartMotion); + } + + if (comboOfTimesInsideDeadzone >= 6) { + theState = PIDSTATE.ANTIGRAV; + } + break; + default: + LoggerExceptionUtils.logException(new IllegalArgumentException("Unknown state. Provided value: " + theState)); + break; + } + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/controllers/OffsetPoint.java b/src/main/java/com/team766/controllers/OffsetPoint.java index ac86b239..d52d9749 100644 --- a/src/main/java/com/team766/controllers/OffsetPoint.java +++ b/src/main/java/com/team766/controllers/OffsetPoint.java @@ -4,16 +4,16 @@ public class OffsetPoint { private double absoluteValue, motorValue; - public OffsetPoint(double absoluteEncoderValue, double motorEncoderValue){ + public OffsetPoint(final double absoluteEncoderValue, final double motorEncoderValue) { absoluteValue = absoluteEncoderValue; motorValue = motorEncoderValue; } - public double getAbsoluteValue(){ + public double getAbsoluteValue() { return absoluteValue; } - public double getMotorEncoderValue(){ + public double getMotorEncoderValue() { return motorValue; } } From 7a2d5b6a1b77530d00307c1c7119617bf0b50eb7 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sun, 20 Aug 2023 02:33:09 +0000 Subject: [PATCH 18/47] make pidstate class and fix part of the errors --- ...SmartMotionNonRotationalPIDController.java | 9 ++--- ...MaxSmartMotionRotationalPIDController.java | 35 ++++++------------- .../com/team766/controllers/pidstate.java | 11 ++++++ 3 files changed, 23 insertions(+), 32 deletions(-) create mode 100644 src/main/java/com/team766/controllers/pidstate.java diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java index dff70813..c89e4ca2 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java @@ -5,10 +5,8 @@ import com.team766.library.SetValueProvider; import com.team766.library.SettableValueProvider; import com.team766.library.ValueProvider; -import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; -import com.team766.logging.Severity; import edu.wpi.first.math.MathUtil; import com.revrobotics.SparkMaxAbsoluteEncoder; import com.revrobotics.SparkMaxPIDController; @@ -22,6 +20,7 @@ import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.library.RateLimiter; +import com.team766.controllers.pidstate.*; public class CanSparkMaxSmartMotionNonRotationalPIDController { @@ -54,11 +53,7 @@ public class CanSparkMaxSmartMotionNonRotationalPIDController { //antigrav coefficient private static double antigravPower; //enum for which state the PID is in - public enum PIDSTATE { - PID, - OFF, - ANTIGRAV - } + //the state of the PID private PIDSTATE theState = PIDSTATE.OFF; diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index aa935893..bf2dcdc3 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -5,10 +5,8 @@ import com.team766.library.SetValueProvider; import com.team766.library.SettableValueProvider; import com.team766.library.ValueProvider; -import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; -import com.team766.logging.Severity; import edu.wpi.first.math.MathUtil; import com.revrobotics.SparkMaxAbsoluteEncoder; import com.revrobotics.SparkMaxPIDController; @@ -22,43 +20,36 @@ import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.library.RateLimiter; +import com.team766.controllers.pidstate.*; + public class CanSparkMaxSmartMotionRotationalPIDController { - // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder + //Motorcontroller for the motor private MotorController mc; + //Variables that we need to be able to do this type of PID private CANSparkMax csm; - private SparkMaxPIDController pid; - private SparkMaxAbsoluteEncoder abs; //PID Related Variables private static double deadzone = 0; - private static double setPointPosition = 0; - private static double comboOfTimesInsideDeadzone = 0; - private static double minPos = 0; - private static double maxPos = 0; - //Precision variables + //Reset encoder variables private double degreesToEncoderUnitsRatio = 0; - private double encoderUnitsPerOneAbsolute = 0; //antigrav coefficient private static double antiGravK; //enum for which state the PID is in - public enum PIDSTATE { - PID, - OFF, - ANTIGRAV - } + + //the state of the PID private PIDSTATE theState = PIDSTATE.OFF; @@ -100,11 +91,6 @@ private double degreesToEu(final double degrees) { return degrees / degreesToEncoderUnitsRatio; } - //manually changing the state - public void updateState(final PIDSTATE state) { - theState = state; - } - //changing all PID values at once public void setPIDF(final double p, final double i, final double d, final double ff) { pid.setP(p); @@ -135,9 +121,8 @@ public void setFf(final double ff) { /* * Here we set the antigrav constant - * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with - * If the mechanism isn't rotational, this is just the amount of power to apply. - * @param k the value to set according to the above condition + * The mechanism is rotational, so this is the amount we multiply the Sine of the sensor position with + * @param k the value to set for the antigrav constant */ public void setAntigravConstant(final double k) { @@ -193,7 +178,7 @@ public void stop() { } //run loop that actually runs the PID - //You need to call this function repedatly in OI as often as possible + //You need to call this function repedatly in the mechanism's run function as often as possible to get the best results public void run() { switch (theState) { case OFF: diff --git a/src/main/java/com/team766/controllers/pidstate.java b/src/main/java/com/team766/controllers/pidstate.java new file mode 100644 index 00000000..07445515 --- /dev/null +++ b/src/main/java/com/team766/controllers/pidstate.java @@ -0,0 +1,11 @@ +package com.team766.controllers; + +abstract class pidstate { + + public enum PIDSTATE { + PID, + OFF, + ANTIGRAV + } + +} From ddbb139835f91d14bda6f52cfd9cf3f0d6ac25c5 Mon Sep 17 00:00:00 2001 From: Max <> Date: Sat, 9 Sep 2023 14:44:59 -0700 Subject: [PATCH 19/47] not done yet but placeholder for now --- ...leCanSparkMaxSmartMotionPIDController.java | 218 ++++++++++++++++++ .../AbstractPIDRuntimeException.java | 5 + 2 files changed, 223 insertions(+) create mode 100644 src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java create mode 100644 src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java new file mode 100644 index 00000000..42f6bbd7 --- /dev/null +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -0,0 +1,218 @@ +package com.team766.controllers; + +import com.team766.config.ConfigFileReader; +import com.team766.hal.RobotProvider; +import com.team766.library.SetValueProvider; +import com.team766.library.SettableValueProvider; +import com.team766.library.ValueProvider; +import com.team766.logging.Logger; +import com.team766.logging.LoggerExceptionUtils; +import edu.wpi.first.math.MathUtil; +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import java.io.IOError; +import javax.swing.text.DefaultStyledDocument.ElementSpec; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.team766.framework.Mechanism; +import com.team766.hal.MotorController; +import com.team766.library.RateLimiter; +import com.team766.controllers.pidstate.*; + + +public abstract class ExtendableCanSparkMaxSmartMotionPIDController { + /* + * These are the hardware variables for the hardware we have on the robot. + * It includes a CanSparkMax, MotorController of any type, and a SparkMaxAbsoluteEncoder. + * @author Max Spier - 9/9/2023 + */ + private MotorController mc; + private CANSparkMax csm; + private SparkMaxAbsoluteEncoder abs; + + /* + * These are PID related variables that we will store locally to run PIDs + * We also create an object for the PIDController + * @author Max Spier - 9/9/2023 + */ + private SparkMaxPIDController pid; + private static double deadzone = 0; + private static double setPointPosition = 0; + private static double comboOfTimesInsideDeadzone = 0; + private static double minPos = 0; + private static double maxPos = 0; + + /* + * These are some variables that we can use so that we can reset the motor controller + * We use this so that we can get an accurate ratio for the hall encoder units to absolute encoder units. + * @author Max Spier - 9/9/2023 + */ + private double encoderUnitsPerOneAbsolute = 0; + + + /* + * The state of the PID controller so that we know whether to call the respective method for each state. + * For more documentation, see the PIDSTATE class. + * @author Max Spier - 9/9/2023 + */ + + private PIDSTATE theState = PIDSTATE.OFF; + + /* + * Default constructor for the class. This is used to create a new CanSparkMaxSmartMotionPIDController object + * @param configName the configuration name for the motorcontroller + * @param absEncoderOffset the offset (ranging from 0.000 - 0.999) of the absolute encoder + * @param absEncoderOffsetForZeroEncoderUnits the offset that the absolute encoder should have in order for the relative encoder on the motorcontroller to equal zero. + * @param first the first offsetpoint used to reset the encoders - for more info see the OffsetPoint class + * @param second the second offsetpoint used to reset the encoders - for more info see the OffsetPoint class + * @author Max Spier - 9/9/2023 + */ + + + public ExtendableCanSparkMaxSmartMotionPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { + try { + mc = RobotProvider.instance.getMotor(configName); + csm = (CANSparkMax) mc; + pid = csm.getPIDController(); + abs = csm.getAbsoluteEncoder(Type.kDutyCycle); + abs.setZeroOffset(absEncoderOffset); + + double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); + double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); + + encoderUnitsPerOneAbsolute = motorEncoderDiference / absoluteDifference; + + double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); + + mc.setSensorPosition(relEncoder); + + } catch (IllegalArgumentException ill) { + throw new AbstractPIDRuntimeException("Error instantiating the PID controller: " + ill); + } + } + + /* + * Method to convert the absolute encoder units into hall encoder units + * @author Max Spier - 9/9/2023 + */ + private double absToEu(final double abs) { + return encoderUnitsPerOneAbsolute * abs; + } + + /* + * Method to change all PIDF values at once + * @params p-ff the values to set each one for the PIDController + * @author Max Spier - 9/9/2023 + */ + public void setPIDF(final double p, final double i, final double d, final double ff) { + pid.setP(p); + pid.setI(i); + pid.setD(d); + pid.setFF(ff); + } + + /* + * Method to change the P value of the PIDController + * @param p the value of P to set for the PIDController + * @author Max Spier - 9/9/2023 + */ + public void setP(final double p) { + pid.setP(p); + } + + /* + * Method to change the I value of the PIDController + * @param i the value of I to set for the PIDController + * @author Max Spier - 9/9/2023 + */ + public void setI(final double i) { + pid.setI(i); + } + + /* + * Method to change the D value of the PIDController + * @param d the value of D to set for the PIDController + * @author Max Spier - 9/9/2023 + */ + public void setD(final double d) { + pid.setD(d); + } + + /* + * Method to change the FF value of the PIDController + * @param ff the value of FF to set for the PIDController + * @author Max Spier - 9/9/2023 + */ + public void setFF(final double ff) { + pid.setFF(ff); + } + + /* + * Method to update the value of the deadzone for the PIDController + * This deadzone is in hall encoder units. + * @param dz the value of the deadzone in hall encoder units + * @author Max Spier - 9/9/2023 + */ + public void setDeadzone(final double dz) { + deadzone = dz; + } + + /* + * This is a method to update the minimum and maximum output ranges of motorpower for the mechanism + * @param min the minimum value for motor power (make sure that it is negative if you want the mechanism to be able to go both directions) + * @param max the maximum value for motor power (make sure that it is positive if you want the mechanism to be able to go both directions) + * @author Max Spier - 9/9/2023 + */ + public void setOutputRange(final double min, final double max) { + pid.setOutputRange(min, max); + } + + /* + * This is the method to set the minimum and maximum locations of the mechanism. + * Please check your subclasses documentation for what units this is supposed to be in (usually whichever one is used as an input for the setSetpoint method) + * @author Max Spier - 9/9/2023 + */ + public void setMinMaxLocation(final double min, final double max) { + maxPos = max; + minPos = min; + } + + /* + * This method is used to set the maximum velocity of the mechanism for use with PID + * @param max the value of the maximum amount of velocity for the motorcontroller + * The method also sets the minimum velocity to zero, as a failsafe. + * @author Max Spier - 9/9/2023 + */ + public void setMaxVelocity(final double max) { + pid.setSmartMotionMaxVelocity(max, 0); + pid.setSmartMotionMinOutputVelocity(0, 0); + } + + /* + * This is the method to set the maximum acelleration of the robot for the PIDController + * @param max the value for the maximum acelleration + * @author Max Spier - 9/9/2023 + */ + public void setMaxAccel(final double max) { + pid.setSmartMotionMaxAccel(max, 0); + } + + /* + * This is the method where you update the setpoint position from the subclass + * + */ + public void updateSetpointFromSubclass(double x){ + x = MathUtil.Clamp(x, minPos, maxPos); + setPointPosition = x; + } + + + + + + + +} diff --git a/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java b/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java new file mode 100644 index 00000000..f03499ab --- /dev/null +++ b/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java @@ -0,0 +1,5 @@ +public class AbstractPIDRuntimeException extends RuntimeException { + public AbstractPIDRuntimeException (String errorMessage) { + super(errorMessage); + } +} From 5bbebcaa17eedf9b834a342efb6c776c6148ede4 Mon Sep 17 00:00:00 2001 From: Max <> Date: Sat, 9 Sep 2023 15:25:47 -0700 Subject: [PATCH 20/47] hehe added run loop --- ...leCanSparkMaxSmartMotionPIDController.java | 46 ++++++++++++++++++- 1 file changed, 45 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 42f6bbd7..ab24b37c 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -21,6 +21,7 @@ import com.team766.hal.MotorController; import com.team766.library.RateLimiter; import com.team766.controllers.pidstate.*; +import com.team766.framework.Exceptions.*; public abstract class ExtendableCanSparkMaxSmartMotionPIDController { @@ -202,12 +203,54 @@ public void setMaxAccel(final double max) { /* * This is the method where you update the setpoint position from the subclass - * + * @param x the setpoint + * @author Max Spier - 9/9/2023 */ public void updateSetpointFromSubclass(double x){ x = MathUtil.Clamp(x, minPos, maxPos); setPointPosition = x; } + + /* + * Failsafe for the PIDController to stop it. + */ + public void stop() { + theState = PIDSTATE.OFF; + } + + /* + * This is the run loop that actually runs the PID Controller + * You need to call this as frequently as possible when running the mechanism + * @author Max Spier - 9/9/2023 + */ + public void run() { + switch (theState) { + case OFF: + mc.set(0); + break; + case ANTIGRAV: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { + antigrav(); + } else { + theState = PIDSTATE.PID; + } + case PID: + if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { + comboOfTimesInsideDeadzone++; + } else { + comboOfTimesInsideDeadzone = 0; + pid.setReference(setPointPosition, ControlType.kSmartMotion); + } + + if (comboOfTimesInsideDeadzone >= 6) { + theState = PIDSTATE.ANTIGRAV; + } + break; + default: + LoggerExceptionUtils.logException(new IllegalArgumentException("Unknown state. Provided value: " + theState)); + break; + } + } @@ -215,4 +258,5 @@ public void updateSetpointFromSubclass(double x){ + } From 1f7c1bc9275e1130b114ce6051f1f77b5c44501e Mon Sep 17 00:00:00 2001 From: Max <> Date: Sat, 9 Sep 2023 15:28:28 -0700 Subject: [PATCH 21/47] fix exception --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index ab24b37c..4c3d82f5 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -247,7 +247,7 @@ public void run() { } break; default: - LoggerExceptionUtils.logException(new IllegalArgumentException("Unknown state. Provided value: " + theState)); + throw new AbstractPIDRuntimeException("Error instantiating the PID controller: " + ill); break; } } From 9b8bdbb79f67524b931f8e6af5deaf1c2a5cccba Mon Sep 17 00:00:00 2001 From: Max <> Date: Sat, 9 Sep 2023 16:00:26 -0700 Subject: [PATCH 22/47] e --- ...parkMaxSmartMotionLinearPIDController.java | 17 ++++++++++++++ ...leCanSparkMaxSmartMotionPIDController.java | 23 ++++++++++++++++++- 2 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java new file mode 100644 index 00000000..94563941 --- /dev/null +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -0,0 +1,17 @@ +public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { + + private double antigravPower; + + + public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { + super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); + } + + + + public void runPIDs(){ + updateAntigrav(antigravPower); + run(); + } + +} diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 4c3d82f5..d8197b66 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -52,6 +52,8 @@ public abstract class ExtendableCanSparkMaxSmartMotionPIDController { * @author Max Spier - 9/9/2023 */ private double encoderUnitsPerOneAbsolute = 0; + + private double curAntigrav = 0; /* @@ -213,11 +215,30 @@ public void updateSetpointFromSubclass(double x){ /* * Failsafe for the PIDController to stop it. + * @author Max Spier - 9/9/2023 */ public void stop() { theState = PIDSTATE.OFF; } + /* + * This method does the antigrav + * @author Max Spier - 9/9/2023 + */ + private void antigrav(){ + mc.set(curAntigrav); + } + + /* + * This method sets the antigrav power amount. + * It gets the value for this from the subclass + * @param passedValueFromSubclass the power to set the motorcontroller for antigrav + * @author Max Spier - 9/9/2023 + */ + public void updateAntigrav(double passedValueFromSubclass){ + curAntigrav = passedValueFromSubclass + } + /* * This is the run loop that actually runs the PID Controller * You need to call this as frequently as possible when running the mechanism @@ -247,7 +268,7 @@ public void run() { } break; default: - throw new AbstractPIDRuntimeException("Error instantiating the PID controller: " + ill); + throw new AbstractPIDRuntimeException("Invalid state error"); break; } } From 761553a22bb88bf9621e2e01ea3a2ec7067a9bd1 Mon Sep 17 00:00:00 2001 From: Max <> Date: Sat, 9 Sep 2023 16:38:48 -0700 Subject: [PATCH 23/47] ready for pr --- ...parkMaxSmartMotionLinearPIDController.java | 9 +- ...SmartMotionNonRotationalPIDController.java | 218 ------------------ ...MaxSmartMotionRotationalPIDController.java | 212 ++--------------- 3 files changed, 26 insertions(+), 413 deletions(-) delete mode 100644 src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java index 94563941..196714a8 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -1,12 +1,19 @@ public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { - private double antigravPower; + private final double antigravPower; public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); } + public void setAntigravPower(double power){ + antigravPower = power; + } + + public void setNewSetpoint(double setPoint){ + updateSetpointFromSubclass(setPoint); + } public void runPIDs(){ diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java deleted file mode 100644 index c89e4ca2..00000000 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java +++ /dev/null @@ -1,218 +0,0 @@ -package com.team766.controllers; - -import com.team766.config.ConfigFileReader; -import com.team766.hal.RobotProvider; -import com.team766.library.SetValueProvider; -import com.team766.library.SettableValueProvider; -import com.team766.library.ValueProvider; -import com.team766.logging.Logger; -import com.team766.logging.LoggerExceptionUtils; -import edu.wpi.first.math.MathUtil; -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import java.io.IOError; -import javax.swing.text.DefaultStyledDocument.ElementSpec; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.revrobotics.CANSparkMax; -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; -import com.team766.controllers.pidstate.*; - - -public class CanSparkMaxSmartMotionNonRotationalPIDController { - - // The attributes of the class include references to the motor controller, SparkMax controller, PID controller, and absolute encoder - private MotorController mc; - - private CANSparkMax csm; - - private SparkMaxPIDController pid; - - private SparkMaxAbsoluteEncoder abs; - - //PID Related Variables - private static double deadzone = 0; - - private static double setPointPosition = 0; - - private static double comboOfTimesInsideDeadzone = 0; - - private static double minPos = 0; - - private static double maxPos = 0; - - //Precision variables - private double degreesToEncoderUnitsRatio = 0; - - private double encoderUnitsPerOneAbsolute = 0; - - //antigrav coefficient - private static double antigravPower; - //enum for which state the PID is in - - //the state of the PID - private PIDSTATE theState = PIDSTATE.OFF; - - //constructor for the class not using an absolute encoder for kDutyCycle - public CanSparkMaxSmartMotionNonRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double ratio) { - - try { - mc = RobotProvider.instance.getMotor(configName); - csm = (CANSparkMax) mc; - pid = csm.getPIDController(); - abs = csm.getAbsoluteEncoder(Type.kDutyCycle); - abs.setZeroOffset(absEncoderOffset); - degreesToEncoderUnitsRatio = ratio; - - double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); - double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); - - encoderUnitsPerOneAbsolute = motorEncoderDiference / absoluteDifference; - - double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); - - mc.setSensorPosition(relEncoder); - - } catch (IllegalArgumentException ill) { - throw new RuntimeException("Error instantiating the PID controller: " + ill); - } - - } - - private double absToEu(final double abs) { - return encoderUnitsPerOneAbsolute / (1 / abs); - } - - private double degreesToEu(final double degrees) { - return (1 / degreesToEncoderUnitsRatio) * degrees; - } - - //manually changing the state - public void updateState(final PIDSTATE state) { - theState = state; - } - - //changing all PID values at once - public void setPIDF(final double p, final double i, final double d, final double ff) { - pid.setP(p); - pid.setI(i); - pid.setD(d); - pid.setFF(ff); - } - - //changing the P value - public void setP(final double p) { - pid.setP(p); - } - - //changing the I value - public void setI(final double i) { - pid.setI(i); - } - - //changing the D value - public void setD(final double d) { - pid.setD(d); - } - - //changing the FF value - public void setFf(final double ff) { - pid.setFF(ff); - } - - /* - * Here we set the antigrav constant - * If the mechanism is rotational, this is the amount we multiply the Sine of the sensor position with - * If the mechanism isn't rotational, this is just the amount of power to apply. - * @param k the value to set according to the above condition - */ - - public void setAntigravPower(final double power) { - antigravPower = power; - } - - private void antigrav() { - mc.set(antigravPower); - } - - //changing the deadzone - public void setDeadzone(final double dz) { - deadzone = dz; - } - - //changing the output range of the speed of the motors - public void setOutputRange(final double min, final double max) { - pid.setOutputRange(min, max); - } - - //changing the neutral mode of the motor (brake/coast) - public void setMotorMode(final NeutralMode mode) { - mc.setNeutralMode(mode); - } - - //setting the maximum and minimum locations that the motor can go to - public void setMinMaxLocation(final double min, final double max) { - maxPos = max; - minPos = min; - } - - //setting the maximum velocity of the motor - public void setMaxVel(final double max) { - pid.setSmartMotionMaxVelocity(max, 0); - pid.setSmartMotionMinOutputVelocity(0, 0); - } - - //setting the maximum acceleration of the motor - public void setMaxAccel(final double max) { - pid.setSmartMotionMaxAccel(max, 0); - } - - //1st step to go to a position using the normal PID, setting what you want the position to be - public void setSetpoint(final double positionInDegrees) { - setPointPosition = MathUtil.clamp(degreesToEu(positionInDegrees), minPos, maxPos); - theState = PIDSTATE.PID; - } - - //Failsafe - public void stop() { - setPointPosition = mc.getSensorPosition(); - theState = PIDSTATE.OFF; - } - - //run loop that actually runs the PID - //You need to call this function repedatly in OI as often as possible - public void run() { - switch (theState) { - case OFF: - mc.set(0); - break; - case ANTIGRAV: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { - antigrav(); - } else { - theState = PIDSTATE.PID; - } - - case PID: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { - comboOfTimesInsideDeadzone++; - } else { - comboOfTimesInsideDeadzone = 0; - pid.setReference(setPointPosition, ControlType.kSmartMotion); - } - - if (comboOfTimesInsideDeadzone >= 6) { - theState = PIDSTATE.ANTIGRAV; - } - - break; - default: - LoggerExceptionUtils.logException(new IllegalArgumentException("Unknown state. Provided value: " + theState)); - break; - } - } -} \ No newline at end of file diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index bf2dcdc3..69856ea1 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -1,210 +1,34 @@ -package com.team766.controllers; +import java.*; -import com.team766.config.ConfigFileReader; -import com.team766.hal.RobotProvider; -import com.team766.library.SetValueProvider; -import com.team766.library.SettableValueProvider; -import com.team766.library.ValueProvider; -import com.team766.logging.Logger; -import com.team766.logging.LoggerExceptionUtils; -import edu.wpi.first.math.MathUtil; -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import java.io.IOError; -import javax.swing.text.DefaultStyledDocument.ElementSpec; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.revrobotics.CANSparkMax; -import com.team766.framework.Mechanism; -import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; -import com.team766.controllers.pidstate.*; +public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { + + private final double degreesToEncoderUnitsRatio; + private final double antiGravK; - - -public class CanSparkMaxSmartMotionRotationalPIDController { - - //Motorcontroller for the motor - private MotorController mc; - - //Variables that we need to be able to do this type of PID - private CANSparkMax csm; - private SparkMaxPIDController pid; - private SparkMaxAbsoluteEncoder abs; - - //PID Related Variables - private static double deadzone = 0; - private static double setPointPosition = 0; - private static double comboOfTimesInsideDeadzone = 0; - private static double minPos = 0; - private static double maxPos = 0; - - //Reset encoder variables - private double degreesToEncoderUnitsRatio = 0; - private double encoderUnitsPerOneAbsolute = 0; - - //antigrav coefficient - private static double antiGravK; - //enum for which state the PID is in - - - //the state of the PID - private PIDSTATE theState = PIDSTATE.OFF; - - //constructor for the class not using an absolute encoder for kDutyCycle - public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double ratio) { - - try { - mc = RobotProvider.instance.getMotor(configName); - csm = (CANSparkMax) mc; - pid = csm.getPIDController(); - abs = csm.getAbsoluteEncoder(Type.kDutyCycle); - abs.setZeroOffset(absEncoderOffset); - degreesToEncoderUnitsRatio = ratio; - - double absoluteDifference = second.getAbsoluteValue() - first.getAbsoluteValue(); - double motorEncoderDiference = second.getMotorEncoderValue() - first.getMotorEncoderValue(); - - encoderUnitsPerOneAbsolute = motorEncoderDiference / absoluteDifference; - - double relEncoder = absToEu(abs.getPosition() - absEncoderOffsetForZeroEncoderUnits); - - mc.setSensorPosition(relEncoder); - - } catch (IllegalArgumentException ill) { - throw new RuntimeException("Error instantiating the PID controller: " + ill); - } - - } - - private double absToEu(final double abs) { - return encoderUnitsPerOneAbsolute * abs; - } - - private double euToDegrees(final double eu) { - return eu * degreesToEncoderUnitsRatio; - } - - private double degreesToEu(final double degrees) { - return degrees / degreesToEncoderUnitsRatio; + public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { + super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); } - //changing all PID values at once - public void setPIDF(final double p, final double i, final double d, final double ff) { - pid.setP(p); - pid.setI(i); - pid.setD(d); - pid.setFF(ff); + public void updateDegreesToEncoderUnitsRatio(double ratio){ + degreesToEncoderUnitsRatio = ratio; } - //changing the P value - public void setP(final double p) { - pid.setP(p); - } - - //changing the I value - public void setI(final double i) { - pid.setI(i); - } - - //changing the D value - public void setD(final double d) { - pid.setD(d); - } - - //changing the FF value - public void setFf(final double ff) { - pid.setFF(ff); - } - - /* - * Here we set the antigrav constant - * The mechanism is rotational, so this is the amount we multiply the Sine of the sensor position with - * @param k the value to set for the antigrav constant - */ - - public void setAntigravConstant(final double k) { + public void setAntigravConstant(double k) { antiGravK = k; } - private void antigrav() { - mc.set(antiGravK * Math.sin(euToDegrees(mc.getSensorPosition()))); - } - - //changing the deadzone - public void setDeadzone(final double dz) { - deadzone = dz; - } - - //changing the output range of the speed of the motors - public void setOutputRange(final double min, final double max) { - pid.setOutputRange(min, max); - } - - //changing the neutral mode of the motor (brake/coast) - public void setMotorMode(final NeutralMode mode) { - mc.setNeutralMode(mode); - } - - //setting the maximum and minimum locations that the motor can go to - public void setMinMaxLocation(final double min, final double max) { - maxPos = max; - minPos = min; - } - - //setting the maximum velocity of the motor - public void setMaxVel(final double max) { - pid.setSmartMotionMaxVelocity(max, 0); - pid.setSmartMotionMinOutputVelocity(0, 0); - } - - //setting the maximum acceleration of the motor - public void setMaxAccel(final double max) { - pid.setSmartMotionMaxAccel(max, 0); + public void setNewSetpoint(double setPoint){ + updateSetpointFromSubclass(setPoint); } - //1st step to go to a position using the normal PID, setting what you want the position to be - public void setSetpoint(final double positionInDegrees) { - setPointPosition = MathUtil.clamp(degreesToEu(positionInDegrees), minPos, maxPos); - theState = PIDSTATE.PID; + private double euToDegrees(final double eu) { + return eu * degreesToEncoderUnitsRatio; } - //Failsafe - public void stop() { - setPointPosition = mc.getSensorPosition(); - theState = PIDSTATE.OFF; + public void runPIDs(){ + updateAntigrav(antiGravK * Math.sin(euToDegrees(mc.getSensorPosition()))); + run(); } - //run loop that actually runs the PID - //You need to call this function repedatly in the mechanism's run function as often as possible to get the best results - public void run() { - switch (theState) { - case OFF: - mc.set(0); - break; - case ANTIGRAV: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { - antigrav(); - } else { - theState = PIDSTATE.PID; - } - case PID: - if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { - comboOfTimesInsideDeadzone++; - } else { - comboOfTimesInsideDeadzone = 0; - pid.setReference(setPointPosition, ControlType.kSmartMotion); - } - if (comboOfTimesInsideDeadzone >= 6) { - theState = PIDSTATE.ANTIGRAV; - } - break; - default: - LoggerExceptionUtils.logException(new IllegalArgumentException("Unknown state. Provided value: " + theState)); - break; - } - } -} \ No newline at end of file +} From 1a4285eac0f3c3862b10a6d188c404157b79dd48 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:36:16 -0700 Subject: [PATCH 24/47] change 2 methods from public to protected --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index d8197b66..9b7cc46e 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -208,7 +208,7 @@ public void setMaxAccel(final double max) { * @param x the setpoint * @author Max Spier - 9/9/2023 */ - public void updateSetpointFromSubclass(double x){ + protected void updateSetpointFromSubclass(double x){ x = MathUtil.Clamp(x, minPos, maxPos); setPointPosition = x; } @@ -235,7 +235,7 @@ private void antigrav(){ * @param passedValueFromSubclass the power to set the motorcontroller for antigrav * @author Max Spier - 9/9/2023 */ - public void updateAntigrav(double passedValueFromSubclass){ + protected void updateAntigrav(double passedValueFromSubclass){ curAntigrav = passedValueFromSubclass } From 28e426e3a1a3eb926da7bbac9a9ae41ea98cf0d5 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:44:51 -0700 Subject: [PATCH 25/47] Add documentation for OffsetPoint.java --- src/main/java/com/team766/controllers/OffsetPoint.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/OffsetPoint.java b/src/main/java/com/team766/controllers/OffsetPoint.java index d52d9749..0392820c 100644 --- a/src/main/java/com/team766/controllers/OffsetPoint.java +++ b/src/main/java/com/team766/controllers/OffsetPoint.java @@ -1,5 +1,9 @@ package com.team766.controllers; - + /* + * This is a class to make an offset point for the PID controller to be able to reset encoders. + * You input the absoluteEncoderValue that equals the motorEncoderValue. + * @author Max Spier - 9/12/23 + */ public class OffsetPoint { private double absoluteValue, motorValue; From 01d079e684e039af2ccb8ae8ae7b69fcf08f3191 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:48:37 -0700 Subject: [PATCH 26/47] Delete class --- src/main/java/com/team766/controllers/pidstate.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/com/team766/controllers/pidstate.java b/src/main/java/com/team766/controllers/pidstate.java index 07445515..a4eca012 100644 --- a/src/main/java/com/team766/controllers/pidstate.java +++ b/src/main/java/com/team766/controllers/pidstate.java @@ -1,11 +1,7 @@ package com.team766.controllers; -abstract class pidstate { - public enum PIDSTATE { PID, OFF, ANTIGRAV } - -} From d8dcc62d1a62f2f8564c4e4558c16b254b8bffa7 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:49:02 -0700 Subject: [PATCH 27/47] Rename pidstate.java to PIDSTATE.java --- .../java/com/team766/controllers/{pidstate.java => PIDSTATE.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/com/team766/controllers/{pidstate.java => PIDSTATE.java} (100%) diff --git a/src/main/java/com/team766/controllers/pidstate.java b/src/main/java/com/team766/controllers/PIDSTATE.java similarity index 100% rename from src/main/java/com/team766/controllers/pidstate.java rename to src/main/java/com/team766/controllers/PIDSTATE.java From e08dfda499ed071c4c153b1147d069f84c57ab7b Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:50:08 -0700 Subject: [PATCH 28/47] Refactoring changes needed to be made with the renaming of pidstate to PIDSTATE --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 9b7cc46e..cd810c5f 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -20,7 +20,7 @@ import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.library.RateLimiter; -import com.team766.controllers.pidstate.*; +import com.team766.controllers.PIDSTATE.*; import com.team766.framework.Exceptions.*; From dd4ca19ecf658d46e639418fcc4613256bbe54bb Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:51:47 -0700 Subject: [PATCH 29/47] Added method to constructor --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 69856ea1..6ce8e6ac 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -5,14 +5,11 @@ public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCan private final double degreesToEncoderUnitsRatio; private final double antiGravK; - public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { + public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, double degToEncoderUnitsRatio) { + degreesToEncoderUnitsRatio = degToEncoderUnitsRatio; super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); } - public void updateDegreesToEncoderUnitsRatio(double ratio){ - degreesToEncoderUnitsRatio = ratio; - } - public void setAntigravConstant(double k) { antiGravK = k; } From e82f04307ce5d9572266559ab64c661e6fd669fd Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Tue, 12 Sep 2023 20:59:03 -0700 Subject: [PATCH 30/47] Add documentation --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 6ce8e6ac..df337037 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -14,6 +14,8 @@ public void setAntigravConstant(double k) { antiGravK = k; } + + //Setpoint is in hall encoder units. public void setNewSetpoint(double setPoint){ updateSetpointFromSubclass(setPoint); } From fc78f8f4a6e506f4b0dc414ff5e7a3f04ab47885 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Wed, 13 Sep 2023 17:43:10 -0700 Subject: [PATCH 31/47] fix final method with change --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index df337037..8e176c53 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -4,16 +4,12 @@ public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCan private final double degreesToEncoderUnitsRatio; private final double antiGravK; - - public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, double degToEncoderUnitsRatio) { + + public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, double degToEncoderUnitsRatio, double antigravityCoefficent) { + antiGravK = antigravityCoefficent; degreesToEncoderUnitsRatio = degToEncoderUnitsRatio; super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); } - - public void setAntigravConstant(double k) { - antiGravK = k; - } - //Setpoint is in hall encoder units. public void setNewSetpoint(double setPoint){ From ed5d4d04d0a61b3c789a8fd87d0b4f8fcb8674f3 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 02:32:00 +0000 Subject: [PATCH 32/47] ryans changes --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 6 +----- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 4 ++-- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 8e176c53..f497d677 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -6,15 +6,11 @@ public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCan private final double antiGravK; public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, double degToEncoderUnitsRatio, double antigravityCoefficent) { + super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); antiGravK = antigravityCoefficent; degreesToEncoderUnitsRatio = degToEncoderUnitsRatio; - super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); } - //Setpoint is in hall encoder units. - public void setNewSetpoint(double setPoint){ - updateSetpointFromSubclass(setPoint); - } private double euToDegrees(final double eu) { return eu * degreesToEncoderUnitsRatio; diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index cd810c5f..1d91cd84 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -208,8 +208,8 @@ public void setMaxAccel(final double max) { * @param x the setpoint * @author Max Spier - 9/9/2023 */ - protected void updateSetpointFromSubclass(double x){ - x = MathUtil.Clamp(x, minPos, maxPos); + public void setNewSetpoint(double x){ + x = MathUtil.clamp(x, minPos, maxPos); setPointPosition = x; } From 65ee961e9b055ca44c27328609b4d6aa5aad1de7 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 02:36:47 +0000 Subject: [PATCH 33/47] fix the antigrav code --- .../CanSparkMaxSmartMotionRotationalPIDController.java | 6 ++++-- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 9 +++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index f497d677..6abfffcb 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -1,4 +1,6 @@ -import java.*; +package com.team766.controllers; + +import java.util.*; public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { @@ -17,7 +19,7 @@ private double euToDegrees(final double eu) { } public void runPIDs(){ - updateAntigrav(antiGravK * Math.sin(euToDegrees(mc.getSensorPosition()))); + updateAntigrav(antiGravK * Math.sin(euToDegrees(getHallSensorPosition()))); run(); } diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 1d91cd84..1c126089 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -104,6 +104,15 @@ public ExtendableCanSparkMaxSmartMotionPIDController(final String configName, fi private double absToEu(final double abs) { return encoderUnitsPerOneAbsolute * abs; } + + /* + * This is a method to get the hall sensor position from the motorcontroller, so we can use it in the antigravity calculations in the subclass. + * @return the value of the hall sensor position + * @author Max Spier - 9/13/2023 + */ + protected double getHallSensorPosition(){ + return mc.getSensorPosition(); + } /* * Method to change all PIDF values at once From f5b38a994dc05615930e4c574b5d3dc76e64cf03 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 02:37:33 +0000 Subject: [PATCH 34/47] Fix compile errors with the abstract class --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 1c126089..fec471e2 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -245,7 +245,7 @@ private void antigrav(){ * @author Max Spier - 9/9/2023 */ protected void updateAntigrav(double passedValueFromSubclass){ - curAntigrav = passedValueFromSubclass + curAntigrav = passedValueFromSubclass; } /* @@ -278,7 +278,6 @@ public void run() { break; default: throw new AbstractPIDRuntimeException("Invalid state error"); - break; } } From 1ea1b5573871e13c046ef04ecd8c806bb2f40e59 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 02:41:49 +0000 Subject: [PATCH 35/47] fix more compile errors --- .../CanSparkMaxSmartMotionLinearPIDController.java | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java index 196714a8..f43950b7 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -1,19 +1,15 @@ +package com.team766.controllers; + public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { private final double antigravPower; - public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { + public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double antigravityPower) { super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); + antigravPower = antigravityPower; } - public void setAntigravPower(double power){ - antigravPower = power; - } - - public void setNewSetpoint(double setPoint){ - updateSetpointFromSubclass(setPoint); - } public void runPIDs(){ From 98dd22b4ed9679638d6c7b081e0b82b5bca35f38 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 02:44:48 +0000 Subject: [PATCH 36/47] fix spacing --- .../CanSparkMaxSmartMotionLinearPIDController.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java index f43950b7..51f91839 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -3,15 +3,12 @@ public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { private final double antigravPower; - - + public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double antigravityPower) { super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); antigravPower = antigravityPower; } - - public void runPIDs(){ updateAntigrav(antigravPower); run(); From 9bdb154f20e7a18068322730baa26362bbc7b7c1 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 15:34:39 +0000 Subject: [PATCH 37/47] Fix compile errors and add new exceptions and adresss some comments --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 4 ++++ .../AbstractPIDArithmeticDevideByZeroException.java | 7 +++++++ .../framework/Exceptions/AbstractPIDRuntimeException.java | 2 ++ 3 files changed, 13 insertions(+) create mode 100644 src/main/java/com/team766/framework/Exceptions/AbstractPIDArithmeticDevideByZeroException.java diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index fec471e2..7d4cb679 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -102,6 +102,10 @@ public ExtendableCanSparkMaxSmartMotionPIDController(final String configName, fi * @author Max Spier - 9/9/2023 */ private double absToEu(final double abs) { + + if(encoderUnitsPerOneAbsolute == 0){ + throw new AbstractPIDArithmeticDevideByZeroException("Error: encoderUnitsPerOneAbsolute is zero."); + } return encoderUnitsPerOneAbsolute * abs; } diff --git a/src/main/java/com/team766/framework/Exceptions/AbstractPIDArithmeticDevideByZeroException.java b/src/main/java/com/team766/framework/Exceptions/AbstractPIDArithmeticDevideByZeroException.java new file mode 100644 index 00000000..cceba0fb --- /dev/null +++ b/src/main/java/com/team766/framework/Exceptions/AbstractPIDArithmeticDevideByZeroException.java @@ -0,0 +1,7 @@ +package com.team766.framework.Exceptions; + +public class AbstractPIDArithmeticDevideByZeroException extends RuntimeException{ + public AbstractPIDArithmeticDevideByZeroException (String errorMessage) { + super(errorMessage); + } +} diff --git a/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java b/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java index f03499ab..188f52d7 100644 --- a/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java +++ b/src/main/java/com/team766/framework/Exceptions/AbstractPIDRuntimeException.java @@ -1,3 +1,5 @@ +package com.team766.framework.Exceptions; + public class AbstractPIDRuntimeException extends RuntimeException { public AbstractPIDRuntimeException (String errorMessage) { super(errorMessage); From 277e4bf716377eb9303d5ee1177d71de882257dc Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 15:36:29 +0000 Subject: [PATCH 38/47] fix static variables --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 7d4cb679..8dc8f718 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -40,11 +40,11 @@ public abstract class ExtendableCanSparkMaxSmartMotionPIDController { * @author Max Spier - 9/9/2023 */ private SparkMaxPIDController pid; - private static double deadzone = 0; - private static double setPointPosition = 0; - private static double comboOfTimesInsideDeadzone = 0; - private static double minPos = 0; - private static double maxPos = 0; + private double deadzone = 0; + private double setPointPosition = 0; + private double comboOfTimesInsideDeadzone = 0; + private double minPos = 0; + private double maxPos = 0; /* * These are some variables that we can use so that we can reset the motor controller From 89c5df7fcba386595997f76844907e6d9936158c Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Thu, 14 Sep 2023 15:40:38 +0000 Subject: [PATCH 39/47] Rename the variable to make it more simple --- .../CanSparkMaxSmartMotionLinearPIDController.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java index 51f91839..f47665bf 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -2,15 +2,15 @@ public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { - private final double antigravPower; + private final double constantAntigravPower; public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double antigravityPower) { super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); - antigravPower = antigravityPower; + constantAntigravPower = antigravityPower; } public void runPIDs(){ - updateAntigrav(antigravPower); + updateAntigrav(constantAntigravPower); run(); } From 2a26fceec906b3f8aef2e3face16918fb56bb9fa Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 15 Sep 2023 16:32:42 +0000 Subject: [PATCH 40/47] add override method --- src/main/java/com/team766/controllers/OffsetPoint.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/com/team766/controllers/OffsetPoint.java b/src/main/java/com/team766/controllers/OffsetPoint.java index 0392820c..2206cd35 100644 --- a/src/main/java/com/team766/controllers/OffsetPoint.java +++ b/src/main/java/com/team766/controllers/OffsetPoint.java @@ -20,4 +20,12 @@ public double getAbsoluteValue() { public double getMotorEncoderValue() { return motorValue; } + + /* + *@Override + *@author Max Spier - 9/15/2023 + */ + public String toString(){ + return "Absolute Value: " + absoluteValue + " Motor Value: " + motorValue; + } } From a01649767b55475c3590189ecc13e33997e71ae9 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 15 Sep 2023 16:34:48 +0000 Subject: [PATCH 41/47] java enum naming fix --- ...xtendableCanSparkMaxSmartMotionPIDController.java | 12 ++++++------ .../controllers/{PIDSTATE.java => PIDState.java} | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) rename src/main/java/com/team766/controllers/{PIDSTATE.java => PIDState.java} (72%) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 8dc8f718..b554644e 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -20,7 +20,7 @@ import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.library.RateLimiter; -import com.team766.controllers.PIDSTATE.*; +import com.team766.controllers.PIDState.*; import com.team766.framework.Exceptions.*; @@ -58,11 +58,11 @@ public abstract class ExtendableCanSparkMaxSmartMotionPIDController { /* * The state of the PID controller so that we know whether to call the respective method for each state. - * For more documentation, see the PIDSTATE class. + * For more documentation, see the PIDState class. * @author Max Spier - 9/9/2023 */ - private PIDSTATE theState = PIDSTATE.OFF; + private PIDState theState = PIDState.OFF; /* * Default constructor for the class. This is used to create a new CanSparkMaxSmartMotionPIDController object @@ -231,7 +231,7 @@ public void setNewSetpoint(double x){ * @author Max Spier - 9/9/2023 */ public void stop() { - theState = PIDSTATE.OFF; + theState = PIDState.OFF; } /* @@ -266,7 +266,7 @@ public void run() { if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { antigrav(); } else { - theState = PIDSTATE.PID; + theState = PIDState.PID; } case PID: if (setPointPosition <= (deadzone + mc.getSensorPosition()) && setPointPosition >= (mc.getSensorPosition() - deadzone)) { @@ -277,7 +277,7 @@ public void run() { } if (comboOfTimesInsideDeadzone >= 6) { - theState = PIDSTATE.ANTIGRAV; + theState = PIDState.ANTIGRAV; } break; default: diff --git a/src/main/java/com/team766/controllers/PIDSTATE.java b/src/main/java/com/team766/controllers/PIDState.java similarity index 72% rename from src/main/java/com/team766/controllers/PIDSTATE.java rename to src/main/java/com/team766/controllers/PIDState.java index a4eca012..f5eb442d 100644 --- a/src/main/java/com/team766/controllers/PIDSTATE.java +++ b/src/main/java/com/team766/controllers/PIDState.java @@ -1,6 +1,6 @@ package com.team766.controllers; - public enum PIDSTATE { + public enum PIDState { PID, OFF, ANTIGRAV From 65f3aa522f662102ff78e4ec8a9d9107ddd57f32 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 15 Sep 2023 16:48:44 +0000 Subject: [PATCH 42/47] removed newlines --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index b554644e..d1325c52 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -284,12 +284,4 @@ public void run() { throw new AbstractPIDRuntimeException("Invalid state error"); } } - - - - - - - - } From cb3c9486136cf3cbbf7be467f6b16e9fd9044a3c Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 15 Sep 2023 17:11:02 +0000 Subject: [PATCH 43/47] add another toString method --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index d1325c52..1470d8f9 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -284,4 +284,8 @@ public void run() { throw new AbstractPIDRuntimeException("Invalid state error"); } } + + public String toString(){ + return "Setpoint: " + setPointPosition + " Current Position: " + mc.getSensorPosition() + " Current State: " + theState; + } } From 885b37c5eeff86d4ec8f68b27cf92e56aa8fbb50 Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 27 Sep 2023 20:33:53 -0700 Subject: [PATCH 44/47] add default neutral mode for motors --- .../CanSparkMaxSmartMotionLinearPIDController.java | 4 ++-- .../CanSparkMaxSmartMotionRotationalPIDController.java | 4 ++-- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 6 +++++- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java index f47665bf..de50762b 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -4,8 +4,8 @@ public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSpar private final double constantAntigravPower; - public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double antigravityPower) { - super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); + public CanSparkMaxSmartMotionLinearPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final double antigravityPower, final NeutralMode defaultNeutralMode) { + super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second, defaultNeutralMode); constantAntigravPower = antigravityPower; } diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index 6abfffcb..f48ccb1f 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -7,8 +7,8 @@ public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCan private final double degreesToEncoderUnitsRatio; private final double antiGravK; - public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, double degToEncoderUnitsRatio, double antigravityCoefficent) { - super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second); + public CanSparkMaxSmartMotionRotationalPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, double degToEncoderUnitsRatio, double antigravityCoefficent, NeutralMode defaultNeutralMode) { + super(configName, absEncoderOffset, absEncoderOffsetForZeroEncoderUnits, first, second, defaultNeutralMode); antiGravK = antigravityCoefficent; degreesToEncoderUnitsRatio = degToEncoderUnitsRatio; } diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 1470d8f9..9801965a 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -54,6 +54,8 @@ public abstract class ExtendableCanSparkMaxSmartMotionPIDController { private double encoderUnitsPerOneAbsolute = 0; private double curAntigrav = 0; + + private NeutralMode defaultNeutralMode; /* @@ -75,8 +77,9 @@ public abstract class ExtendableCanSparkMaxSmartMotionPIDController { */ - public ExtendableCanSparkMaxSmartMotionPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second) { + public ExtendableCanSparkMaxSmartMotionPIDController(final String configName, final double absEncoderOffset, final double absEncoderOffsetForZeroEncoderUnits, final OffsetPoint first, final OffsetPoint second, final NeutralMode defaultNeutralMode) { try { + this.defaultNeutralMode = defaultNeutralMode; mc = RobotProvider.instance.getMotor(configName); csm = (CANSparkMax) mc; pid = csm.getPIDController(); @@ -92,6 +95,7 @@ public ExtendableCanSparkMaxSmartMotionPIDController(final String configName, fi mc.setSensorPosition(relEncoder); + mc.setNeutralMode(defaultNeutralMode); } catch (IllegalArgumentException ill) { throw new AbstractPIDRuntimeException("Error instantiating the PID controller: " + ill); } From 6456782edfca195f7327e345f7492e3002da5c1e Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 27 Sep 2023 20:36:10 -0700 Subject: [PATCH 45/47] i forgot imports --- .../controllers/CanSparkMaxSmartMotionLinearPIDController.java | 2 ++ .../CanSparkMaxSmartMotionRotationalPIDController.java | 1 + 2 files changed, 3 insertions(+) diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java index de50762b..4f107b9f 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionLinearPIDController.java @@ -1,5 +1,7 @@ package com.team766.controllers; +import com.ctre.phoenix.motorcontrol.NeutralMode; + public class CanSparkMaxSmartMotionLinearPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { private final double constantAntigravPower; diff --git a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java index f48ccb1f..5659cc64 100644 --- a/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java +++ b/src/main/java/com/team766/controllers/CanSparkMaxSmartMotionRotationalPIDController.java @@ -1,6 +1,7 @@ package com.team766.controllers; import java.util.*; +import com.ctre.phoenix.motorcontrol.NeutralMode; public class CanSparkMaxSmartMotionRotationalPIDController extends ExtendableCanSparkMaxSmartMotionPIDController { From 4724223a8237d85820d3079245a7026d1395b1d6 Mon Sep 17 00:00:00 2001 From: Max <> Date: Wed, 27 Sep 2023 20:41:34 -0700 Subject: [PATCH 46/47] added method to disable antigrav --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 9801965a..7d4e3e31 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -256,6 +256,11 @@ protected void updateAntigrav(double passedValueFromSubclass){ curAntigrav = passedValueFromSubclass; } + public void disableAntigrav(){ + curAntigrav = 0; + deadzone = 0; + } + /* * This is the run loop that actually runs the PID Controller * You need to call this as frequently as possible when running the mechanism From ad12708559b0e9f96aeda9c09fa9eea998c80011 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Mon, 2 Oct 2023 16:56:11 +0000 Subject: [PATCH 47/47] removed unused imports --- .../ExtendableCanSparkMaxSmartMotionPIDController.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java index 7d4e3e31..19b16e4e 100644 --- a/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java +++ b/src/main/java/com/team766/controllers/ExtendableCanSparkMaxSmartMotionPIDController.java @@ -2,24 +2,15 @@ import com.team766.config.ConfigFileReader; import com.team766.hal.RobotProvider; -import com.team766.library.SetValueProvider; -import com.team766.library.SettableValueProvider; -import com.team766.library.ValueProvider; -import com.team766.logging.Logger; -import com.team766.logging.LoggerExceptionUtils; import edu.wpi.first.math.MathUtil; import com.revrobotics.SparkMaxAbsoluteEncoder; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import java.io.IOError; -import javax.swing.text.DefaultStyledDocument.ElementSpec; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; -import com.team766.framework.Mechanism; import com.team766.hal.MotorController; -import com.team766.library.RateLimiter; import com.team766.controllers.PIDState.*; import com.team766.framework.Exceptions.*;