Turn Command
Next we are going to create a command called TurnCommand which will allow us to turn the robot in place by a specified angle. You should know enough now to create the framework for this command. We will want to specify the speed to be used in the turn as well as the angle so create the constructor for your class so that it takes two double parameters, the first to specify the speed and the second to specify the angle. After you have created the new command, compare you code to the code below.
.
.
.
.
package commands;
import robot.Robot;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class TurnCommand extends Command
{
private double m_speed;
private double m_angle;
public TurnCommand(double speed, double angle)
{
Logger.Log("TurnCommand", 3, "TurnCommand()");
m_speed = speed;
m_angle = angle;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("TurnCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("TurnCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("TurnCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("TurnCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("TurnCommand", 2, "interrupted()");
}
}
Did you create some member variables (e.g. m_speed and m_angle) to store the speed and angle that are passed in on the constructor? Did you also remember to require the DriveSubsystem? Great!
Now we are going to use the encoders to measure the turn. Once again, we will want to use copies of the encoders like we did in our DriveForDistanceCommand. Add code to declare and initialize the left and right encoder and then compare your code to that below.
.
.
.
.
package commands;
import robot.Robot;
import robotCore.Encoder;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class TurnCommand extends Command
{
private double m_speed;
private double m_angle;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
public TurnCommand(double speed, double angle)
{
Logger.Log("TurnCommand", 3, "TurnCommand()");
m_speed = speed;
m_angle = angle;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("TurnCommand", 2, "initialize()");
m_leftEncoder = Robot.m_driveSubsystem.GetLeftEncoder().Clone();
m_rightEncoder = Robot.m_driveSubsystem.GetRightEncoder().Clone();
m_leftEncoder.reset();
m_rightEncoder.reset();
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("TurnCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("TurnCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("TurnCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("TurnCommand", 2, "interrupted()");
}
}
Did you remember to reset the encoders after you initialized them?
Now in the initialize() function we also want to turn the motors on. To make the robot turn, we need to set the speed to the left and right motors opposite, and we need to set the signs so that it turns in the correct direction. Let’s say that a positive angle should make the robot turn to the right (clockwise) and a negative angle should make the robot turn to the left (anticlockwise). See if you can add code to your initialize() function to accomplish this and then check your result with the one below.
.
.
.
.
protected void initialize()
{
Logger.Log("TurnCommand", 2, "initialize()");
m_leftEncoder = Robot.m_driveSubsystem.GetLeftEncoder().Clone();
m_rightEncoder = Robot.m_driveSubsystem.GetRightEncoder().Clone();
m_leftEncoder.reset();
m_rightEncoder.reset();
if (m_angle < 0)
{
Robot.m_driveSubsystem.SetSpeed(-m_speed, m_speed); // Turn left
}
else
{
Robot.m_driveSubsystem.SetSpeed(m_speed, -m_speed); // Turn right
}
}
Now we need to add some code to the isFinished() function to return true when the robot has turned by the requested angle. Add the following line to the isFinished() function:
int delta = m_leftEncoder.get() - m_rightEncoder.get();
Note that delta will be positive if the robot is turning right (i.e. the left wheel is moving forward and the right wheel is moving backward), and negative if the robot is turning left. The magnitude of delta will also be a measure of how far the robot is turned. So lets return true when the magnitude of delta exceeds the magnitude of the angle.
protected boolean isFinished()
{
Logger.Log("TurnCommand", -1, "isFinished()");
int delta = m_leftEncoder.get() - m_rightEncoder.get();
return(Math.abs(delta) >= Math.abs(m_angle));
}
Note that we are using the absolute value of both delta and m_angle in the comparison since they can both be negative.
Rember to turn the motors back off in the end() function. Your TurnCommand.java file should now look like:
package commands;
import robot.Robot;
import robotCore.Encoder;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class TurnCommand extends Command
{
private double m_speed;
private double m_angle;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
public TurnCommand(double speed, double angle)
{
Logger.Log("TurnCommand", 3, "TurnCommand()");
m_speed = speed;
m_angle = angle;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("TurnCommand", 2, "initialize()");
m_leftEncoder = Robot.m_driveSubsystem.GetLeftEncoder().Clone();
m_rightEncoder = Robot.m_driveSubsystem.GetRightEncoder().Clone();
m_leftEncoder.reset();
m_rightEncoder.reset();
if (m_angle < 0)
{
Robot.m_driveSubsystem.SetSpeed(-m_speed, m_speed); // Turn left
}
else
{
Robot.m_driveSubsystem.SetSpeed(m_speed, -m_speed); // Turn right
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("TurnCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("TurnCommand", -1, "isFinished()");
int delta = m_leftEncoder.get() - m_rightEncoder.get();
return(Math.abs(delta) >= Math.abs(m_angle));
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("TurnCommand", 2, "end()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("TurnCommand", 2, "interrupted()");
}
}
Note that if we want to be able to set the angle in degrees, we will need to figure out a conversion factor that will convert degrees to the arbitrary encoder units returned by the encoders. For now we will pass in an angle in the encoder units and see how far the robot turns. Configure the OI class so that this command will be executed with 0.5 speed for and angle of 2000 when we press our test button 2 on the joystick. When you have made that change, compare it to the code below.
.
.
.
.
package robot;
import robotCore.Joystick;
import commands.DriveForTimeCommand;
import commands.TurnCommand;
import robotWpi.buttons.Button;
import robotWpi.buttons.JoystickButton;;
/**
* This class is the glue that binds the controls on the physical operator
* interface to the commands and command groups that allow control of the robot.
*/
public class OI {
//// CREATING BUTTONS
// One type of button is a joystick button which is any button on a joystick.
// You create one by telling it which joystick it's on and which button
// number it is.
Joystick m_stick = new Joystick(0);
Button m_trigger = new JoystickButton(m_stick, 1);
Button m_testButton = new JoystickButton(m_stick, 2);
// There are a few additional built in buttons you can use. Additionally,
// by subclassing Button you can create custom triggers and bind those to
// commands the same as any other Button.
//// TRIGGERING COMMANDS WITH BUTTONS
// Once you have a button, it's trivial to bind it to a button in one of
// three ways:
// Start the command when the button is pressed and let it run the command
// until it is finished as determined by it's isFinished method.
public OI()
{
m_trigger.whenPressed(new DriveForTimeCommand(0.75, 2));
m_testButton.whenPressed(new TurnCommand(0.5, 2000));
}
}
Now deploy and run your program and see how far the robot turns. Try and find a number to replace the 2000 value that will make the robot turn exactly 360 degrees.
.
.
.
.
You should find that approximately 4900 encoder units will make the robot turn 360 degrees. This means that if the input angle is in degrees, we need to multiply it by (4900 / 360) to get the number of encoder units that we need. Implementing this in the code we get:
public TurnCommand(double speed, double angle)
{
Logger.Log("TurnCommand", 3, "TurnCommand()");
m_speed = speed;
m_angle = (angle * 4900) / 360;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
Now change the line in the OI constructor to turn the robot by 180 degrees:
m_testTurnButton.whenPressed(new TurnCommand(0.5, 180));
Before moving on, verify that if you specify a negative angle the robot will turn the other way:
m_testTurnButton.whenPressed(new TurnCommand(0.5, -180));
You may find that the robot does not turn exactly the desired 180 degrees. This is because the accuracy of the turns using this method is poor because of wheel slippage and other issues. If the robot had a gyroscope, we could use that for more accurate turns, but since it does not, we will need to live with the inaccuracies.