How do I control a Mechanism without a Mechanism Class?
Sometimes you have "loosely mechanically coupled" design which requires you to control both motors independently instead of using one as a follower.
Subsystems with SmartMotorController's
SmartMotorController'sControlling an Elevator without Elevator.setHeight()
public class ExampleSubsystem extends SubsystemBase {
// Chain driven elevator drum calculation
private Distance chainPitch = Inches.of(0.25);
private int teethCount = 22;
private Distance drumCircumference = chainPitch.times(teethCount);
private Distance drumRadius = drumCircumference.div(2*Math.PI);
// Gearing from the motor rotor to final shaft.
// In this example GearBox.fromReductionStages(3,4) is the same as GearBox.fromStages("3:1","4:1") which corresponds to the gearbox attached to your motor.
private MechanismGearing gearing = new MechanismGearing(GearBox.fromReductionStages(3,4));
// Profiled PID Controller
private double kP = 4, kI = 0, kD = 0;
private LinearVelocity maxVel = MetersPerSecond.of(0.5);
private LinearAcceleration maxAccel = MetersPerSecondPerSecond.of(0.5);
// Feedforwards
private ElevatorFeedforward realFeedforward = new ElevatorFeedforward(0,0,0);
private ElevatorFeedforward simFeedforward = new ElevatorFeedforward(0,0,0);
private MotorMode idle = MotorMode.BRAKE;
private Current statorLimit = Amps.of(40);
// Ramp rate is how long the motor should take to run from 0 to 100%
private Time openLoopRampRate = Seconds.of(0.25);
private Time closedLoopRampRate = Seconds.of(0.25);
private SmartMotorControllerConfig leftSmcConfig = new SmartMotorControllerConfig(this)
.withControlMode(ControlMode.CLOSED_LOOP)
// Mechanism Circumference is the distance traveled by each mechanism rotation converting rotations to meters.
.withMechanismCircumference(drumCircumference)
// Feedback Constants (PID Constants)
.withClosedLoopController(kP, kI, kD, maxVel, maxAccel)
// Uncomment below to set sim only values.
//.withSimClosedLoopController(4, 0, 0, MetersPerSecond.of(0.5), MetersPerSecondPerSecond.of(0.5))
// Feedforward Constants
.withFeedforward(realFeedforward)
.withSimFeedforward(simFeedforward)
// Telemetry name and verbosity level
.withTelemetry("LeftElevatorMotor", TelemetryVerbosity.HIGH)
// Gearing from the motor rotor to final shaft.
// In this example gearbox(3,4) is the same as gearbox("3:1","4:1") which corresponds to the gearbox attached to your motor.
.withGearing(gearing)
// Motor properties to prevent over currenting.
.withMotorInverted(false)
.withIdleMode(idle)
.withStatorCurrentLimit(statorLimit)
.withClosedLoopRampRate(closedLoopRampRate)
.withOpenLoopRampRate(openLoopRampRate);
private SmartMotorControllerConfig rightSmcConfig = new SmartMotorControllerConfig(this)
.withControlMode(ControlMode.CLOSED_LOOP)
// Mechanism Circumference is the distance traveled by each mechanism rotation converting rotations to meters.
.withMechanismCircumference(drumCircumference)
// Feedback Constants (PID Constants)
.withClosedLoopController(kP, kI, kD, maxVel, maxAccel)
// Uncomment below to set sim only values.
//.withSimClosedLoopController(4, 0, 0, MetersPerSecond.of(0.5), MetersPerSecondPerSecond.of(0.5))
// Feedforward Constants
.withFeedforward(realFeedforward)
.withSimFeedforward(simFeedforward)
// Telemetry name and verbosity level
.withTelemetry("RightElevatorMotor", TelemetryVerbosity.HIGH)
// Gearing from the motor rotor to final shaft.
// In this example gearbox(3,4) is the same as gearbox("3:1","4:1") which corresponds to the gearbox attached to your motor.
.withGearing(gearing)
// Motor properties to prevent over currenting.
.withMotorInverted(false)
.withIdleMode(idle)
.withStatorCurrentLimit(statorLimit)
.withClosedLoopRampRate(closedLoopRampRate)
.withOpenLoopRampRate(openLoopRampRate);
// Vendor motor controller object
private SparkMax leftSpark = new SparkMax(4, MotorType.kBrushless);
private SparkMax rightSpark = new SparkMax(6, MotorType.kBrushless);
// Create our SmartMotorController from our Spark and config with the NEO.
private SmartMotorController leftSparkSmartMotorController = new SparkWrapper(leftSpark, DCMotor.getNEO(1), leftSmcConfig);
private SmartMotorController rightSparkSmartMotorController = new SparkWrapper(rightSpark, DCMotor.getNEO(1), rightSmcConfig);
// Include this only if you want a pretty sim
/*
private ElevatorConfig elevconfig = new ElevatorConfig(sparkSmartMotorController)
.withStartingHeight(Meters.of(0.5))
.withHardLimits(Meters.of(0), Meters.of(3))
.withTelemetry("Elevator", TelemetryVerbosity.HIGH)
.withMass(Pounds.of(16));
// Elevator Mechanism
private Elevator elevator = new Elevator(elevconfig);
*/
/**
* Set the dutycycle of the SmartMotorController without interrupting
*
* @param dutyCycle Value between [-1, 1] which will control the speed of the motor.
* @return StartRun command which turns off the closed loop controller, sets the dutycycle of the SMC then turns the closed loop controller back on when interrupted.
*/
public Command setDutyCycle(double dutyCycle)
{
return startRun(()->{leftSparkSmartMotorController.stopClosedLoopController();rightSparkSmartMotorController.stopClosedLoopController();}, // Stop the closed loop controller since the motor is in ControlMode.CLOSED_LOOP
() -> {leftSparkSmartMotorController.setDutyCycle(dutycycle);rightSparkSmartMotorController.setDutyCycle(dutycycle);}) // Apply the dutycycle given
.finallyDo(()->{leftSparkSmartMotorController.startClosedLoopController();rightSparkSmartMotorController.startClosedLoopController();}) // Start the closed loop controller when this command is interrupted
.withName("Custom SetDutyCycle"); // Be nice, give your command name :)
}
/**
* Set the position of the spark motor controller.
* @param position Mechanism position to go to.
* @return RunCommand which will set the position of the mechanism until interrupted.
*/
public Command setMechanismPosition(Angle position)
{
return run(() -> {leftSparkSmartMotorController.setPosition(position);rightSparkSmartMotorController.setPosition(position);})
.withName("Custom SetPosition");
}
/**
* Set the position of the spark motor controller.
* @param position Measurement position to go to.
* @return RunCommand which will set the position of the measurement until interrupted.
*/
public Command setMechanismPosition(Distance height)
{
return run(() -> {leftSparkSmartMotorController.setPosition(position);rightSparkSmartMotorController.setPosition(position);})
.withName("Custom SetPosition with Height");
}
@Override
public void periodic() {
// This method will be called once per scheduler run
leftSparkSmartMotorController.updateTelemetry();
rightSparkSmartMotorController.updateTelemetry();
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
leftSparkSmartMotorController.simIterate();
rightSparkSmartMotorController.simIterate();
}
}Last updated