Drive Straight
Now that we have the ability to control the speed of the motors let’s change our DriveForDistanceCommand so to use speed rather than power to make the robot drive more straight. Open your DriveForDistanceCommand.java file and change the SetPower calls to SetSpeed calls. Your DriveForDistanceCommand.java file should now look like:
package commands;
import robotCore.Logger;
import robotWpi.command.Command;
import robot.Robot;
/**
*
*/
public class DriveForDistanceCommand extends Command
{
private double m_power;
private double m_distance;
public DriveForDistanceCommand(double power, double distance)
{
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_power = power;
m_distance = distance;
// 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("DriveForDistanceCommand", 2, "initialize()");
Robot.m_driveSubsystem.GetLeftEncoder().reset();
Robot.m_driveSubsystem.SetSpeed(m_power, m_power);
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveForDistanceCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveForDistanceCommand", -1, "isFinished()");
return(Robot.m_driveSubsystem.GetLeftEncoder().get() >= m_distance);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForDistanceCommand", 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("DriveForDistanceCommand", 2, "interrupted()");
end();
}
}
Open up the OI.java file and change the test button (2) back to run the DriveForDistanceCommand and also increase the distance it travels to 4000 units so we can better see how it behaves. So your OI.java file should now look like:
package robot;
import robotCore.Joystick;
import commands.DriveForDistanceCommand;
import commands.DriveForTimeCommand;
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 DriveForDistanceCommand(0.5, 4000));
}
}
Now if you deploy and run the (remember that this command is tied to button 2 on the joystick), you will see that once the robot gets going, it drives pretty straight. However while it is starting up, it turns a little bit. This is because, as we saw from the speed graphs in the previous chapter, that the motors do not start up quite in sync. What we want is a way to measure if the robot has turned and compensate the power so that it drives in a straight line.
To do this, we are going to change our DriveForDistanceCommand to use both the left and right encoders to keep the robot driving straight. To do this we are going to need to access both the left and right encoders (currently we are only using the left encoder to measure the distance the robot has traveled). Now we could use the left and right encoders from the DriveSubsystem directly like we did before, but since we are going to be wanting to reset the encoders from time to time it would be nice to have our own copies so that doing so does not affect others who might need the encoders.
To this effect, we are going to create two new member variables that will hold copies of the left and right encoders:
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
Now we need to initialize these variable somewhere. We don’t want to do it in the constructor for this class because the encoders for the DriveSubsystem may not have yet been created when the constructor is called. Therefore we will do it in the initialize() function, when this command is invoked.
protected void initialize()
{
Logger.Log("DriveForDistanceCommand", 2, "initialize()");
m_leftEncoder = Robot.m_driveSubsystem.GetLeftEncoder().Clone();
m_rightEncoder = Robot.m_driveSubsystem.GetRightEncoder().Clone();
m_leftEncoder.reset();
m_rightEncoder.reset();
Robot.m_driveSubsystem.SetSpeed(m_power, m_power);
}
Note that when we call the GetLeftEncoder() function for the DriveSubsystem, we call it’s Clone() function. This will make a copy of the encoder. This copy will correspond to the same physical encoder, but will have it’s own zero point, so resetting this encoder will not affect the zero point of the encoder used by the DriveSubsystem.
Not also, that after obtaining the copies of the left and right encoders, we reset their current values to zero (we also removed the previous call to Robot.m_driveSubsystem.GetLeftEncoder().reset()).
Now in the execute() function we are going to adjust the speed of the motors to keep the robot driving straight. The first thing we want to do is to get the current left and right encoder values:
int leftDistance = m_leftEncoder.get();
int rightDistance = m_rightEncoder.get();
How can we use these numbers to keep the robot driving straight? Consider what we get if we compute the difference between these two numbers:
int deltaDistance = rightDistance - leftDistance;
Now if the robot is driving straight then deltaDistance should be zero because the left and right wheels have turned by the same amount. It is, therefore, our goal to keep this deltaDistance as close to zero as we can at all times. If deltaDistance is positive, then this means that the right wheel has turned farther than the left and we need to slow it down and speed up the left. Conversely, if deltaDistance is negative then the left wheel is running too fast and we need to slow it down and speed up the right. Conceptually we want to subtract the deltaDistance from the speed of the right motor and add it to the speed of the left motor. Something like:
Robot.m_driveSubsystem.SetSpeed(m_power + deltaDistance, m_power - deltaDistance);
This is not quite what we want, however. Remember that the speed value should be in the range of -1.0 to +1.0. So if the difference between the left and right distance is greater than or equal to one, it will slam the robot to the left or right at full power which is not what we want. So what we need to do is to scale the deltaDistance by some fractional constant. Since we know that a full revolution of the wheel is about 1000 distance units, lets start with a scale factor of 0.001:
Robot.m_driveSubsystem.SetSpeed(m_power + deltaDistance * k_scale, m_power - deltaDistance * k_scale);
Once again we are defining the scale factor as a constant k_scale which we define as follows:
private static final double k_scale = 0.001;
The completed execute() function should now look like:
protected void execute()
{
Logger.Log("DriveForDistanceCommand", -1, "execute()");
int leftDistance = m_leftEncoder.get();
int rightDistance = m_rightEncoder.get();
int deltaDistance = rightDistance - leftDistance;
Robot.m_driveSubsystem.SetSpeed(m_power + deltaDistance * k_scale, m_power - deltaDistance * k_scale);
}
Finally, since we are now using local copies of the encoders we need to change our isFinished() function to use that copy.
protected boolean isFinished()
{
Logger.Log("DriveForDistanceCommand", -1, "isFinished()");
return(m_leftEncoder.get() >= m_distance);
}
Your DriveForDistanceCommand.java file should now look like:
package commands;
import robotCore.Encoder;
import robotCore.Logger;
import robotWpi.command.Command;
import robot.Robot;
/**
*
*/
public class DriveForDistanceCommand extends Command
{
private static final double k_scale = 0.001;
private double m_power;
private double m_distance;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
public DriveForDistanceCommand(double power, double distance)
{
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_power = power;
m_distance = distance;
// 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("DriveForDistanceCommand", 2, "initialize()");
m_leftEncoder = Robot.m_driveSubsystem.GetLeftEncoder().Clone();
m_rightEncoder = Robot.m_driveSubsystem.GetRightEncoder().Clone();
m_leftEncoder.reset();
m_rightEncoder.reset();
Robot.m_driveSubsystem.SetSpeed(m_power, m_power);
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveForDistanceCommand", -1, "execute()");
int leftDistance = m_leftEncoder.get();
int rightDistance = m_rightEncoder.get();
int deltaDistance = rightDistance - leftDistance;
Robot.m_driveSubsystem.SetSpeed(m_power + deltaDistance * k_scale, m_power - deltaDistance * k_scale);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveForDistanceCommand", -1, "isFinished()");
return(m_leftEncoder.get() >= m_distance);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForDistanceCommand", 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("DriveForDistanceCommand", 2, "interrupted()");
end();
}
}
Now deploy and run your program. You should see that the robot does a better job of driving straight. The size of the k_scale factor will control how quickly the robot corrects it’s direction. Since the robot currently is taking a little longer than we like to correct it’s path, try increasing the value of k_scale to 0.002. Note that if you make this number too large, the robot will over correct and start to swerve. Making it even larger can simply result in the robot quickly turning back and forth. Feel free to experiment with some other numbers and see what happens.
There are other mechanisms that you can use to keep your robot driving in a particular direction. For example, if your robot has a gyroscope sensor which reports it current orientation, you can use that sensor in a similar manner to drive a straight line. You would adjust the left and right motor speeds based on how far the robot is deviating from the desired direction. This mechanism has the advantage on not being affected by any wheel slippage that might occur, which would cause errors when using our wheel encoder method.
Before we move on, let’s make one more change to our DriveForDistanceCommand. Specifying the distance in encoder units is OK, but it would really be nice if we could use real world units like inches instead. To do that we will need to compute a conversion factor that will convert inches into the encoder units.
Set the robot up to run the DriveForDistanceCommand again, but this time put a piece of tape on the floor to mark it’s starting position. Then run your program and mark the position where the robot ends and measure the distance it traveled using a tape measure.
.
.
.
.
You should find that the robot moves about 32 inches. This means that 4000 encoder ticks is equal to 32 inches. So if we want to pass in the distance in inches, we will need to multiply that by (4000 / 33) to get encoder units. Change the DriveForDistanceCommand constructor to perform this calculation:
public DriveForDistanceCommand(double power, double distance)
{
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_power = power;
m_distance = (distance * 4000) / 32;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
Now change your OI call to set the distance to drive to be 20 inches.
m_testButton.whenPressed(new DriveForDistanceCommand(0.5, 20));
Now test out your program and see that the robot drives 20 inches.
One final note. This command, as written, will only drive the robot forward. If you attempt to drive backwards by setting the speed negative, you will find the robot never stops. It is left as an exercise for the reader to fix the program so that it can successfully drive either forward or backward.