Motor swivelMotor = ...
Motor shoulderMotor = ...
Motor elbowMotor = ...
Compass swivelAngle = ...
AngleSensor shoulderAngle = ...
AngleSensor elbowAngle = ...
Solenoid claw = ...Why Commands
Before we dive into more detail about how to use the command framework work, let’s start by showing what commands are and why they’re better than the alternative.
Imagine a robot with an articulating 2-join arm that can swivel on the chassis. At the end of the arm is a claw that can open and close. The swivel and each joint has a motor and an encoder so that it knows the angle of the joint, and a pneumatic actuator opens and closes the joint.
First of all, let’s look at the Strongback components that the robot code might use:
(We use a Compass for swivelAngle instead of an AngleSensor because our arm can swivel more than 360º.)
Based upon the game, the robot needs to be able to move the arm to grab a game piece, but that means swiveling the arm to 30º to the right of center, opening the claw, and raising the claw to be 25" off the ground and 12" from the front of the robot (which the mechanical design team tells you corresponds to a shoulder angle of 64º and an elbow angle of 83º). Your job as a programmer is to make this happen, regardless of how the arm is currently positioned.
Using very simplistic sequential-style logic, you might initially try something like this:
// First swivel the arm to the correct angle (within +/- 1 degree) ...
while ( true ) {
double diff = swivelAngle.computeHeadingChangeTo(30.0,1);
if ( diff < 0.0 ) {
swivelMotor.setSpeed(-0.4);
} else if ( diff > 0.0 ) {
swivelMotor.setSpeed(0.4);
} else {
swivelMotor.stop();
break;
}
}
// Raise the shoulder to the correct angle ...
while ( true ) {
double diff = shoulderAngle.getAngleChangeTo(64.0,1);
if ( diff < 0.0 ) {
shoulderMotor.setSpeed(-0.4);
} else if ( shoulderAngle.getAngleChangeTo(64.0,1) > 0.0 ) {
shoulderMotor.setSpeed(0.4);
} else {
shoulderMotor.stop();
break;
}
}
// Move the elbow to the correct angle ...
while ( true ) {
double diff = elbowAngle.getAngleChangeTo(83.0,1);
if ( diff < 0.0 ) {
elbowMotor.setSpeed(-0.4);
} else if ( diff > 0.0 ) {
elbowMotor.setSpeed(0.4);
} else {
elbowMotor.stop();
break;
}
}
// Open the claw ...
claw.retract();Now, we surely could rewrite this to make it more readable and more efficient, or we could put this logic inside a class so that we don’t have to duplicate it in multiple places. Yes, we could make the code better in a number of ways. But no matter what we do, this code will always take multiple seconds to run, or as long as needed until the hardware moves to the desired physical orientation. And while that is happening, the robot really can’t do anything else. (Well, it could, but you’d have to embed that other logic inside this block of code! Try making your robot do 5 or even 10 things at once — yuck!)
Commands make this far easier and simpler. We’ll create a separate Command subclass for each movable part: SwivelArm, RotateShoulder, RotateElbow, OpenClaw, and CloseClaw. Let’s start with SwivelArm:
public class SwivelArm extends Command {
private final Motor swivel;
private final Compass angle;
private final double speed;
private final double desiredAngle;
private final double tolerance;
public SwivelArm( Motor swivel, Compass angle, double speed,
double desiredAngleInDegrees, double toleranceInDegrees ) {
super(swivel);
this.swivel = swivel;
this.angle = angle;
this.speed = Math.abs(speed);
this.desiredAngle = desiredAngleInDegrees;
this.tolerance = toleranceInDegrees;
}
@Override
public boolean execute() {
double diff = angle.computeHeadingChangeTo(desiredAngleInDegrees,toleranceInDegrees);
if ( diff == 0.0 ) {
swivel.stop();
return true;
}
if ( diff < 0.0 ) {
swivel.setSpeed(-speed);
} else {
swivel.setSpeed(speed);
}
return false;
}
}The RotateShoulder and RotateElbow commands will actually be nearly identical. Rather than copy all that code, let’s refactor SwivelArm to put most of the logic inside a base class. And because one would deal with a Compass and the other two a AngleSensor, let’s actually use Java 8 lambdas and java.util.function.DoubleSupplier to tell us whether we need to move the motor. The result is a MoveDevice class that we can use for any motor:
public class MoveDevice extends Command {
private final Motor swivel;
private final DoubleSupplier moveDirection;
private final double speed;
public SwivelArm( Motor swivel, double speed, DoubleSupplier moveDirection ) {
super(swivel);
this.swivel = swivel;
this.speed = Math.abs(speed);
this.moveDirection = moveDirection;
}
@Override
public boolean execute() {
double diff = moveDirection.getAsDouble();
if ( diff == 0.0 ) {
swivel.stop();
return true;
}
if ( diff < 0.0 ) {
swivel.setSpeed(-speed);
} else {
swivel.setSpeed(speed);
}
return false;
}
}
public class SwivelArm extends MoveDevice {
public SwivelArm( Motor motor, Compass angle, double speed,
double desiredHeadingInDegrees, double toleranceInDegrees ) {
super(motor,speed,()->angle.computeHeadingChangeTo(desiredHeadingInDegrees,toleranceInDegrees);
}
}
public class RotateShoulder extends MoveDevice {
public RotateShoulder( Motor motor, Angle angle, double speed,
double desiredAngleInDegrees, double toleranceInDegrees ) {
super(motor,speed,()->angle.computeAngleChangeTo(desiredAngleInDegrees,toleranceInDegrees);
}
}
public class RotateElbow extends MoveDevice {
public RotateElbow( Motor motor, Angle angle, double speed,
double desiredAngleInDegrees, double toleranceInDegrees ) {
super(motor,speed,()->angle.computeAngleChangeTo(desiredAngleInDegrees,toleranceInDegrees);
}
}
public class SolenoidOperation extends Command {
public SolenoidOperation( Solenoid solenoid ) {
}
}
public class OpenClaw extends Command {
private final Solenoid solenoid;
public OpenClaw( Solenoid solenoid ) {
super(solenoid);
this.solenoid = solenoid;
}
@Override
public boolean execute() {
this.solenoid.retract();
return true;
}
}
public class CloseClaw extends SolenoidOperation {
private final Solenoid solenoid;
public CloseClaw( Solenoid solenoid ) {
super(solenoid);
this.solenoid = solenoid;
}
@Override
public boolean execute() {
this.solenoid.extend();
return true;
}
}We can then make our robot position the arm and claw with this simple code:
Strongback.submit(new SwivelArm(swivelMotor,swivelAngle,0.4,30.0,1.0));
Strongback.submit(new RotateShoulder(shoulderMotor,shoulderAngle,0.4,64.0,1.0));
Strongback.submit(new RotateElbow(elbowMotor,elbowAngle,0.4,83.0,1.0));
Strongback.submit(new OpenClaw(claw));This is far simpler, and better yet all of these commands operate asynchronously (in another thread), will complete whenever they are done, and do not prevent our code from doing other things and submitting other commands! And if we submit any commands that use the arm, shoulder, elbow, or claw while these commands are still running, Strongback will automatically cancel any earlier commands that have the same requirements.
Now, if we find that we’re regularly submitting these same commands together, we might want to create a command group. We could create a class, but it might be simpler and make even more readable code if we do all of this in a function that creates an anonymous class. In fact, let’s create a RobotArm class that holds onto the components that make up the arm:
public class RobotArm {
private final Motor swivel;
private final Motor shoulder;
private final Motor elbow;
private final Compass swivelAngle;
private final AngleSensor shoulderAngle;
private final AngleSensor elbowAngle;
private final Solenoid claw;
public RobotArm( Motor swivel, Compass swivelAngle,
Motor shoulder, AngleSensor shoulderAngle,
Motor elbow, AngleSensor elbowAngle,
Solenoid claw ) {
this.swivel = swivel;
this.shoulder = shoulder;
this.elbow = elbow;
this.claw = claw;
this.swivelAngle = swivelAngle;
this.shoulderAngle = shoulderAngle;
this.elbowAngle = elbowAngle;
}
public CommandGroup positionArm( double swivelTargetAngle,
double shoulderTargetAngle,
double elbowTargetAngle,
double tolerance,
double speed,
boolean openClaw ) {
return CommandGroup.runSimultaneously(
new SwivelArm(swivel,swivelAngle,speed,swivelTargetAngle,tolerance));
new RotateShoulder(shoulder,shoulderAngle,speed,shoulderTargetAngle,tolerance));
new RotateElbow(elbow,elbowAngle,speed,elbowTargetAngle,tolerance));
openClaw ? new OpenClaw(claw) : new CloseClaw(claw));
);
}
...
}Then, to position the arm at any time, simply invoke the positionArm method on the RobotArm intance (which would be easy to do via a button):
Strongback.submit( robotArm.positionArm(30.0,64.0,83.0,1.0,0.4,true) );Of course, you enhance the RobotArm class with the logic to convert desired physical positions of the claw (e.g., 30º to the right of center, claw 25" off the ground and 12" from the front of the robot) into the correct target angles:
public class RobotArm {
...
public CommandGroup physicallyPositionArm( double swivelTargetAngle,
double clawHeight,
double clawDistanceForward,
double speed,
boolean openClaw ) {
...
}
}Not only would this encapsulate the arm-related components, but it can also encode your mathematical models of the physical system in a class that is very easy to test.