Wheel Encoders
We have been controlling the motors by setting their power. Unfortunately, this is not ideal because the power only indirectly controls the speed, which is what we are really interested in. As we have already seen, our robot does not exactly drive straight when we apply the same power to both wheels. This is because differences in the motors and the way they are mounted cause them to run at slightly different speeds when the same power is applied.
What we need is some way to tell how far the wheels have turned and how fast. Each of the wheels on this robot have an encoder which will do just that. We will use the encoder to control the distance the robot moves as well as it’s speed.
For our first exercise, we are going to create a new command, similar to our DriveForTimeCommand, that will drive the robot forward a fixed distance, rather than a fixed time.
The encoders that we are using are handled by the Encoder class. We will need to create a separate instance of this class for the left and right motors. Since the encoders are really part of the DriveSubsystem, we will be adding these to that class. Consulting the Encoder class, we see that the constructor take a single integer parameter which specifies the encoder number (i.e. 0 or 1). For this robot the left encoder is connected to port 0, while the right encoder is on port 1. Open the DriveSubsystem.java file and add the following at top of the class.
private Encoder m_rightEncoder = new Encoder(robotCore.Encoder.EncoderType.Analog, k_rightEncoderPin1, k_rightEncoderPin2); private Encoder m_leftEncoder = new Encoder(robotCore.Encoder.EncoderType.Analog, k_leftEncoderPin1, k_leftEncoderPin2);
Once again, we are using constants for the port numbers which must be declared as follows:
private static final int k_leftEncoderPin1 = 0; private static final int k_leftEncoderPin2 = 1; private static final int k_rightEncoderPin1 = 2; private static final int k_rightEncoderPin2 = 3;
Now the way these encoders are wired we will discover that for the right encoder when the wheel moves in the forward direction, the encoder will count up. However the left encoder will count down when the wheel moves forward. Hence we want to reverse the direction of the left encoder. We do this by creating a constructor for the DriveSubsystem class and inverting the left encoder as follows:
public DriveSubsystem()
{
m_leftEncoder.setInverted(true);
}
Now we are going to need access to these encoders from our Command classes, so we will also create functions to retrieve the left and right encoders:
public Encoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public Encoder GetRightEncoder()
{
return(m_rightEncoder);
}
Your DriveSubsystem.java file should now look like:
package subsystem;
import robotWpi.command.Subsystem;
import commands.ArcadeDriveCommand;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_rightMotorPWMPin = 6;
private static final int k_rightMotorDirPin = 7;
private static final int k_leftMotorPWMPin = 5;
private static final int k_leftMotorDirPin = 4;
private static final int k_leftEncoderPin1 = 0;
private static final int k_leftEncoderPin2 = 1;
private static final int k_rightEncoderPin1 = 2;
private static final int k_rightEncoderPin2 = 3;
private PWMMotor m_leftMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
private PWMMotor m_rightMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
private Encoder m_rightEncoder = new Encoder(robotCore.Encoder.EncoderType.Analog, k_rightEncoderPin1, k_rightEncoderPin2);
private Encoder m_leftEncoder = new Encoder(robotCore.Encoder.EncoderType.Analog, k_leftEncoderPin1, k_leftEncoderPin2);
public DriveSubsystem()
{
m_leftEncoder.setInverted(true);
}
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
// Set the default command for a subsystem here.
setDefaultCommand(new ArcadeDriveCommand());
}
public void SetPower(double leftPower, double rightPower)
{
m_rightMotor.set(rightPower);
m_leftMotor.set(leftPower);
}
public Encoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public Encoder GetRightEncoder()
{
return(m_rightEncoder);
}
}
Now it is time to create our new command which we will call DriverForDistanceCommand. Go ahead and create a new class under the commands folder. This time instead of using the ExampleCommand as our starting point, copy the contents of DriveForTimeCommand.java file into your newly created DriveForDistanceCommand.java file. Then replace all instances of DriverForTimeCommand with DriverForDistanceCommand. This will give us a better starting point. Your new file should look like:
package commands;
import robotCore.Logger;
import robotCore.Timer;
import robotWpi.command.Command;
import robot.Robot;
/**
*
*/
public class DriveForDistanceCommand extends Command
{
private Timer m_timer = new Timer();
private double m_power;
private double m_time;
public DriveForDistanceCommand(double power, double time)
{
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_power = power;
m_time = time;
// 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.SetPower(m_power, m_power);
m_timer.reset();
}
// 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(m_timer.get() >= m_time);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForDistanceCommand", 2, "end()");
Robot.m_driveSubsystem.SetPower(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();
}
}
Since we will not be using the timer this time, let’s remove all references to the Timer variable m_timer.
Next we want to change our constructor to take a distance instead of a time. This will also involve in replacing the stored m_time with a stored m_distance.
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);
}
In the initialize we need to reset the encoders. We can use one or both encoders here, but for now we will use the left encoder to measure the distance
protected void initialize()
{
Logger.Log("DriveForDistanceCommand", 2, "initialize()");
Robot.m_driveSubsystem.GetLeftEncoder().reset();
Robot.m_driveSubsystem.SetPower(m_power, m_power);
}
The last thing we need to do is change the isFinished() function to return true when the distance is greater than or equal to the target distance.
protected boolean isFinished()
{
Logger.Log("DriveForDistanceCommand", -1, "isFinished()");
return(Robot.m_driveSubsystem.GetLeftEncoder().get() >= m_distance);
}
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.SetPower(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.SetPower(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 we need to create some way to run this command. What we are going to do is to change the OI.java file so that this command is executed when you press button 2 which is on the side of the joystick handle. To do this we need to define a new button which we will call m_testButton.
Button m_testButton = new JoystickButton(m_stick, 2);
And then in the constructor, OI(), we will configure the DriveForDistance command to be executed when this new button is pressed.
m_testButton.whenPressed(new DriveForDistanceCommand(0.5, 2000));
Note that we have arbitrarily chosen to drive for 2000 units. The units that the encoder uses is a function of the type of encoder. For these encoders, the encoder will register a little more than 1000 units per revolution of the wheel.
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, 2000));
}
}
Build and deploy your program and verify that the robot moves forward a short distance and then stops when you press button 2 on the joystick. Also note that since we are now using distance rather than time, we can change the speed and the robot should still move the same distance. Try it out and see.
At some point you could make a conversion from encoder units to say, inches, by measuring the distance the robot moves in a fixed number of units and then computing the scale factor that will convert units to inches. This will be left as an exercise for the reader.