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.
packageedu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;importedu.wpi.first.wpilibj.ADXRS450_Gyro;importedu.wpi.first.wpilibj.Encoder;importedu.wpi.first.wpilibj.PWMVictorSPX;importedu.wpi.first.wpilibj.SpeedControllerGroup;importedu.wpi.first.wpilibj.drive.DifferentialDrive;importedu.wpi.first.wpilibj.geometry.Pose2d;importedu.wpi.first.wpilibj.interfaces.Gyro;importedu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;importedu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;importedu.wpi.first.wpilibj2.command.SubsystemBase;importedu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;publicclassDriveSubsystemextendsSubsystemBase{// The motors on the left side of the drive.privatefinalSpeedControllerGroupm_leftMotors=newSpeedControllerGroup(newPWMVictorSPX(DriveConstants.kLeftMotor1Port),newPWMVictorSPX(DriveConstants.kLeftMotor2Port));// The motors on the right side of the drive.privatefinalSpeedControllerGroupm_rightMotors=newSpeedControllerGroup(newPWMVictorSPX(DriveConstants.kRightMotor1Port),newPWMVictorSPX(DriveConstants.kRightMotor2Port));// The robot's driveprivatefinalDifferentialDrivem_drive=newDifferentialDrive(m_leftMotors,m_rightMotors);// The left-side drive encoderprivatefinalEncoderm_leftEncoder=newEncoder(DriveConstants.kLeftEncoderPorts[0],DriveConstants.kLeftEncoderPorts[1],DriveConstants.kLeftEncoderReversed);// The right-side drive encoderprivatefinalEncoderm_rightEncoder=newEncoder(DriveConstants.kRightEncoderPorts[0],DriveConstants.kRightEncoderPorts[1],DriveConstants.kRightEncoderReversed);// The gyro sensorprivatefinalGyrom_gyro=newADXRS450_Gyro();// Odometry class for tracking robot poseprivatefinalDifferentialDriveOdometrym_odometry;/** * Creates a new DriveSubsystem. */publicDriveSubsystem(){// Sets the distance per pulse for the encodersm_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);resetEncoders();m_odometry=newDifferentialDriveOdometry(m_gyro.getRotation2d());}@Overridepublicvoidperiodic(){// Update the odometry in the periodic blockm_odometry.update(m_gyro.getRotation2d(),m_leftEncoder.getDistance(),m_rightEncoder.getDistance());}/** * Returns the currently-estimated pose of the robot. * * @return The pose. */publicPose2dgetPose(){returnm_odometry.getPoseMeters();}/** * Returns the current wheel speeds of the robot. * * @return The current wheel speeds. */publicDifferentialDriveWheelSpeedsgetWheelSpeeds(){returnnewDifferentialDriveWheelSpeeds(m_leftEncoder.getRate(),m_rightEncoder.getRate());}/** * Resets the odometry to the specified pose. * * @param pose The pose to which to set the odometry. */publicvoidresetOdometry(Pose2dpose){resetEncoders();m_odometry.resetPosition(pose,m_gyro.getRotation2d());}/** * Drives the robot using arcade controls. * * @param fwd the commanded forward movement * @param rot the commanded rotation */publicvoidarcadeDrive(doublefwd,doublerot){m_drive.arcadeDrive(fwd,rot);}/** * Controls the left and right sides of the drive directly with voltages. * * @param leftVolts the commanded left output * @param rightVolts the commanded right output */publicvoidtankDriveVolts(doubleleftVolts,doublerightVolts){m_leftMotors.setVoltage(leftVolts);m_rightMotors.setVoltage(-rightVolts);m_drive.feed();}/** * Resets the drive encoders to currently read a position of 0. */publicvoidresetEncoders(){m_leftEncoder.reset();m_rightEncoder.reset();}/** * Gets the average distance of the two encoders. * * @return the average of the two encoder readings */publicdoublegetAverageEncoderDistance(){return(m_leftEncoder.getDistance()+m_rightEncoder.getDistance())/2.0;}/** * Gets the left drive encoder. * * @return the left drive encoder */publicEncodergetLeftEncoder(){returnm_leftEncoder;}/** * Gets the right drive encoder. * * @return the right drive encoder */publicEncodergetRightEncoder(){returnm_rightEncoder;}/** * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. * * @param maxOutput the maximum output to which the drive will be constrained */publicvoidsetMaxOutput(doublemaxOutput){m_drive.setMaxOutput(maxOutput);}/** * Zeroes the heading of the robot. */publicvoidzeroHeading(){m_gyro.reset();}/** * Returns the heading of the robot. * * @return the robot's heading in degrees, from -180 to 180 */publicdoublegetHeading(){returnm_gyro.getRotation2d().getDegrees();}/** * Returns the turn rate of the robot. * * @return The turn rate of the robot, in degrees per second */publicdoublegetTurnRate(){return-m_gyro.getRate();}}
#pragma once#include<frc/ADXRS450_Gyro.h>#include<frc/Encoder.h>#include<frc/PWMVictorSPX.h>#include<frc/SpeedControllerGroup.h>#include<frc/drive/DifferentialDrive.h>#include<frc/geometry/Pose2d.h>#include<frc/kinematics/DifferentialDriveOdometry.h>#include<frc2/command/SubsystemBase.h>#include<units/voltage.h>#include"Constants.h"classDriveSubsystem:publicfrc2::SubsystemBase{public:DriveSubsystem();/** * Will be called periodically whenever the CommandScheduler runs. */voidPeriodic()override;// Subsystem methods go here./** * Drives the robot using arcade controls. * * @param fwd the commanded forward movement * @param rot the commanded rotation */voidArcadeDrive(doublefwd,doublerot);/** * Controls each side of the drive directly with a voltage. * * @param left the commanded left output * @param right the commanded right output */voidTankDriveVolts(units::volt_tleft,units::volt_tright);/** * Resets the drive encoders to currently read a position of 0. */voidResetEncoders();/** * Gets the average distance of the TWO encoders. * * @return the average of the TWO encoder readings */doubleGetAverageEncoderDistance();/** * Gets the left drive encoder. * * @return the left drive encoder */frc::Encoder&GetLeftEncoder();/** * Gets the right drive encoder. * * @return the right drive encoder */frc::Encoder&GetRightEncoder();/** * Sets the max output of the drive. Useful for scaling the drive to drive * more slowly. * * @param maxOutput the maximum output to which the drive will be constrained */voidSetMaxOutput(doublemaxOutput);/** * Returns the heading of the robot. * * @return the robot's heading in degrees, from -180 to 180 */units::degree_tGetHeading()const;/** * Returns the turn rate of the robot. * * @return The turn rate of the robot, in degrees per second */doubleGetTurnRate();/** * Returns the currently-estimated pose of the robot. * * @return The pose. */frc::Pose2dGetPose();/** * Returns the current wheel speeds of the robot. * * @return The current wheel speeds. */frc::DifferentialDriveWheelSpeedsGetWheelSpeeds();/** * Resets the odometry to the specified pose. * * @param pose The pose to which to set the odometry. */voidResetOdometry(frc::Pose2dpose);private:// Components (e.g. motor controllers and sensors) should generally be// declared private and exposed only through public methods.// The motor controllersfrc::PWMVictorSPXm_left1;frc::PWMVictorSPXm_left2;frc::PWMVictorSPXm_right1;frc::PWMVictorSPXm_right2;// The motors on the left side of the drivefrc::SpeedControllerGroupm_leftMotors{m_left1,m_left2};// The motors on the right side of the drivefrc::SpeedControllerGroupm_rightMotors{m_right1,m_right2};// The robot's drivefrc::DifferentialDrivem_drive{m_leftMotors,m_rightMotors};// The left-side drive encoderfrc::Encoderm_leftEncoder;// The right-side drive encoderfrc::Encoderm_rightEncoder;// The gyro sensorfrc::ADXRS450_Gyrom_gyro;// Odometry class for tracking robot posefrc::DifferentialDriveOdometrym_odometry;};
#include"subsystems/DriveSubsystem.h"#include<frc/geometry/Rotation2d.h>#include<frc/kinematics/DifferentialDriveWheelSpeeds.h>usingnamespaceDriveConstants;DriveSubsystem::DriveSubsystem():m_left1{kLeftMotor1Port},m_left2{kLeftMotor2Port},m_right1{kRightMotor1Port},m_right2{kRightMotor2Port},m_leftEncoder{kLeftEncoderPorts[0],kLeftEncoderPorts[1]},m_rightEncoder{kRightEncoderPorts[0],kRightEncoderPorts[1]},m_odometry{m_gyro.GetRotation2d()}{// Set the distance per pulse for the encodersm_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);ResetEncoders();}voidDriveSubsystem::Periodic(){// Implementation of subsystem periodic method goes here.m_odometry.Update(m_gyro.GetRotation2d(),units::meter_t(m_leftEncoder.GetDistance()),units::meter_t(m_rightEncoder.GetDistance()));}voidDriveSubsystem::ArcadeDrive(doublefwd,doublerot){m_drive.ArcadeDrive(fwd,rot);}voidDriveSubsystem::TankDriveVolts(units::volt_tleft,units::volt_tright){m_leftMotors.SetVoltage(left);m_rightMotors.SetVoltage(-right);m_drive.Feed();}voidDriveSubsystem::ResetEncoders(){m_leftEncoder.Reset();m_rightEncoder.Reset();}doubleDriveSubsystem::GetAverageEncoderDistance(){return(m_leftEncoder.GetDistance()+m_rightEncoder.GetDistance())/2.0;}frc::Encoder&DriveSubsystem::GetLeftEncoder(){returnm_leftEncoder;}frc::Encoder&DriveSubsystem::GetRightEncoder(){returnm_rightEncoder;}voidDriveSubsystem::SetMaxOutput(doublemaxOutput){m_drive.SetMaxOutput(maxOutput);}units::degree_tDriveSubsystem::GetHeading()const{returnm_gyro.GetRotation2d().Degrees();}doubleDriveSubsystem::GetTurnRate(){return-m_gyro.GetRate();}frc::Pose2dDriveSubsystem::GetPose(){returnm_odometry.GetPose();}frc::DifferentialDriveWheelSpeedsDriveSubsystem::GetWheelSpeeds(){return{units::meters_per_second_t(m_leftEncoder.GetRate()),units::meters_per_second_t(m_rightEncoder.GetRate())};}voidDriveSubsystem::ResetOdometry(frc::Pose2dpose){ResetEncoders();m_odometry.ResetPosition(pose,m_gyro.GetRotation2d());}
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.
The encoder ports are specified in the encoder’s constructor, like so:
3839404142434445
privatefinalEncoderm_leftEncoder=newEncoder(DriveConstants.kLeftEncoderPorts[0],DriveConstants.kLeftEncoderPorts[1],DriveConstants.kLeftEncoderReversed);// The right-side drive encoderprivatefinalEncoderm_rightEncoder=newEncoder(DriveConstants.kRightEncoderPorts[0],DriveConstants.kRightEncoderPorts[1],DriveConstants.kRightEncoderReversed);
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 FRC-Characterization config. Remember, as well, that the distance should be measured in meters!
// Set the distance per pulse for the encodersm_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
To access the values measured by the encoders, we include the following method:
Important
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.
8283848586878889
/** * Returns the current wheel speeds of the robot. * * @return The current wheel speeds. */publicDifferentialDriveWheelSpeedsgetWheelSpeeds(){returnnewDifferentialDriveWheelSpeeds(m_leftEncoder.getRate(),m_rightEncoder.getRate());}
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:
To access the current heading measured by the gyroscope, we include the following method:
173174175176177178179180
/** * Returns the heading of the robot. * * @return the robot's heading in degrees, from -180 to 180 */publicdoublegetHeading(){returnm_gyro.getRotation2d().getDegrees();}
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
privatefinalDifferentialDriveOdometrym_odometry;
146147
// Odometry class for tracking robot posefrc::DifferentialDriveOdometrym_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:
6667686970
publicvoidperiodic(){// Update the odometry in the periodic blockm_odometry.update(m_gyro.getRotation2d(),m_leftEncoder.getDistance(),m_rightEncoder.getDistance());}
303132333435
voidDriveSubsystem::Periodic(){// Implementation of subsystem periodic method goes here.m_odometry.Update(m_gyro.GetRotation2d(),units::meter_t(m_leftEncoder.GetDistance()),units::meter_t(m_rightEncoder.GetDistance()));}
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 SpeedController interface. The default WPILib drive class does not include this functionality, so we must write it ourselves:
110111112113114115116117118119120
/** * Controls the left and right sides of the drive directly with voltages. * * @param leftVolts the commanded left output * @param rightVolts the commanded right output */publicvoidtankDriveVolts(doubleleftVolts,doublerightVolts){m_leftMotors.setVoltage(leftVolts);m_rightMotors.setVoltage(-rightVolts);m_drive.feed();}
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 characterization data), this is essential to ensuring their accuracy.