in the midst of making controller based pid control
This commit is contained in:
Tylr-J42
parent
68da3c630c
commit
42d47d6075
@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Second;
|
||||
import static edu.wpi.first.units.Units.Seconds;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
@ -22,6 +23,7 @@ public class ElevatorConstants {
|
||||
|
||||
public static final int kCurrentLimit = 40;
|
||||
|
||||
/*
|
||||
public static final double kUpControllerP = 5.6;//7; //
|
||||
public static final double kUpControllerI = 0;
|
||||
public static final double kUpControllerD = 0.28;//0.28
|
||||
@ -31,15 +33,20 @@ public class ElevatorConstants {
|
||||
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
|
||||
|
||||
public static final double kMaintainP = 3;
|
||||
|
||||
*/
|
||||
|
||||
public static final double kP = 5.6;//7; //
|
||||
public static final double kI = 0;
|
||||
public static final double kD = 0.28;//0.28
|
||||
|
||||
public static final double kAllowedError = 1;
|
||||
|
||||
public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */
|
||||
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
|
||||
public static final double kFeedForwardV = 0.12; // calculated value 0.12
|
||||
|
||||
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
public static final double kMaxVelocity = 100.0; // 120 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 50; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
|
||||
public static final double kCoralIntakePosition = 0;
|
||||
public static final double kL1Position = 17;
|
||||
@ -83,5 +90,9 @@ public class ElevatorConstants {
|
||||
motorConfig.encoder
|
||||
.positionConversionFactor(kEncoderPositionConversionFactor)
|
||||
.velocityConversionFactor(kEncoderVelocityConversionFactor);
|
||||
motorConfig.closedLoop
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.pid(kP, kI, kD)
|
||||
.outputRange(-1, 1);
|
||||
}
|
||||
}
|
||||
|
||||
@ -13,6 +13,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.ElevatorFeedforward;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@ -26,12 +28,16 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
private PIDController pidController;
|
||||
|
||||
private PIDController maintainPID;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
|
||||
private TrapezoidProfile trapProfile;
|
||||
private TrapezoidProfile.State goal;
|
||||
private TrapezoidProfile.State state;
|
||||
private TrapezoidProfile.Constraints elevatorConstraints;
|
||||
|
||||
public Elevator() {
|
||||
elevatorMotor1 = new SparkMax(
|
||||
@ -62,33 +68,13 @@ public class Elevator extends SubsystemBase {
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidControllerDown = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
|
||||
|
||||
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
ElevatorConstants.kFeedForwardG,
|
||||
ElevatorConstants.kFeedForwardV
|
||||
);
|
||||
|
||||
trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(elevatorConstraints.maxVelocity, ElevatorConstants.kMaxAcceleration));
|
||||
}
|
||||
|
||||
@Override
|
||||
@ -98,8 +84,6 @@ public class Elevator extends SubsystemBase {
|
||||
}
|
||||
|
||||
Logger.recordOutput("elevator position", getEncoderPosition());
|
||||
Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
|
||||
Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
|
||||
}
|
||||
|
||||
/**
|
||||
@ -153,21 +137,10 @@ public class Elevator extends SubsystemBase {
|
||||
public Command maintainPosition() {
|
||||
|
||||
return startRun(() -> {
|
||||
maintainPID.reset();
|
||||
maintainPID.setSetpoint(pidControllerUp.getSetpoint());
|
||||
|
||||
},
|
||||
() -> {
|
||||
|
||||
double maintainOutput = maintainPID.calculate(getEncoderPosition());
|
||||
|
||||
if(!maintainPID.atSetpoint())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
maintainOutput + feedForward.calculate(0), -2, 2)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -192,123 +165,26 @@ public class Elevator extends SubsystemBase {
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
*
|
||||
* @param setpoint Target destination of the subsystem
|
||||
* @param timeout Time to achieve the setpoint before quitting
|
||||
* @return Sets motor voltage to achieve the target destination
|
||||
*/
|
||||
public Command goToSetpoint(DoubleSupplier setpoint) {
|
||||
public Command goToSetpoint(DoubleSupplier setGoal) {
|
||||
|
||||
if (setpoint.getAsDouble() == 0) {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.5)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
return startRun(() -> {
|
||||
goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
|
||||
}, () -> {
|
||||
TrapezoidProfile.State setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
|
||||
|
||||
|
||||
}).until();
|
||||
|
||||
} else {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
|
||||
}
|
||||
}
|
||||
|
||||
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
|
||||
|
||||
if (setpoint.getAsDouble() == 0) {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.5)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
|
||||
} else {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
|
||||
);
|
||||
}
|
||||
return startRun(() -> {
|
||||
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
|
||||
}
|
||||
}).until();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -349,11 +225,11 @@ public class Elevator extends SubsystemBase {
|
||||
}
|
||||
|
||||
public double getPIDUpSetpoint() {
|
||||
return pidControllerUp.getSetpoint();
|
||||
return pidController.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDUpError() {
|
||||
return pidControllerUp.getError();
|
||||
return pidController.getError();
|
||||
}
|
||||
|
||||
public double getPIDDownSetpoint() {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user