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] "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; } }