Being fairly new to programming, I am not really on the “bleeding edge” and got stumped by this one thing. How would you get a motor connected to a Talon SRX, and the encoder (being a magnetic encoder) also connected to that Talon SRX, to run to a set encoder value and stop?
Currently my code looks like this:
package frc.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj.command.Subsystem;
import frc.robot.commands.LiftDrive;
public class Lift extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
TalonSRX liftMotor;
double liftMin; double liftMax; // Minimum and maximum lift encoder values
double liftHoleBottom; double liftHoleMiddle; double liftHoleTop; // Encoder heights for rocket holes
public boolean allowSnap; // Override boolean to cancel or prohibit the gotoPosition(); function
public Lift() {
liftMotor = new TalonSRX(5);
liftMin = 0; liftMax = 1000;
liftHoleBottom = 250; liftHoleMiddle = 500; liftHoleTop = 750;
}
private double deadBand(double power) {
if(Math.abs(power) < .1) {
power = 0;
}
return power;
}
public void setPower(double motorPower) {
motorPower = deadBand(motorPower);
// See if were in between the limits
if (motorPower>0 && liftMotor.getSelectedSensorPosition()>=liftMax) {
motorPower = 0;
}
if (motorPower<0 && liftMotor.getSelectedSensorPosition()<=liftMin) {
motorPower = 0;
}
liftMotor.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, motorPower);
}
public void gotoPosition(double position){
if (allowSnap) {
position = Math.round(position/25);
if (position<getSelectedSensorPositionRounded()) {
// Go DOWN to where you're told
while (position<getSelectedSensorPositionRounded() && allowSnap){
setPower(-.25);
}
} else {
// Go UP to where you're told
while (position>getSelectedSensorPositionRounded() && allowSnap){
setPower(.25);
}
}
setPower(0);
}
}
public double getSelectedSensorPositionRounded() {
return Math.round(liftMotor.getSelectedSensorPosition()/25);
}
public void gotoRocketHole(int holeID){
if (holeID == 0){
gotoPosition(liftHoleBottom);
} else if (holeID == 1){
gotoPosition(liftHoleMiddle);
} else if (holeID == 2){
gotoPosition(liftHoleTop);
}
}
@Override
public void initDefaultCommand() {
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());
this.setDefaultCommand(new LiftDrive());
}
}
It does work but since I am using a while loop, it causes problems with the iterative robot setup that FRC provides as standard. Is there any better way I can do this? I was thinking PID loops, but I have no clue how to use, code, and properly implement them. Simply put, can I do this better, and possibly “port” it to work with PID?
This is what the commands are for. Don’t try to coordinate things across time in the subsystem too much.
That is, goToRocketHole() will be a command, not a method in the subsystem. In that command, the execute() method will apply whatever force is required to continue moving to the desired position, arriving at low speed.
The isFinished() method will determine if the endpoint has been achieved, and return true if it has (at the right position, speed zero, with suitably small error).
For an example, you can look here:
This is the post-season code for 3946’s STEAMworks robot. One of the tasks was to push a gear onto the peg, stop when either it reached the appropriate set point or the command timed out, and retract the pushing scoosh (a swoosh-shaped device which scooted the gear along). The subsystem for this is Scoosh, and the command is HangGear. As you can see, the Scoosh methods just let a command set the motor throttle and read the position (angle) and speed of the scoosh. The “while()” work is done in the Command, with isFinished being the (inverted) value of what would go inside the while parenthesis.
OBTW, once HangGear finishes, the robot then automatically starts LoadHoldGear, because it is the default command for the subsystem. LoadHoldGear will bring the scoosh back to the internal position, ready to load another gear.
The way timing works on the RIO, you “run” commands, and the “scheduler” runs all the commands every ~20ms. A command will continue being called every 20ms until the isFinished() method returns “true”, at which point the command is taken out of the scheduler.
Feel free to correct me if someone has a more detailed analysis.
Nothing incorrect, just a few more details. This is my mental model from working with it, not necessarily what the scheduler does under the hood. Thanks to @fovea1959 for fixes!
When a command is created/instantiated with new myCommand(), the myCommand() method is called, which usually instantiates sub-objects and reads calibration tables and such. Also, the “requires()” calls are entered into a database for the scheduler.
When the command is run, any running commands which requires() the same subsystems as the current command in the scheduler database are “interrupted” by calling Old.interrupt(), and those oldCommands are removed from the scheduler list. Then the the initialize() method is called. Also, myCommand() is added to the scheduler list.
The scheduler regularly* checks to see if the command isFinished(). If not, the scheduler runs execute(). If it is, the scheduler runs end() to clean up loose ends, removes the command from the scheduler list, and runs a new defaultCommand for each of its subsystems, if defined.
as noted above, interrupted() is called if another command is started using one or more of the command’s subsystems, giving you a chance to clean up, stop motors, etc. It is not unusual for interrupted() and end() to have identical functionality, or for one to call the other.
* Regularly is on a timed basis if using TimedRobot, or as each packet is received from the Driver Station if using Iterative.
@GeeTwo that’s a great summary, a couple of details:
I think requires() is usually in the constructor, not in initialize()?
initialize() is called when the command is started, not when the command is created with new. The former can happen many times, the latter only once (usually when the robot is first started, in robotInit(), though YMMV.
oldCommand.interrupted() is called when the new command is started, not when the requires() is called. whatever you said for requires() gets saved, and is evaluated and appropriate action (interrupting old commands) taken when the command is started…