Skip to content

Commit

Permalink
"Please lint my branch" he said
Browse files Browse the repository at this point in the history 
so i tried
  • Loading branch information
@JayAgra
JayAgra committed Aug 18, 2023
1 parent 5bbdf08 commit 4de9cf4
Show file tree
Hide file tree
Showing 3 changed files with 199 additions and 179 deletions.
189 changes: 99 additions & 90 deletions src/main/java/com/team766/controllers/CanSparkMaxSmartMotionNonRotationalPIDController.java
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

package com.team766.controllers;

import com.team766.config.ConfigFileReader;
Expand All @@ -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;
Expand All @@ -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;
}
}
}
Loading

0 comments on commit 4de9cf4

Please sign in to comment.