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. Also since we are now using speed rather than power let’s rename the power parameter to speed. The best way to do this is to use VS Studio’s Rename function. Right click on the parameter power and choose Rename Symbol, then change the name to power. Note that this changes all instances in your program automatically. Let’s do the same for the member variable m_power, changing it to m_speed.
Your DriveForDistanceCommand.java file should now look like:
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Encoder;
import robotCore.Logger;
/**
* An example command that uses an example subsystem.
*/
public class DriveForDistanceCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private double m_speed;
private double m_distance;
private Encoder m_leftEncoder;
/**
* Creates a new DriveForDistanceCommand.
*
* @param subsystem The subsystem used by this command.
*/
public DriveForDistanceCommand(DriveSubsystem subsystem, double speed, double distance) {
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_subsystem = subsystem;
m_speed = speed;
m_distance = distance;
m_leftEncoder = subsystem.getLeftEncoder();
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
Logger.Log("DriveForDistanceCommand", 2, "initialize()");
m_leftEncoder.reset();
m_subsystem.setSpeed(m_speed, m_speed);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("DriveForDistanceCommand", -1, "execute()");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
Logger.Log("DriveForDistanceCommand", 2, String.format("end(%b)", interrupted));
m_subsystem.setPower(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
Logger.Log("DriveForDistanceCommand", -1, "isFinished()");
return (m_leftEncoder.get() >= m_distance);
}
}
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 may turn 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). Add a declaration for m_rightEncoder and initialize it in the constructor:
public class DriveForDistanceCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private double m_speed;
private double m_distance;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
/**
* Creates a new DriveForDistanceCommand.
*
* @param subsystem The subsystem used by this command.
*/
public DriveForDistanceCommand(DriveSubsystem subsystem, double speed, double distance) {
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_subsystem = subsystem;
m_speed = speed;
m_distance = distance;
m_leftEncoder = subsystem.getLeftEncoder();
m_rightEncoder = subsystem.getRightEncoder();
In the initialize() function we need to reset the right encoder:
public void initialize() {
Logger.Log("DriveForDistanceCommand", 2, "initialize()");
m_leftEncoder.reset();
m_rightEncoder.reset();
m_subsystem.setSpeed(m_speed, m_speed);
}
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 and compute the difference between the two:
public void execute() {
int leftDistance = m_leftEncoder.get();
int rightDistance = m_rightEncoder.get();
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_speed + deltaDistance, m_speed - 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_speed + deltaDistance * k_scale, m_speed - deltaDistance * k_scale);
Once again we are defining the scale factor as a constant k_scale which we define as follows:
public class DriveForDistanceCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private double m_speed;
private double m_distance;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
private static final double k_scale = 0.001;
The completed execute() function should now look like:
public void execute() {
Logger.Log("DriveForDistanceCommand", -1, "execute()");
int leftDistance = m_leftEncoder.get();
int rightDistance = m_rightEncoder.get();
int deltaDistance = rightDistance - leftDistance;
m_subsystem.setSpeed(m_speed + deltaDistance * k_scale, m_speed - deltaDistance * k_scale);
}
Your DriveForDistanceCommand.java file should now look like:
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Encoder;
import robotCore.Logger;
/**
* An example command that uses an example subsystem.
*/
public class DriveForDistanceCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private double m_speed;
private double m_distance;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
private static final double k_scale = 0.001;
/**
* Creates a new DriveForDistanceCommand.
*
* @param subsystem The subsystem used by this command.
*/
public DriveForDistanceCommand(DriveSubsystem subsystem, double speed, double distance) {
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_subsystem = subsystem;
m_speed = speed;
m_distance = distance;
m_leftEncoder = subsystem.getLeftEncoder();
m_rightEncoder = subsystem.getRightEncoder();
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
Logger.Log("DriveForDistanceCommand", 2, "initialize()");
m_leftEncoder.reset();
m_rightEncoder.reset();
m_subsystem.setSpeed(m_speed, m_speed);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("DriveForDistanceCommand", -1, "execute()");
int leftDistance = m_leftEncoder.get();
int rightDistance = m_rightEncoder.get();
int deltaDistance = rightDistance - leftDistance;
m_subsystem.setSpeed(m_speed + deltaDistance * k_scale, m_speed - deltaDistance * k_scale);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
Logger.Log("DriveForDistanceCommand", 2, String.format("end(%b)", interrupted));
m_subsystem.setPower(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
Logger.Log("DriveForDistanceCommand", -1, "isFinished()");
return (m_leftEncoder.get() >= m_distance);
}
}
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 units is equal to 32 inches. So if we want to pass in the distance in inches, we will need to multiply that by (4000 / 32) to get encoder units. Change the DriveForDistanceCommand constructor to perform this calculation:
public DriveForDistanceCommand(DriveSubsystem subsystem, double speed, double distance) {
Logger.Log("DriveForDistanceCommand", 3, "DriveForDistanceCommand()");
m_subsystem = subsystem;
m_speed = speed;
m_distance = (distance * 4000) / 32;;
m_leftEncoder = subsystem.getLeftEncoder();
m_rightEncoder = subsystem.getRightEncoder();
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
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.