Hey, this is Mohamed from Team 5142. I have successfully implemented a ProfiledPIDController on my robot’s Arm Swivel, Telescope, and Elevator. However, as soon as I press enable for teleoperated mode, the robot shoots (And I mean shoots, the speed is crazy, it almost broke the arm) to the set position. I have to figure out how to slow it down as well as disable the autofiring on initialization (We want the set positions to correspond to buttons on a joystick, which I have written, but it doesn’t do so). All code pertaining to the problem is noted below; any help would be wholly appreciated.
The instantCommand that uses all three subsystems
public class GrabGround extends InstantCommand {
private final Swivel swivel;
private final Telescope telescope;
private final Elevator elevator;
private final double swivTargetPosition;
private final double teleTargetPosition;
private final double elevTargetPosition;
public GrabGround(Swivel swivel, double swivTargetPosition, Telescope telescope, double teleTargetPosition, Elevator elevator, double elevTargetPosition) {
this.swivel = swivel;
this.swivTargetPosition = swivTargetPosition;
this.telescope = telescope;
this.teleTargetPosition = teleTargetPosition;
this.elevator = elevator;
this.elevTargetPosition = elevTargetPosition;
addRequirements(swivel);
addRequirements(telescope);
addRequirements(elevator);
}
@Override
public void initialize() {
swivel.setTargetPosition(swivTargetPosition);
telescope.setTargetPosition(teleTargetPosition);
elevator.setTargetPosition(elevTargetPosition);
}
}
The ArmSwivel Subsystem
public class Swivel extends SubsystemBase {
/** Creates a new Swivel. */
// All reference angles (For placement/collection)
private double groundSwivelAngle = 30; // Swivel angle for ground collection
private double singleSubSwivelAngle = 180; // Swivel angle for single substation collection
private double straightSwivelAngle = 90; // Swivel angle for double substation collection
// The TalonSRX motor controller that controls the swivel motion.
private final CANSparkMax m_swivel;
private final RelativeEncoder m_encoder;
private final ProfiledPIDController m_controller;
// Target position for the motor
public double m_targetPosition;
private static final double kP = 0.1;
private static final double kI = 0;
private static final double kD = 0;
private static final double kMaxVelocity = (.01);
private static final double kMaxAcceleration = (.01);
public Swivel() {
m_swivel = new CANSparkMax(7, MotorType.kBrushless); // Swivel Motor CAN ID
m_encoder = m_swivel.getEncoder();
m_swivel.setIdleMode(IdleMode.kBrake); // Initialize the motor as braked
m_controller = new ProfiledPIDController(kP, kI, kD,
new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration)); // Profiled PID Control (Requires more testing)
}
public void swivelUp() {
m_swivel.set(0.2); // Sets motor speed to +20%
}
public void swivelDown() {
m_swivel.set(-0.2); // Sets motor speed to -40%
}
public void stop() {
m_swivel.set(0); // Sets motor speed to 0
}
public void swivelUpPower(double power) {
m_swivel.set(power); // Sets a constant holding power (To resist gravity)
}
// Set the output of the NEO motor using the output of the ProfiledPIDController
public void setOutput(double output) {
m_swivel.set(output);
}
// Get the current position of the NEO motor using the built-in encoder
public double getEncoder() {
return m_encoder.getPosition();
}
// Calculate the output of the ProfiledPIDController based on the current position
public double calculate() {
return m_controller.calculate(getEncoder());
}
// Set a target position for the NEO motor using the ProfiledPIDController
public void setTargetPosition(double swivTargetPosition) {
m_controller.setGoal(swivTargetPosition);
}
@Override
public void periodic() {
SmartDashboard.putNumber("SwivelEnc", getEncoder()); // For testing
// This method will be called once per scheduler run
setOutput(calculate());
}
}
The ArmTelescope SubSystem
public class Telescope extends SubsystemBase {
/** Creates a new Telescope. */
// All reference points (For placement/collection)
private double shortTelescopeLength = 0; // Telescope length for short setpoint
private double midTelescopeLength = 10; // Telescope length for mid setpoint
private double longTelescopeLength = 20; // Telescope length for long setpoint
// Declare class variables
private final TalonSRX m_telescope; // TalonSRX motor controller for the telescope arm
private final Encoder m_encoder; // Encoder for the RedLine motor
private final ProfiledPIDController m_controller; // PID controller for position control
public double m_targetPosition; // Target position for the motor
double dist = 0.5 * 3.14 / 1024; // ft per pulse
// Constants for PID and motion profiling
private static final double kP = 0.1;
private static final double kI = 0;
private static final double kD = 0;
private static final double kMaxVelocity = (.01);
private static final double kMaxAcceleration = (.01);
public Telescope() {
m_telescope = new TalonSRX(8); // Telescoping motor CAN ID
TalonSRXConfiguration configuration = new TalonSRXConfiguration(); // Man idk what this does
m_encoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
m_encoder.setDistancePerPulse(dist);
m_telescope.setNeutralMode(NeutralMode.Brake); // Initialize the motor as braked
m_telescope.configAllSettings(configuration); // Man idk what this does
// Configure the ProfiledPIDController with PID constants and constraints
m_controller = new ProfiledPIDController(kP, kI, kD,
new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration));
}
public void TelescopeOut() {
m_telescope.set(ControlMode.PercentOutput, 0.5); // Sets motor output to +50%
}
public void TelescopeIn() {
m_telescope.set(ControlMode.PercentOutput, -0.5); // Sets motor output to -50%
}
public void stop() {
m_telescope.set(ControlMode.Position, 0); // Sets motor output to 0
}
// Set a target position for the Talon SRX motor using the ProfiledPIDController
public void setTargetPosition(double teleTargetPosition) {
m_controller.setGoal(teleTargetPosition);
}
// Get the current position of the Talon SRX motor using the MagEncoder
public double getCurrentPosition() {
return m_telescope.getSelectedSensorPosition();
}
// Calculate the output of the ProfiledPIDController based on the current position
public double calculate() {
return m_controller.calculate(getCurrentPosition());
}
// Set the output of the Talon SRX motor using the output of the ProfiledPIDController
public void setOutput(double output) {
m_telescope.set(ControlMode.PercentOutput, output);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Telescope Encoder", m_encoder.get());
// Update the motor output with the calculated output from the ProfiledPIDController
setOutput(calculate());
}
}
The Arm Elevator Subsystem
public class Elevator extends SubsystemBase {
/** Creates a new Elevator. */
// All reference angles (For placement/collection)
private double lowElevPosition = 0; // Position for ground collect, single substation, and dump low
private double midElevPosition = 50; // Position for Mid cone / cube
private double highElevPosition = 100; // Position for High cone / cube, and double substation
// Define the elevator motor, Profiled PID controller, and Limit Switch
private CANVenom m_elevator;
private ProfiledPIDController m_controller;
// private DigitalInput m_limit;
private static final double kP = 0.1;
private static final double kI = 0;
private static final double kD = 0;
private static final double kMaxVelocity = (.01);
private static final double kMaxAcceleration = (.01);
// Constructor for the elevator subsystem
public Elevator() {
m_elevator = new CANVenom(4);
m_elevator.setBrakeCoastMode(BrakeCoastMode.Brake);
m_elevator.setControlMode(ControlMode.PositionControl);
// m_limit = new DigitalInput(1);
m_controller = new ProfiledPIDController(kP, kI, kD,
new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration)); // Profiled PID Control (Requires more testing)
}
public void ElevatorUp() {
m_elevator.set(0.2); // Sets motor speed to +20%
}
public void elevatorDown() {
m_elevator.set(-0.2); // Sets motor speed to -40%
}
// Method to stop the elevator
public void stop() {
m_elevator.set(0);
}
public void elevatorUpPower(double ePower) {
m_elevator.set(ePower); // Sets a constant holding power (To resist gravity)
}
public void setSpeed(double speed) {
m_elevator.set(speed);
}
// Set the output of the NEO motor using the output of the ProfiledPIDController
public void setOutput(double output) {
m_elevator.set(output);
}
// Get the current position of the NEO motor using the built-in encoder
public double getEncoder() {
return m_elevator.getPosition();
}
// Calculate the output of the ProfiledPIDController based on the current position
public double calculate() {
return m_controller.calculate(getEncoder());
}
// Set a target position for the NEO motor using the ProfiledPIDController
public void setTargetPosition(double elevTargetPosition) {
m_controller.setGoal(elevTargetPosition);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("ElevatorEnc", getEncoder());
// SmartDashboard.putBoolean("ElevatorLimt", m_limit.get());
setOutput(calculate());
}
}
The RobotContainer
public class RobotContainer {
private final Drivetrain m_drive = new Drivetrain();
private final Elevator elevator = new Elevator();
private final Swivel swivel = new Swivel();
private final Telescope telescope = new Telescope();
private final Claw m_claw = new Claw();
private static Joystick driver = new Joystick(0);
private static Joystick driver2 = new Joystick(1);
public static Joystick operator = new Joystick(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 POVButton povPadUp = new POVButton(operator, 0); // Swivel Up
// final POVButton povPadDown = new POVButton(operator, 180); // Swivel Down
// final POVButton povPadRight = new POVButton(operator, 90); // Telescope Out
// final POVButton povPadLeft = new POVButton(operator, 270); // Telescope In
// Driver Command Bindings
driverRightThumb.onTrue(new ToggleButterfly(m_drive));
// Operator Command Bindings
// triggerButton.onTrue
thumbButton.onTrue(new ToggleClaw(m_claw));
button3.whileTrue(new ClawIn(m_claw));
button4.whileTrue(new ClawOut(m_claw));
button5.onTrue(new GrabGround(swivel, 100, telescope, 100, elevator, 100));
// button6.onTrue(new)
// button7.onTrue(new)
// button8.onTrue(new)
// button9.onTrue(new)
// button10.onTrue(new)
// button11.onTrue(new GrabSubSingle(swivel, 100, telescope, 100, elevator, 100));
// button12.onTrue(new GrabSubDouble(swivel, 100, telescope, 100, elevator, 100));
// povPadRight.whileTrue(new TelescopeOut(m_tele));
// povPadUp.whileTrue(new SwivelUp(m_swivel));
// povPadDown.whileTrue(new SwivelDown(m_swivel));
// povPadLeft.whileTrue(new TelescopeIn(m_tele));
}
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return new AutoDriveForward(m_drive);
}
}