Speed Control
Now that we know how to use the encoders, we want to change our DriveSubsystem class so that we can set the motors to run at a specific speed by automatically adjusting the power of the motors to maintain that speed.
Before we tackle that, however, we need to gather some information as to the relationship between the power applied to the motors and their speed. We are going to create a new command where we will run the robot forward at different power settings and record the speed that results.
Start by adding a new class TestMotorSpeedCommand to the commands folder, and copy the contents of ExampleCommand.java replacing the ExampleCommand and ExampleSubsystem strings. Your result should 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.Logger;
/**
* An example command that uses an example subsystem.
*/
public class TestMotorSpeedCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
/**
* Creates a new TestMotorSpeedCommand.
*
* @param subsystem The subsystem used by this command.
*/
public TestMotorSpeedCommand(DriveSubsystem subsystem) {
Logger.Log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");
m_subsystem = subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
Logger.Log("TestMotorSpeedCommand", 2, "initialize()");
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("TestMotorSpeedCommand", -1, "execute()");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
Logger.Log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
Logger.Log("TestMotorSpeedCommand", -1, "isFinished()");
return false;
}
}
Now what we want to do is to start the robot at 0 power and gradually increase the power, recording the speed, until we reach full power. To accomplish this we need to create a member variable m_power to hold the current power setting. We are also going to want to record the speed of the motors so we will need an Encoder instance for the left and right encoders.
public class TestMotorSpeedCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private double m_power;
private final Encoder m_leftEncoder;
private final Encoder m_rightEncoder;
/**
* Creates a new TestMotorSpeedCommand.
*
* @param subsystem The subsystem used by this command.
*/
public TestMotorSpeedCommand(DriveSubsystem subsystem) {
Logger.Log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");
m_subsystem = subsystem;
m_leftEncoder = subsystem.getLeftEncoder();
m_rightEncoder = subsystem.getRightEncoder();
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
In the initialize() function we need to set the power to 0. Also we will be logging the motor speeds in the execute() function in a format that will allow us to import them into a spreadsheet. To make the spreadsheet easier to read we add a title row which labels each column.
public void initialize() {
Logger.Log("TestMotorSpeedCommand", 2, "initialize()");
Logger.Log("TestMotorSpeedCommand", 0, ",Power,Left,Right");
m_power = 0;
}
Then in the execute() function we will set the drive power to the current power and then increase the power by 0.0025 each time we are called. We also want to log the speeds of the two motors so that we can view them later. To accomplish this we change the execute() function as follows:
public void execute() {
Logger.Log("TestMotorSpeedCommand", -1, "execute()");
Logger.Log("TestMotorSpeedCommand", 0, String.format(",%f,%d,%d", m_power, m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));
m_subsystem.setPower(m_power, m_power);
m_power += 0.0025;
}
Then we need to change the isFinished() function to return true when the power exceeds 1.3. Note that we are increasing the power up to 1.3 even though the highest power the motor will accept is 1.0 (anything over that will be treated the same as 1.0). The reason we are doing this is to see exactly where the speed tops out at.
public boolean isFinished() {
Logger.Log("TestMotorSpeedCommand", -1, "isFinished()");
return m_power > 1.3;
}
Finally when the command is finished, we need to turn the motors off.
public void end(boolean interrupted) {
Logger.Log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));
m_subsystem.SetPower(0, 0);
}
Your TestMotorSpeedCommand.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 TestMotorSpeedCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private double m_power;
private final Encoder m_leftEncoder;
private final Encoder m_rightEncoder;
/**
* Creates a new TestMotorSpeedCommand.
*
* @param subsystem The subsystem used by this command.
*/
public TestMotorSpeedCommand(DriveSubsystem subsystem) {
Logger.Log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");
m_subsystem = subsystem;
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("TestMotorSpeedCommand", 2, "initialize()");
Logger.Log("TestMotorSpeedCommand", 0, ",Power,Left,Right");
m_power = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("TestMotorSpeedCommand", -1, "execute()");
Logger.Log("TestMotorSpeedCommand", 0,
String.format(",%f,%d,%d", m_power, m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));
m_subsystem.setPower(m_power, m_power);
m_power += 0.0025;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
Logger.Log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));
m_subsystem.setPower(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
Logger.Log("TestMotorSpeedCommand", -1, "isFinished()");
return m_power > 1.3;
}
}
Finally we want to set it so that button 3 on the joystick will run this command. Switching to the RobotContainer.java file, we add a declaration for m_button3:
public class RobotContainer {
// The robot's subsystems and commands are defined here...
@SuppressWarnings("unused")
private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
private final Joystick m_joystick = new Joystick(0);
private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);
and then configure it to run our TestMotorSpeedCommand when it is pressed:
private void configureButtonBindings() {
m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));
m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
}
Now run your program and connect and enable your robot. Before pressing the B3 button to run our command we want to clear the TERMINAL in VS Code. Unfortunately the only way I have found to do that is a little bit convoluted (if anyone can figure out a simpler way to do this, please let me know!). First right click on the word TERMINAL at the bottom of the screen. This will bring up the following menu:
Now ignore this menu and without closing it move the cursor down to the TERMINAL area and right click. This should then give you the menu:
Choose the Clear option from this menu.
Now press the B3 button on the Driver Station. The robot should drive forward slowly increasing it speed. When it has stopped, the logged information should be in the TERMINAL pane:
Now we want to select all of the text in this window so we can paste it into our spreadsheet. Unfortunately using the normal Windows shortcut CTRL + a does not seem to work for some reason. However we can us the same trick that we used to clear the TERMINAL. First right click on the word TERMINAL. Then, ignoring that menu, right click on the TERMINAL window and choose Select All. Then repeat this procedure and this time choose Copy.
Now that you have the text copied to the clipboard start up LibreOffice Calc. Then choose Edit / Paste and you should get the following dialog:
Make sure the Comma option is checked under the Separator Options and then click OK
Now select and delete all of the rows up to the row that hat the Power / Left / Right labels (i.e. rows 1-8 as shown in blue above).
Select the Power, Left and Right columns and click the Insert Chart icon at the top:
For the chart type choose the XY (Scatter) and Lines Only options as shown below:
Which will produce the following graph:
This is a plot of the motor speed vs the power applied. There are several things we can see from looking at the graph. The first is that the motors will not even start running until the power reaches about 0.3. The second is that the left motor is significantly less powerful than the right which is the reason the robot turns left. The last takeaway is that the max speed for the right motor is about 1700 and the max speed of the left motor is about 1500. If we want the robot to drive straight then the max speed for the whole robot will need to be limited to the slower motor (i.e. 1500).
Note that these are the results for this particular robot. Your robot may differ and you should adjust these numbers accordingly.
The SmartMotor class (which the PWMMotor class inherits) has a built in mechanism to control the speed of the motor using a PID controller. PID stands for Proportional, Integral, Differential. When we use this control mechanism, we will set a target for the speed we desire (called the setPoint) and the SmartMotor class will adjust the power sent to the motor based on the following formula:
power = (P * error) + (I * totalError) + (D * deltaError) + (F * setPoint)
where
- P – represents the proportional scale factor.
- error – represents the difference between the current speed and the setPoint.
- I – represents the integral scale factor.
- totalError – represents the total accumulated error over time.
- D – represents the derivative scale factor.
- deltaError – represents the difference between the current error and the last.
- F – represents the constant scale factor which is proportional to the setPoint.
- setPoint – represents the current target speed.
The trick is picking the right combination of P, I, D and F to make the motor behave like you want.
We will start with the F term which represents a constant power which proportional to the setPoint. Now we know that at full power the maximum speed of the right motor is about 1700 units/sec. Thus we need a F term that, when multiplied by the max speed (i.e. 1700) will result in full power being applied (i.e. a power of 1.0). That number is 1/1700 or about 0.00059, which is the number we will use for the right motor. In a similar manner we compute that we should use 0.00066 for the left motor.
Note if we use only the constant term, this will essentially be the same as controlling the motor by power alone. If there is more or less resistance to the motors, no adjustments will occur and the motors will slow down or speed up. However, we want to get this term set so that the motors run close to the target speed we desire.
To set the F terms, we add following two lines to the DriveSubsystem constructor:
m_leftMotor.setFTerm(k_Fleft);
m_rightMotor.setFTerm(k_Fright);
Of course we must define k_Fleft and k_Fright:
private static final double k_Fleft = .00066; // Use the correct number for your robot private static final double k_Fright = 0.00059; // Use the correct number for your robot
Also, in order for the motor controller to control the speed of the motors, it will need to know what encoders are being used to measure the speed. We set the by adding the following two lines to the DriveSubsystem constructor:
m_leftMotor.setFeedbackDevice(m_leftEncoder);
m_rightMotor.setFeedbackDevice(m_rightEncoder);
Next we want to set a scale factor so when we want to set the speed, we can just use a range from -1.0 to +1.0 like we do with other motors. Otherwise we would need to set the speed based on the arbitrary units of the encoder. We do this by adding the following two lines to the constructor:
m_leftMotor.setMaxSpeed(k_maxSpeed);
m_rightMotor.setMaxSpeed(k_maxSpeed);
Defining k_maxSpeed at the top of the class as follows (remember to use the max speed that is appropriate for your particular robot).
public static final int k_maxSpeed = 1500; // Use the correct number for your robot
Note that we have make k_maxSpeed public here. This is so we can later use it to log the target speed in the units of the encoders.
Finally, we could change the setPower function of our DriveSubsystem class to set the speed instead, but there may be a time in the future where we still want to control the power. Therefore, we will create a new function setSpeed which we will use to set the speed as follows:
public void setSpeed(double leftSpeed, double rightSpeed) {
m_leftMotor.setControlMode(SmartMotorMode.Speed);
m_rightMotor.setControlMode(SmartMotorMode.Speed);
m_leftMotor.set(leftSpeed);
m_rightMotor.set(rightSpeed);
}
First we ensure that the controller is in speed mode by calling the setControlMode function. Then we set the desired speeds.
We also need to change the setPower function to set the control mode back to power by adding the following to setPower:
public void setPower(double leftPower, double rightPower) {
m_leftMotor.setControlMode(SmartMotorMode.Power);
m_rightMotor.setControlMode(SmartMotorMode.Power);
m_rightMotor.set(rightPower);
m_leftMotor.set(leftPower);
}
You DriveSubsystem.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.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;
public class DriveSubsystem extends SubsystemBase {
private static final int k_rightMotorPWMPin = 5;
private static final int k_rightMotorDirPin = 4;
private static final int k_leftMotorPWMPin = 6;
private static final int k_leftMotorDirPin = 7;
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;
public static final int k_maxSpeed = 1500; // Use the correct number for your robot
private static final double k_Fleft = .00066; // Use the correct number for your robot
private static final double k_Fright = 0.00059; // Use the correct number for your robot
private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
private PWMMotor m_leftMotor = 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);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
Logger.Log("DriveSubsystem", 3, "DriveSubsystem()");
m_leftEncoder.setInverted(true);
m_leftMotor.setFeedbackDevice(m_leftEncoder);
m_rightMotor.setFeedbackDevice(m_rightEncoder);
m_leftMotor.setMaxSpeed(k_maxSpeed);
m_rightMotor.setMaxSpeed(k_maxSpeed);
m_leftMotor.setFTerm(k_Fleft);
m_rightMotor.setFTerm(k_Fright);
}
public void setPower(double leftPower, double rightPower) {
m_leftMotor.setControlMode(SmartMotorMode.Power);
m_rightMotor.setControlMode(SmartMotorMode.Power);
m_rightMotor.set(rightPower);
m_leftMotor.set(leftPower);
}
public void setSpeed(double leftSpeed, double rightSpeed) {
m_leftMotor.setControlMode(SmartMotorMode.Speed);
m_rightMotor.setControlMode(SmartMotorMode.Speed);
m_leftMotor.set(leftSpeed);
m_rightMotor.set(rightSpeed);
}
public Encoder getLeftEncoder() {
return (m_leftEncoder.Clone());
}
public Encoder getRightEncoder() {
return (m_rightEncoder.Clone());
}
@Override
public void periodic() {
// This method will be called once per scheduler run
Logger.Log("DriveSubsystem", -1, "periodic()");
}
}
Next we want to add a new command that we can use to test out our speed control. Create a new class under the commands folder called TestSpeedControlCommand and, as before, copy over the code from ExampleCommand, renaming the the file name, and replacing ExampleCommand and ExampleSubsystem as you have done before.
Now for this test, we are simply going to run the robot at a constant speed and log the results so that we can see how close to that speed we come. Since we will be logging the motor speeds, we will need access to the left and right encoders so we declare member variables to hold those values and initialize them in the constructor:
public class TestSpeedControlCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private final double k_speed = 0.75;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
/**
* Creates a new TestSpeedControlCommand.
*
* @param subsystem The subsystem used by this command.
*/
public TestSpeedControlCommand(DriveSubsystem subsystem) {
Logger.Log("TestSpeedControlCommand", 3, "TestSpeedControlCommand()");
m_subsystem = subsystem;
m_leftEncoder = m_subsystem.getLeftEncoder();
m_rightEncoder = m_subsystem.getRightEncoder();
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
In the initialize() function we will simply use our new setSpeed(…) function to start the robot forward at 0.75 speed. Since we will also be logging the speed, we log the title row for our CSV file.
public void initialize() {
Logger.Log("TestSpeedControlCommand", 2, "initialize()");
Logger.Log("TestSpeedControlCommand", 3, "...,Target Speed,Left Speed, Right Speed");
m_subsystem.setSpeed(k_speed, k_speed);
}
And we define k_speed as:
private final double k_speed = 0.75;
In the execute() function we want to log the current time, power, and speeds of the two motors.
public void execute() {
Logger.Log("TestSpeedControlCommand", -1, "execute()");
Logger.Log("TestSpeedControl", 3, String.format(",%.0f,%d,%d", k_speed * DriveSubsystem.k_maxSpeed,
m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));
}
Note that we are also logging the target speed and we need to multiply k_speed by the k_maxSpeed defined in the DriveSubsystem so that the units will match those returned by the encoder’s getSpeed() function.
Now for this command we are going to make it run as long as we hold down button 4 on the joystick. Since the command will never end on it’s own, we want to always return false from the isFinished() function.
When we release button 4 on the joystick, this command will be interrupted and it’s end() function will be called. At that point we need to turn the motors back off.
public void end(boolean interrupted) {
Logger.Log("TestSpeedControlCommand", 2, String.format("end(%b)", interrupted));
m_subsystem.setPower(0, 0);
}
Your TestSpeedControlCommand.java 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 TestSpeedControlCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private final double k_speed = 0.75;
private Encoder m_leftEncoder;
private Encoder m_rightEncoder;
/**
* Creates a new TestSpeedControlCommand.
*
* @param subsystem The subsystem used by this command.
*/
public TestSpeedControlCommand(DriveSubsystem subsystem) {
Logger.Log("TestSpeedControlCommand", 3, "TestSpeedControlCommand()");
m_subsystem = subsystem;
m_leftEncoder = m_subsystem.getLeftEncoder();
m_rightEncoder = m_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("TestSpeedControlCommand", 2, "initialize()");
Logger.Log("TestSpeedControlCommand", 3, "...,Target Speed,Left Speed, Right Speed");
m_subsystem.setSpeed(k_speed, k_speed);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("TestSpeedControlCommand", -1, "execute()");
Logger.Log("TestSpeedControl", 3, String.format(",%.0f,%d,%d", k_speed * DriveSubsystem.k_maxSpeed,
m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
Logger.Log("TestSpeedControlCommand", 2, String.format("end(%b)", interrupted));
m_subsystem.setPower(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
Logger.Log("TestSpeedControlCommand", -1, "isFinished()");
return false;
}
}
Now connect our test button 4 to our new TestSpeedControlCommand by adding a m_button4 to RobotContainer:
public class RobotContainer {
// The robot's subsystems and commands are defined here...
@SuppressWarnings("unused")
private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
private final Joystick m_joystick = new Joystick(0);
private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);
private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);
And configuring it so that our new command will run while the button his held down:
private void configureButtonBindings() {
m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));
m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
m_button4.whileHeld(new TestSpeedControlCommand(m_driveSubsystem));
}
Your RobotContainer.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;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import robot.commands.ArcadeDriveCommand;
import robot.commands.DriveForDistanceCommand;
import robot.commands.DriveForTimeCommand;
import robot.commands.ExampleCommand;
import robot.commands.TestMotorSpeedCommand;
import robot.commands.TestSpeedControlCommand;
import robot.subsystems.DriveSubsystem;
import robot.subsystems.ExampleSubsystem;
import robotCore.Joystick;
/**
* This class is where the bulk of the robot should be declared. Since
* Command-based is a "declarative" paradigm, very little robot logic should
* actually be handled in the {@link Robot} periodic methods (other than the
* scheduler calls). Instead, the structure of the robot (including subsystems,
* commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
@SuppressWarnings("unused")
private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
private final Joystick m_joystick = new Joystick(0);
private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);
private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);
private final ExampleCommand m_autoCommand = null; // new ExampleCommand(m_exampleSubsystem);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick));
// Configure the button bindings
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));
m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
m_button4.whileHeld(new TestSpeedControlCommand(m_driveSubsystem));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
return m_autoCommand;
}
}
Now run your program, and connect and enable your robot. Then, as before, clear the TERMINAL window in VS Code. Then switch to the Driver Station and hold the B4 button down for a few seconds. Go back to VS Code and copy all of the text in the TERMINAL window and paste it into LibreOffice Calc. As before, delete all of the rows up to the header line. Now select the Target Speed, Left Speed, and Right Speed columns and plot the data. This time, however, choose the Line / Lines Only option as shown below:
This should result in a graph which looks something like this:
The blue line represents the target speed and the red and yellow lines are the actual speeds of the motors. We can see from the graph that the motor speeds are too low so we will need to increase the F term for both motors. To get an approximation of how much we need to change them we can multiply the current value by the ratio of the target speed to the actual speed. We can then multiply that by the current F values to get new estimated F values. We can see that the target speed is 1125 and for both motors the current speed is around 900. This means we need to multiply the current F values by 1125/900 or 1.25. Performing this calculation gives us 0.0083 for the left motor and 0.0074 for the right motor:
private static final double k_Fleft = 0.00083; // Use the correct number for your robot private static final double k_Fright = 0.00074; // Use the correct number for your robot
Remember that these numbers are for this particular robot only. Your robot may behave slightly differently and you should adjust your number accordingly.
Now run your program again and copy the data to LibreOffice Calc and you should get a graph something like:
This is better, but still a little high so we are going to reduce the F term a little bit. Let’s try 0.008 for the left motor and 0.007 for the right:
private static final double k_Fleft = 0.0008; // Use the correct number for your robot private static final double k_Fright = 0.0007; // Use the correct number for your robot
With these numbers we get the graph:
Now this is much better. Note that we do not need this to be perfect. In fact it is pretty much impossible to get it perfect using only the F term, that is what the other terms are for. But do notice that even now the robot is driving a lot straighter than it was before.
Now that we have the F term established we are going to work on the P term. The P stands for proportional. What this means is that power is added or subtracted depending on how far off the actual speed is from the target speed. Determining the first guess for the P term is somewhat difficult so we are just going to try something and see how it works. Let’s try 0.005 (in this case we won’t need to use different values for the two motors).
To set the P term we add the following to the constructor:
m_leftMotor.setPTerm(k_P);
m_rightMotor.setPTerm(k_P);
And defining k_P as follows:
private static final double k_P = 0.0005; // Use the correct number for your robot
Using this value, we get the following graph:
We can see that this graph already looks better than the previous one that did not have a P term. Now what we want is the largest value of P that does not make the robot unstable. If P is too large, the robot will start to oscillate and we don’t want that. So what we are going to do is keep increasing the value of P until we see it become unstable, and then back off.
If we try doubling P value to 0.001 we will get the following graph:
Since this still looks good, let’s double the P value again to 0.002. When we do this and run the program we can actually hear the robot struggling. Looking at the graph we can see that there is some oscillation going on:
We can clearly see that our ears did not deceive us and we have some undesirable oscillation going on. This means that a value of 0.002 is too high and we should go back to 0.001.
Now adding the P term helps a lot but if the robot were to encounter some additional resistance (such as going up a ramp) it would not be able to completely compensate. For that we need the I term.
The I stands for integral. What the PID controller does is run in a loop. Each time through the loop it computes an error term which is the difference between the set point and the actual speed. The P term then multiplies that error term to make it’s adjustment. However, the PID loop also keeps track of a total error, and each time through the loop the current error term is added to the total error. The I term is then multiplied by the total error and that is added to the power equation. So, lets say, that the speed is too low because the robot is going up a ramp. As long as the speed is too slow, the total error will continue to increase which will cause the power applied to continue to increase until there is enough power to maintain the correct speed.
Once again choosing an initial value for the I term is difficult and we will have to make a guess. Let’s start with 0.0006. We set the I term like this:
m_leftMotor.setITerm(k_I);
m_rightMotor.setITerm(k_I);
Defining k_I as follows:
private static final double k_I = 0.0005; // Use the correct number for your robot
There is one last thing we need to do before we run our program. Using the I term is great but really only works when our speed is close to the target value. When we first start the robot, it takes some time for the robot to get up to speed and during that time we build up a large total error term. This then causes us to overshoot our target. Eventually it settles down to the correct value but we don’t want the overshoot. The way to prevent this is to only use the I term when the current speed is close to the target speed. We set this range by using the setIZone() function. Now we need to set the zone large enough that it encompasses the natural oscillation of the motors. Looking at the graphs we can see that this range is around 150. Set the I Zone as follows:
m_leftMotor.setIZone(k_IZone);
m_rightMotor.setIZone(k_IZone);
Defining k_IZone as:
private static final double k_IZone = 150;
Now if we run the program we get the following graph:
This looks pretty good. Now we want to do the same thing we did with the P term and keep increasing the value until the robot becomes unstable. If we double the I term to 0.001 we get:
We can see that this still looks pretty good, although there does seem to be a little more variation of the speed of the motors, particularly the right motor. Let’s try 0.002 and see what we get:
We can see the speed variation is even larger. Indeed we can actually hear the increase instability when we run the robot. Therefore we will back down and settle on 0.001 for our I term.
Your DriveSubsystem.java file should now look something 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.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;
public class DriveSubsystem extends SubsystemBase {
private static final int k_rightMotorPWMPin = 5;
private static final int k_rightMotorDirPin = 4;
private static final int k_leftMotorPWMPin = 6;
private static final int k_leftMotorDirPin = 7;
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;
public static final int k_maxSpeed = 1500; // Use the correct number for your robot
private static final double k_Fleft = 0.0008; // Use the correct number for your robot
private static final double k_Fright = 0.0007; // Use the correct number for your robot
private static final double k_P = 0.0010; // Use the correct number for your robot
private static final double k_I = 0.001; // Use the correct number for your robot
private static final double k_IZone = 150;
private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
private PWMMotor m_leftMotor = 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);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
Logger.Log("DriveSubsystem", 3, "DriveSubsystem()");
m_leftEncoder.setInverted(true);
m_leftMotor.setFeedbackDevice(m_leftEncoder);
m_rightMotor.setFeedbackDevice(m_rightEncoder);
m_leftMotor.setMaxSpeed(k_maxSpeed);
m_rightMotor.setMaxSpeed(k_maxSpeed);
m_leftMotor.setFTerm(k_Fleft);
m_rightMotor.setFTerm(k_Fright);
m_leftMotor.setPTerm(k_P);
m_rightMotor.setPTerm(k_P);
m_leftMotor.setITerm(k_I);
m_rightMotor.setITerm(k_I);
m_leftMotor.setIZone(k_IZone);
m_rightMotor.setIZone(k_IZone);
}
public void setPower(double leftPower, double rightPower) {
m_leftMotor.setControlMode(SmartMotorMode.Power);
m_rightMotor.setControlMode(SmartMotorMode.Power);
m_rightMotor.set(rightPower);
m_leftMotor.set(leftPower);
}
public void setSpeed(double leftSpeed, double rightSpeed) {
m_leftMotor.setControlMode(SmartMotorMode.Speed);
m_rightMotor.setControlMode(SmartMotorMode.Speed);
m_leftMotor.set(leftSpeed);
m_rightMotor.set(rightSpeed);
}
public Encoder getLeftEncoder() {
return (m_leftEncoder.Clone());
}
public Encoder getRightEncoder() {
return (m_rightEncoder.Clone());
}
@Override
public void periodic() {
// This method will be called once per scheduler run
Logger.Log("DriveSubsystem", -1, "periodic()");
}
}
Remember that the actual values for the F, I, and P terms for your robot may differ from the ones shown here.
Before we wrap up, let’s see how it behaves at another speed. Change the k_speed constant in TestSpeedControlCommand make a graph for 0.5 speed. You should see something like:
We can see that it looks pretty good. It does seem to be taking longer to get up to the correct speed and we might want to consider increasing the I and/or P terms to improve it but, the fact is, no single set of PID terms is going to work for all speeds. When tuning your robot you should tune it for the speed that is most important. You should then check other speeds and adjust the PID parameters only if there is a serious problem.
Let’s make one last change before we move on. Change the call to setPower in the ArcadeDrive() function to setSpeed. This will allow us to control the robot by speed even during teleop which can make it easier to control your robot, especially if the two motors run at different speeds for the same power setting. Your ArcadeDriveCommand.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.Joystick;
import robotCore.Logger;
/**
* An example command that uses an example subsystem.
*/
public class ArcadeDriveCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
private final Joystick m_joystick;
/**
* Creates a new ArcadeDriveCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ArcadeDriveCommand(DriveSubsystem subsystem, Joystick joystick) {
Logger.Log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");
m_subsystem = subsystem;
m_joystick = joystick;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
Logger.Log("ArcadeDriveCommand", 2, "initialize()");
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
double x = m_joystick.getX();
x = x * x * x;
y = y * y * y;
m_subsystem.setSpeed(y + x, y - x);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
Logger.Log("ArcadeDriveCommand", 2, String.format("end(%b)", interrupted));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
Logger.Log("ArcadeDriveCommand", -1, "isFinished()");
return false;
}
}