Hello, I am the lead programmer from team 5142. I am attempting to use position control in combination with PIDControllers to have my Elevator, Arm Swivel, and Arm Telescope go to set positions. However, the program simply runs the first if statement in my command, which tells it to go to a single position only. Does anybody have any advice on how to make this work? My code is posted below, as well as the Github link. If you have already solved a similar scenario, please link your code, it would be a massive help!
Placement Subsystem -
import edu.wpi.first.math.controller.PIDController;
import frc.utils.DmntrPIDController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import edu.wpi.first.wpilibj.Encoder;
import com.revrobotics.CANSparkMax;
import com.playingwithfusion.CANVenom;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
import com.revrobotics.CANSparkMaxLowLevel;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Placement extends SubsystemBase {
/** Creates a new Placement. */
public static CANSparkMax swivMotor = new CANSparkMax(Constants.CAN_IDs.SwivelMotorID, MotorType.kBrushless);
private final RelativeEncoder swivEncoder = swivMotor.getEncoder();
public TalonSRX teleMotor = new TalonSRX(Constants.CAN_IDs.TelescopeMotorID);
Encoder MagEncoder; // Telescope motor encoder
double dist = 0.5*3.14/1024; // ft per pulse (Not really, still needs some tuning)
public static CANVenom elevMotor = new CANVenom(Constants.CAN_IDs.elevatorMotorID);
double swivGearRatio = 0.01451614776; // The gear ratio for the swivel motor (68.88:1)
double teleGearRatio = 0.015625; // The gear ratio for the telescope motor (64:1)
double elevGearRatio = 0.04090916528; // The gear ratio for the elevator motor (24.44:1)
public double swivState = 0;
public double teleState = 0;
public double elevState = 0;
public DmntrPIDController swivController = new DmntrPIDController(0.001, 0, 0, 0.01, () -> getDegreesSwivel(),
0,0);
// public DmntrPIDController teleController = new DmntrPIDController(0,001, 0.0, 0, 0.01, () -> getDistanceTelescope(),
// 0, 0);
public DmntrPIDController elevController = new DmntrPIDController(0.001, 0, 0, 0.01, () -> getDistanceElevator(),
0, 0);
/** Creates a new Placement. */
public Placement() {
setBrakeArm();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
swivController.setSetpoint(swivState);
// teleController.setSetpoint(teleState);
elevController.setSetpoint(elevState);
if(elevState == Constants.PlacementSetpoints.resetElev){
// teleController.innerController.setP(0.01);
elevController.innerController.setP(0.007);
}
else if(elevState != Constants.PlacementSetpoints.resetElev){
// teleController.innerController.setP(0.01);
elevController.innerController.setP(0.008);
}
swivMotor.set(swivController.getEffort());
// teleMotor.set(TalonSRXControlMode.PercentOutput, teleController.getEffort());
elevMotor.set(elevController.getEffort());
// }
SmartDashboard.putNumber("SwivelDistance", getDegreesSwivel());
// SmartDashboard.putNumber("TelescopeDistance", getDistanceTelescope());
SmartDashboard.putNumber("ElevatorDistance", getDistanceElevator());
SmartDashboard.putNumber("SwivelSpeed", getDegreesSwivel());
// SmartDashboard.putNumber("TelescopeSpeed", getDistanceTelescope());
SmartDashboard.putNumber("ElevatorSpeed", getDistanceElevator());
SmartDashboard.putNumber("swivState", swivState);
SmartDashboard.putNumber("teleState", teleState);
SmartDashboard.putNumber("elevState", elevState);
SmartDashboard.getBoolean("Has Reached Target", hasReachedTarget());
}
public static void moveSwivel(double swivSpeed) {
swivMotor.set(swivSpeed);
}
// public void moveTelescope(double teleSpeed) {
// teleMotor.set(TalonSRXControlMode.PercentOutput, teleSpeed);
// }
public static void moveElevator(double elevSpeed) {
elevMotor.set(elevSpeed);
}
// public double getDistanceTelescope() {
// return (MagEncoder.getDistance() * teleGearRatio);
// }
public double getDegreesSwivel() {
return (swivMotor.getEncoder().getPosition());
}
public double getDistanceElevator(){
return (elevMotor.getPosition());
}
public void resetSetpoints(){
swivState = 0;
teleState = 0;
elevState = 0;
}
public boolean hasReachedTarget() {
return
// (Math.abs(getDistanceTelescope() - teleController.getSetpoint()) < 12
(Math.abs(getDegreesSwivel() - swivController.getSetpoint()) < 1
&& Math.abs(getDistanceElevator() - elevController.getSetpoint()) < 1);
}
public void resetEncoders() {
swivMotor.getEncoder().setPosition(0);
//MagEncoder.reset();
elevMotor.resetPosition();
}
public void setBrakeArm(){
// shoulderMotor.setIdleMode(IdleMode.kBrake);
// forearmMotor.setIdleMode(IdleMode.kBrake);
// wristMotor.setIdleMode(IdleMode.kBrake);
}
public void setCoastArm(){
// shoulderMotor.setIdleMode(IdleMode.kCoast);
// forearmMotor.setIdleMode(IdleMode.kCoast);
// wristMotor.setIdleMode(IdleMode.kCoast);
}
}
Placement Command -
import com.revrobotics.CANSparkMax.IdleMode;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.subsystems.Placement;
import frc.robot.RobotContainer.Subsystems;
public class PlacementCommand extends CommandBase {
/** Creates a new ArmCommand. */
public PlacementCommand() {
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
Placement.moveSwivel(0);
// Subsystems.Placement.moveTelescope(0);
Placement.moveElevator(0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (RobotContainer.operator.getAButton()) { // Home Position
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.resetTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.resetElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.resetSwiv;
}
else if(RobotContainer.operator.getXButton()){ // Ground collection / Dump Low
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.floorTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.floorElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.floorSwiv;
}
else if(RobotContainer.operator.getRightBumper()){ // High Cone/Cube
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.sSubTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.highElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.highSwiv;
}
else if (RobotContainer.operator.getRightTriggerAxis() > 0.1){ // Mid Cone/Cube
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.sSubTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.midElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.midSwiv;
}
else if(RobotContainer.operator.getLeftTriggerAxis() > 0.1){ // Single Substation
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.sSubTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.sSubElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.sSubSwiv;
}
else if(RobotContainer.operator.getLeftBumper()){ // Double SubStation
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.dSubTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.dSubElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.dSubSwiv;
}
else{
// Subsystems.Placement.teleState = Constants.PlacementSetpoints.resetTele;
Subsystems.m_placement.elevState = Constants.PlacementSetpoints.resetElev;
Subsystems.m_placement.swivState = Constants.PlacementSetpoints.resetSwiv;
}
} // end isAutomated
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
DmntrPIDController Utility
import java.util.function.Supplier;
import edu.wpi.first.math.controller.PIDController;
public class DmntrPIDController {
public PIDController innerController;
private double setpoint;
private Supplier<Double> measurementSupplier;
private double minEffort;
public PIDController getInnerController() {
return this.innerController;
}
public void setInnerController(PIDController innerController) {
this.innerController = innerController;
}
public void setPositionTolerance(double tolerance) {
innerController.setTolerance(tolerance);
}
public void setTolerance(double tolerancePos, double toleranceVel) {
innerController.setTolerance(tolerancePos, toleranceVel);
}
public double getSetpoint() {
return this.setpoint;
}
public void setSetpoint(double setpoint) {
this.setpoint = setpoint;
}
public Supplier<Double> getMeasurementSupplier() {
return this.measurementSupplier;
}
public void setMeasurementSupplier(Supplier<Double> measurementSupplier) {
this.measurementSupplier = measurementSupplier;
}
public double getMinEffort() {
return this.minEffort;
}
public void setMinEffort(double minEffort) {
this.minEffort = minEffort;
}
/**
*
* @param p Proportional gain
* @param i Integral gain
* @param d Derivative gain
* @param minEffort Minimum effort to be applied to motor
* @param measurement Supplier of reading from feedback device (ex: encoder value)
* @param initialSetpoint Goal measurement reading, the controller aims to make current measurement reach this value
* @param positionTolerance Absolute acceptable positional error (velocity tolerance may also be set)
*/
public DmntrPIDController(double p, double i, double d, double minEffort, Supplier<Double> measurement,
double initialSetpoint, double positionTolerance) {
innerController = new PIDController(p,i,d);
this.setpoint = initialSetpoint;
this.measurementSupplier = measurement;
this.minEffort = minEffort;
innerController.setTolerance(positionTolerance);
}
public double getEffort() {
double mez = measurementSupplier.get();
double effort = innerController.calculate(mez, setpoint);
if(!isFinished()) {
return (minEffort * (-Math.signum(mez) )) + effort;
} else {
return 0;
}
}
public boolean isFinished() {
return innerController.atSetpoint();
}
}
RobotContainer
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.POVButton;
import frc.robot.commands.AutoDriveForward;
import frc.robot.commands.ClawIn;
import frc.robot.commands.ClawOut;
import frc.robot.commands.PlacementCommand;
import frc.robot.commands.ToggleButterfly;
import frc.robot.commands.ToggleClaw;
import frc.robot.subsystems.Claw;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Placement;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and trigger mappings) should be declared here.
*/
public class RobotContainer {
public final Drivetrain m_drive = new Drivetrain();
public final Claw m_claw = new Claw();
public static class Subsystems{
public final static Placement m_placement = new Placement();
}
private static Joystick driver = new Joystick(0);
private static Joystick driver2 = new Joystick(1);
public static XboxController operator = new XboxController(2);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
m_drive.setDefaultCommand(
new RunCommand(
() -> m_drive.drive(driver.getRawAxis(1), driver.getRawAxis(0), driver2.getRawAxis(0)),
m_drive));
}
private void configureBindings() {
// Driver Buttons
final JoystickButton driverRightThumb = new JoystickButton(driver2, 2);
// Operator Buttons
// final JoystickButton triggerButton = new JoystickButton(operator, 1); // Place Dump Low
// final JoystickButton thumbButton = new JoystickButton(operator, 2); // Toggle Claw
// final JoystickButton button3 = new JoystickButton(operator, 3); // Claw In
// final JoystickButton button4 = new JoystickButton(operator, 4); // Claw Out
// final JoystickButton button5 = new JoystickButton(operator, 5); // Grab Ground Position
// final JoystickButton button6 = new JoystickButton(operator, 6); // Home Position
// final JoystickButton button7 = new JoystickButton(operator, 7); // Place High Cone
// final JoystickButton button8 = new JoystickButton(operator, 8); // Place High Cube
// final JoystickButton button9 = new JoystickButton(operator, 9); // Place Mid Cone
// final JoystickButton button10 = new JoystickButton(operator, 10); // Place Mid Cube
// final JoystickButton button11 = new JoystickButton(operator, 11); // Single Substation Position
// final JoystickButton button12 = new JoystickButton(operator, 12); // Double Substation Position
final JoystickButton aButton = new JoystickButton(operator, 1); // Home Posiiton
// final JoystickButton bButton = new JoystickButton(operator, 2); // TBD
final JoystickButton xButton = new JoystickButton(operator, 3); // Claw Solenoid
final JoystickButton yButton = new JoystickButton(operator, 4); // Ground Position
final JoystickButton leftBut = new JoystickButton(operator, 5); // Double Substation
final JoystickButton rightBut = new JoystickButton(operator, 6); // High Cone/Cube
final JoystickButton leftTrig = new JoystickButton(operator, 7); // Single Substation
final JoystickButton rightTrig = new JoystickButton(operator, 8); // Mid Cone/Cube
final POVButton dpadUp = new POVButton(operator, 0); // Claw Out
final POVButton dpadDown = new POVButton(operator, 180); // Claw In
// Driver Command Bindings
driverRightThumb.onTrue(new ToggleButterfly(m_drive));
// Operator Command Bindings
xButton.onTrue(new ToggleClaw(m_claw)); // Toggles Claw Pistons (Horizontal/Perpendicular)
dpadDown.whileTrue(new ClawIn(m_claw)); // Runs Claw motor forward (Brings pieces in)
dpadUp.whileTrue(new ClawOut(m_claw)); // Runs Claw motor backward (Spits pieces out)
}
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return new AutoDriveForward(m_drive);
}
}