Step 3: Creating a Drive Subsystem
Now that our drive is characterized, it is time to start writing our robot code proper. As mentioned before, we will use the command-based framework for our robot code. Accordingly, our first step is to write a suitable drive subsystem class.
The full drive class from the RamseteCommand Example Project (Java, C++) can be seen below. The rest of the article will describe the steps involved in writing this class.
5package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;
6
7import edu.wpi.first.math.geometry.Pose2d;
8import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
9import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
10import edu.wpi.first.wpilibj.ADXRS450_Gyro;
11import edu.wpi.first.wpilibj.Encoder;
12import edu.wpi.first.wpilibj.drive.DifferentialDrive;
13import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
14import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
15import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
16import edu.wpi.first.wpilibj2.command.SubsystemBase;
17
18public class DriveSubsystem extends SubsystemBase {
19 // The motors on the left side of the drive.
20 private final MotorControllerGroup m_leftMotors =
21 new MotorControllerGroup(
22 new PWMSparkMax(DriveConstants.kLeftMotor1Port),
23 new PWMSparkMax(DriveConstants.kLeftMotor2Port));
24
25 // The motors on the right side of the drive.
26 private final MotorControllerGroup m_rightMotors =
27 new MotorControllerGroup(
28 new PWMSparkMax(DriveConstants.kRightMotor1Port),
29 new PWMSparkMax(DriveConstants.kRightMotor2Port));
30
31 // The robot's drive
32 private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
33
34 // The left-side drive encoder
35 private final Encoder m_leftEncoder =
36 new Encoder(
37 DriveConstants.kLeftEncoderPorts[0],
38 DriveConstants.kLeftEncoderPorts[1],
39 DriveConstants.kLeftEncoderReversed);
40
41 // The right-side drive encoder
42 private final Encoder m_rightEncoder =
43 new Encoder(
44 DriveConstants.kRightEncoderPorts[0],
45 DriveConstants.kRightEncoderPorts[1],
46 DriveConstants.kRightEncoderReversed);
47
48 // The gyro sensor
49 private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
50
51 // Odometry class for tracking robot pose
52 private final DifferentialDriveOdometry m_odometry;
53
54 /** Creates a new DriveSubsystem. */
55 public DriveSubsystem() {
56 // We need to invert one side of the drivetrain so that positive voltages
57 // result in both sides moving forward. Depending on how your robot's
58 // gearbox is constructed, you might have to invert the left side instead.
59 m_rightMotors.setInverted(true);
60
61 // Sets the distance per pulse for the encoders
62 m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
63 m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
64
65 resetEncoders();
66 m_odometry =
67 new DifferentialDriveOdometry(
68 m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
69 }
70
71 @Override
72 public void periodic() {
73 // Update the odometry in the periodic block
74 m_odometry.update(
75 m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
76 }
77
78 /**
79 * Returns the currently-estimated pose of the robot.
80 *
81 * @return The pose.
82 */
83 public Pose2d getPose() {
84 return m_odometry.getPoseMeters();
85 }
86
87 /**
88 * Returns the current wheel speeds of the robot.
89 *
90 * @return The current wheel speeds.
91 */
92 public DifferentialDriveWheelSpeeds getWheelSpeeds() {
93 return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
94 }
95
96 /**
97 * Resets the odometry to the specified pose.
98 *
99 * @param pose The pose to which to set the odometry.
100 */
101 public void resetOdometry(Pose2d pose) {
102 resetEncoders();
103 m_odometry.resetPosition(
104 m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
105 }
106
107 /**
108 * Drives the robot using arcade controls.
109 *
110 * @param fwd the commanded forward movement
111 * @param rot the commanded rotation
112 */
113 public void arcadeDrive(double fwd, double rot) {
114 m_drive.arcadeDrive(fwd, rot);
115 }
116
117 /**
118 * Controls the left and right sides of the drive directly with voltages.
119 *
120 * @param leftVolts the commanded left output
121 * @param rightVolts the commanded right output
122 */
123 public void tankDriveVolts(double leftVolts, double rightVolts) {
124 m_leftMotors.setVoltage(leftVolts);
125 m_rightMotors.setVoltage(rightVolts);
126 m_drive.feed();
127 }
128
129 /** Resets the drive encoders to currently read a position of 0. */
130 public void resetEncoders() {
131 m_leftEncoder.reset();
132 m_rightEncoder.reset();
133 }
134
135 /**
136 * Gets the average distance of the two encoders.
137 *
138 * @return the average of the two encoder readings
139 */
140 public double getAverageEncoderDistance() {
141 return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
142 }
143
144 /**
145 * Gets the left drive encoder.
146 *
147 * @return the left drive encoder
148 */
149 public Encoder getLeftEncoder() {
150 return m_leftEncoder;
151 }
152
153 /**
154 * Gets the right drive encoder.
155 *
156 * @return the right drive encoder
157 */
158 public Encoder getRightEncoder() {
159 return m_rightEncoder;
160 }
161
162 /**
163 * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
164 *
165 * @param maxOutput the maximum output to which the drive will be constrained
166 */
167 public void setMaxOutput(double maxOutput) {
168 m_drive.setMaxOutput(maxOutput);
169 }
170
171 /** Zeroes the heading of the robot. */
172 public void zeroHeading() {
173 m_gyro.reset();
174 }
175
176 /**
177 * Returns the heading of the robot.
178 *
179 * @return the robot's heading in degrees, from -180 to 180
180 */
181 public double getHeading() {
182 return m_gyro.getRotation2d().getDegrees();
183 }
184
185 /**
186 * Returns the turn rate of the robot.
187 *
188 * @return The turn rate of the robot, in degrees per second
189 */
190 public double getTurnRate() {
191 return -m_gyro.getRate();
192 }
193}
5#pragma once
6
7#include <frc/ADXRS450_Gyro.h>
8#include <frc/Encoder.h>
9#include <frc/drive/DifferentialDrive.h>
10#include <frc/geometry/Pose2d.h>
11#include <frc/kinematics/DifferentialDriveOdometry.h>
12#include <frc/motorcontrol/MotorControllerGroup.h>
13#include <frc/motorcontrol/PWMSparkMax.h>
14#include <frc2/command/SubsystemBase.h>
15#include <units/voltage.h>
16
17#include "Constants.h"
18
19class DriveSubsystem : public frc2::SubsystemBase {
20 public:
21 DriveSubsystem();
22
23 /**
24 * Will be called periodically whenever the CommandScheduler runs.
25 */
26 void Periodic() override;
27
28 // Subsystem methods go here.
29
30 /**
31 * Drives the robot using arcade controls.
32 *
33 * @param fwd the commanded forward movement
34 * @param rot the commanded rotation
35 */
36 void ArcadeDrive(double fwd, double rot);
37
38 /**
39 * Controls each side of the drive directly with a voltage.
40 *
41 * @param left the commanded left output
42 * @param right the commanded right output
43 */
44 void TankDriveVolts(units::volt_t left, units::volt_t right);
45
46 /**
47 * Resets the drive encoders to currently read a position of 0.
48 */
49 void ResetEncoders();
50
51 /**
52 * Gets the average distance of the TWO encoders.
53 *
54 * @return the average of the TWO encoder readings
55 */
56 double GetAverageEncoderDistance();
57
58 /**
59 * Gets the left drive encoder.
60 *
61 * @return the left drive encoder
62 */
63 frc::Encoder& GetLeftEncoder();
64
65 /**
66 * Gets the right drive encoder.
67 *
68 * @return the right drive encoder
69 */
70 frc::Encoder& GetRightEncoder();
71
72 /**
73 * Sets the max output of the drive. Useful for scaling the drive to drive
74 * more slowly.
75 *
76 * @param maxOutput the maximum output to which the drive will be constrained
77 */
78 void SetMaxOutput(double maxOutput);
79
80 /**
81 * Returns the heading of the robot.
82 *
83 * @return the robot's heading in degrees, from -180 to 180
84 */
85 units::degree_t GetHeading() const;
86
87 /**
88 * Returns the turn rate of the robot.
89 *
90 * @return The turn rate of the robot, in degrees per second
91 */
92 double GetTurnRate();
93
94 /**
95 * Returns the currently-estimated pose of the robot.
96 *
97 * @return The pose.
98 */
99 frc::Pose2d GetPose();
100
101 /**
102 * Returns the current wheel speeds of the robot.
103 *
104 * @return The current wheel speeds.
105 */
106 frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
107
108 /**
109 * Resets the odometry to the specified pose.
110 *
111 * @param pose The pose to which to set the odometry.
112 */
113 void ResetOdometry(frc::Pose2d pose);
114
115 private:
116 // Components (e.g. motor controllers and sensors) should generally be
117 // declared private and exposed only through public methods.
118
119 // The motor controllers
120 frc::PWMSparkMax m_left1;
121 frc::PWMSparkMax m_left2;
122 frc::PWMSparkMax m_right1;
123 frc::PWMSparkMax m_right2;
124
125 // The motors on the left side of the drive
126 frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
127
128 // The motors on the right side of the drive
129 frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
130
131 // The robot's drive
132 frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
133
134 // The left-side drive encoder
135 frc::Encoder m_leftEncoder;
136
137 // The right-side drive encoder
138 frc::Encoder m_rightEncoder;
139
140 // The gyro sensor
141 frc::ADXRS450_Gyro m_gyro;
142
143 // Odometry class for tracking robot pose
144 frc::DifferentialDriveOdometry m_odometry;
145};
5#include "subsystems/DriveSubsystem.h"
6
7#include <frc/geometry/Rotation2d.h>
8#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
9
10using namespace DriveConstants;
11
12DriveSubsystem::DriveSubsystem()
13 : m_left1{kLeftMotor1Port},
14 m_left2{kLeftMotor2Port},
15 m_right1{kRightMotor1Port},
16 m_right2{kRightMotor2Port},
17 m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
18 m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
19 m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
20 // We need to invert one side of the drivetrain so that positive voltages
21 // result in both sides moving forward. Depending on how your robot's
22 // gearbox is constructed, you might have to invert the left side instead.
23 m_rightMotors.SetInverted(true);
24
25 // Set the distance per pulse for the encoders
26 m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
27 m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
28
29 ResetEncoders();
30}
31
32void DriveSubsystem::Periodic() {
33 // Implementation of subsystem periodic method goes here.
34 m_odometry.Update(m_gyro.GetRotation2d(),
35 units::meter_t{m_leftEncoder.GetDistance()},
36 units::meter_t{m_rightEncoder.GetDistance()});
37}
38
39void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
40 m_drive.ArcadeDrive(fwd, rot);
41}
42
43void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
44 m_leftMotors.SetVoltage(left);
45 m_rightMotors.SetVoltage(right);
46 m_drive.Feed();
47}
48
49void DriveSubsystem::ResetEncoders() {
50 m_leftEncoder.Reset();
51 m_rightEncoder.Reset();
52}
53
54double DriveSubsystem::GetAverageEncoderDistance() {
55 return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
56}
57
58frc::Encoder& DriveSubsystem::GetLeftEncoder() {
59 return m_leftEncoder;
60}
61
62frc::Encoder& DriveSubsystem::GetRightEncoder() {
63 return m_rightEncoder;
64}
65
66void DriveSubsystem::SetMaxOutput(double maxOutput) {
67 m_drive.SetMaxOutput(maxOutput);
68}
69
70units::degree_t DriveSubsystem::GetHeading() const {
71 return m_gyro.GetRotation2d().Degrees();
72}
73
74double DriveSubsystem::GetTurnRate() {
75 return -m_gyro.GetRate();
76}
77
78frc::Pose2d DriveSubsystem::GetPose() {
79 return m_odometry.GetPose();
80}
81
82frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
83 return {units::meters_per_second_t{m_leftEncoder.GetRate()},
84 units::meters_per_second_t{m_rightEncoder.GetRate()}};
85}
86
87void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
88 ResetEncoders();
89 m_odometry.ResetPosition(m_gyro.GetRotation2d(),
90 units::meter_t{m_leftEncoder.GetDistance()},
91 units::meter_t{m_rightEncoder.GetDistance()}, pose);
92}
Configuring the Drive Encoders
The drive encoders measure the rotation of the wheels on each side of the drive. To properly configure the encoders, we need to specify two things: the ports the encoders are plugged into, and the distance per encoder pulse. Then, we need to write methods allowing access to the encoder values from code that uses the subsystem.
Encoder Ports
The encoder ports are specified in the encoder’s constructor, like so:
34 // The left-side drive encoder
35 private final Encoder m_leftEncoder =
36 new Encoder(
37 DriveConstants.kLeftEncoderPorts[0],
38 DriveConstants.kLeftEncoderPorts[1],
39 DriveConstants.kLeftEncoderReversed);
40
41 // The right-side drive encoder
42 private final Encoder m_rightEncoder =
43 new Encoder(
44 DriveConstants.kRightEncoderPorts[0],
45 DriveConstants.kRightEncoderPorts[1],
46 DriveConstants.kRightEncoderReversed);
17 m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
18 m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
Encoder Distance per Pulse
The distance per pulse is specified by calling the encoder’s setDistancePerPulse method. Note that for the WPILib Encoder class, «pulse» refers to a full encoder cycle (i.e. four edges), and thus will be 1/4 the value that was specified in the SysId config. Remember, as well, that the distance should be measured in meters!
62 m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
63 m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
26 m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
27 m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
Encoder Accessor Method
To access the values measured by the encoders, we include the following method:
Importante
The returned velocities must be in meters! Because we configured the distance per pulse on the encoders above, calling getRate() will automatically apply the conversion factor from encoder units to meters. If you are not using WPILib’s Encoder class, you must perform this conversion either through the respective vendor’s API or by manually multiplying by a conversion factor.
87 /**
88 * Returns the current wheel speeds of the robot.
89 *
90 * @return The current wheel speeds.
91 */
92 public DifferentialDriveWheelSpeeds getWheelSpeeds() {
93 return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
94 }
82frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
83 return {units::meters_per_second_t{m_leftEncoder.GetRate()},
84 units::meters_per_second_t{m_rightEncoder.GetRate()}};
85}
We wrap the measured encoder values in a DifferentialDriveWheelSpeeds object for easier integration with the RamseteCommand class later on.
Configuring the Gyroscope
The gyroscope measures the rate of change of the robot’s heading (which can then be integrated to provide a measurement of the robot’s heading relative to when it first turned on). In our example, we use the Analog Devices ADXRS450 FRC Gyro Board, which has been included in the kit of parts for several years:
49 private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
140 // The gyro sensor
141 frc::ADXRS450_Gyro m_gyro;
Gyroscope Accessor Method
To access the current heading measured by the gyroscope, we include the following method:
176 /**
177 * Returns the heading of the robot.
178 *
179 * @return the robot's heading in degrees, from -180 to 180
180 */
181 public double getHeading() {
182 return m_gyro.getRotation2d().getDegrees();
183 }
70units::degree_t DriveSubsystem::GetHeading() const {
71 return m_gyro.GetRotation2d().Degrees();
72}
Configuring the Odometry
Now that we have our encoders and gyroscope configured, it is time to set up our drive subsystem to automatically compute its position from the encoder and gyroscope readings.
First, we create a member instance of the DifferentialDriveOdometry class:
51 // Odometry class for tracking robot pose
52 private final DifferentialDriveOdometry m_odometry;
143 // Odometry class for tracking robot pose
144 frc::DifferentialDriveOdometry m_odometry;
Updating the Odometry
The odometry class must be regularly updated to incorporate new readings from the encoder and gyroscope. We accomplish this inside the subsystem’s periodic method, which is automatically called once per main loop iteration:
71 @Override
72 public void periodic() {
73 // Update the odometry in the periodic block
74 m_odometry.update(
75 m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
76 }
32void DriveSubsystem::Periodic() {
33 // Implementation of subsystem periodic method goes here.
34 m_odometry.Update(m_gyro.GetRotation2d(),
35 units::meter_t{m_leftEncoder.GetDistance()},
36 units::meter_t{m_rightEncoder.GetDistance()});
37}
Odometry Accessor Method
To access the robot’s current computed pose, we include the following method:
78 /**
79 * Returns the currently-estimated pose of the robot.
80 *
81 * @return The pose.
82 */
83 public Pose2d getPose() {
84 return m_odometry.getPoseMeters();
85 }
78frc::Pose2d DriveSubsystem::GetPose() {
79 return m_odometry.GetPose();
80}
Importante
Before running a RamseteCommand, teams are strongly encouraged to deploy and test the odometry code alone, with values sent to the SmartDashboard or Shuffleboard during the DriveSubsystem’s periodic(). This odometry must be correct for a RamseteCommand to successfully work, as sign or unit errors can cause a robot to move at high speeds in unpredictable directions.
Voltage-Based Drive Method
Finally, we must include one additional method - a method that allows us to set the voltage to each side of the drive using the setVoltage() method of the MotorController interface. The default WPILib drive class does not include this functionality, so we must write it ourselves:
117 /**
118 * Controls the left and right sides of the drive directly with voltages.
119 *
120 * @param leftVolts the commanded left output
121 * @param rightVolts the commanded right output
122 */
123 public void tankDriveVolts(double leftVolts, double rightVolts) {
124 m_leftMotors.setVoltage(leftVolts);
125 m_rightMotors.setVoltage(rightVolts);
126 m_drive.feed();
127 }
43void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
44 m_leftMotors.SetVoltage(left);
45 m_rightMotors.SetVoltage(right);
46 m_drive.Feed();
47}
It is very important to use the setVoltage() method rather than the ordinary set() method, as this will automatically compensate for battery «voltage sag» during operation. Since our feedforward voltages are physically-meaningful (as they are based on measured identification data), this is essential to ensuring their accuracy.
Aviso
RamseteCommand itself does not internally enforce any speed or acceleration limits before providing motor voltage parameters to this method. During initial code development, teams are strongly encouraged to apply both maximum and minimum bounds on the input variables before passing these values to setVoltage() while ensuring the trajectory velocity and acceleration are achievable. For example, generate a trajectory with a little less than half of the Robot’s maximum velocity and limit voltage to 6 volts.