The drive subsystem for a swerve drive robot is considerably more complex than that of the simple tank drive that you created for the Minibot. If you don’t remember how to create a new subsystem, you can review the procedure from the Minibot tutorial here.
The robot’s motion is controlled by eight motors grouped in pairs, with one pair on each of the four corners of the robot. We will refer to each of these pairs of motors as a Swerve Module:
The yellow box in this image shows one of these modules. In particular this is the Swerve Module for the front left corner of the robot. If we view this module from a different angle we can see the two motors which control this wheel:
The motor marked Steering controls the orientation of the wheel. The wheel is free to rotate a full 360 degrees. The motor marked Drive powers the wheel and drives the robot.
SwerveModule class
Since we will have four of these modules for our robot, we need to create a class which we will call SwerveModule. Each instance of this class will control the two motors for that specific module. We will need to create four instances of this class, one for each corner of the robot. This class does not need to be defined as a subsystem, and should just be a simple class something like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
package frc.robot.subsystems; public class SwerveModule { private final PWMMotor m_driveMotor; private final PWMMotor m_steeringMotor; private final Encoder m_driveEncoder; private final Encoder m_steeringEncoder; public SwerveModule(int drivePWM, int driveDir, int driveEncInt, int driveEncDir, int steeringPWM, int steeringDir, int steeringEncA, int steeringEncB, int i2cAddr, String name) { } } |
Initializing motors
Since each Swerve Module will control two motors, we will need the pin numbers for each of the motors. To instantiate the motors you will need something like this:
|
1 2 |
m_driveMotor = new PWMMotor(drivePWM, driveDir, i2cAddr); m_steeringMotor = new PWMMotor(steeringPWM, steeringDir, i2cAddr); |
Note that we are using the constructor which includes the I2C address.
Initializing encoders
In addition, each motor has an associated encoder. For the Drive motor we use a relative Quadrature encoder. for the Steering motor we use an absolute AnalogRotational encoder. Hence we will also need the four pins the configure the two encoders. To instantiate the encoders you will need something like this:
|
1 2 3 |
m_steeringEncoder = new Encoder(robotCore.Encoder.EncoderType.AnalogRotational, steeringEncA, steeringEncB, i2cAddr, true); m_driveEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, driveEncInt, driveEncDir, i2cAddr, true); |
Once again note that we are using the constructor with the I2C address. Also note the last parameter group. If this is set to true then the read of the speed or position for the encoders are grouped at a low level to reduce the time needed to retrieve this information from the motors. In order to have sufficient bandwidth to operate the swerve drive it is important that group be set to true for these two encoders.
Finally, don’t forget to set the feedback device for the two motors:
|
1 2 |
m_driveMotor.setFeedbackDevice(m_driveEncoder); m_steeringMotor.setFeedbackDevice(m_steeringEncoder); |
DriveSubsystem
Finally, when you create the DriveSubsystem, you will need to create four instances of your SwerveModule class. You will, of course, need to know all of the required pin number. Hence your DriveSubsystem class should look something like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 |
package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import robotCore.Device; import robotCore.Logger; public class DriveSubsystem extends SubsystemBase { public static final int FLI2CAddr = 5; public static final int FLDrivePWM = Device.M1_1_PWM; public static final int FLDriveDir = Device.M1_1_DIR; public static final int FLSteeringPWM = Device.M1_2_PWM; public static final int FLSteeringDir = Device.M1_2_DIR; public static final int FLDriveEncInt = Device.Q1_INT; public static final int FLDriveEncDir = Device.Q1_DIR; public static final int FLSteeringEncA = Device.A1_A; public static final int FLSteeringEncB = Device.A1_B; public static final int BLI2CAddr = 5; public static final int BLDrivePWM = Device.M2_1_PWM; public static final int BLDriveDir = Device.M2_1_DIR; private static final int BLSteeringPWM = Device.M2_2_PWM; private static final int BLSteeringDir = Device.M2_2_DIR; public static final int BLDriveEncInt = Device.Q2_INT; public static final int BLDriveEncDir = Device.Q2_DIR; public static final int BLSteeringEncA = Device.A2_A; public static final int BLSteeringEncB = Device.A2_B; public static final int BRI2CAddr = 6; public static final int BRDrivePWM = Device.M1_1_PWM; public static final int BRDriveDir = Device.M1_1_DIR; public static final int BRSteeringPWM = Device.M1_2_PWM; public static final int BRSteeringDir = Device.M1_2_DIR; private static final int BRDriveEncInt = Device.Q1_INT; private static final int BRDriveEncDir = Device.Q1_DIR; private static final int BRSteeringEncA = Device.A1_A; private static final int BRSteeringEncB = Device.A1_B; private static final int FRI2CAddr = 6; private static final int FRDrivePWM = Device.M2_1_PWM; private static final int FRDriveDir = Device.M2_1_DIR; private static final int FRSteeringPWM = Device.M2_2_PWM; private static final int FRSteeringDir = Device.M2_2_DIR; private static final int FRDriveEncInt = Device.Q2_INT; private static final int FRDriveEncDir = Device.Q2_DIR; private static final int FRSteeringEncA = Device.A2_A; private static final int FRSteering`EncB = Device.A2_B; SwerveModule m_frontLeft = new SwerveModule(FLDrivePWM, FLDriveDir, FLDriveEncInt, FLDriveEncDir, FLSteeringPWM, FLSteeringDir, FLSteeringEncA, FLSteeringEncB, FLI2CAddr, "FrontLeft"); SwerveModule m_backLeft = new SwerveModule(BLDrivePWM, BLDriveDir, BLDriveEncInt, BLDriveEncDir, BLSteeringPWM, BLSteeringDir, BLSteeringEncA, BLSteeringEncB, BLI2CAddr, "BackLeft"); SwerveModule m_backRight = new SwerveModule(BRDrivePWM, BRDriveDir, BRDriveEncInt, BRDriveEncDir, BRSteeringPWM, BRSteeringDir, BRSteeringEncA, BRSteeringEncB, BRI2CAddr, "BackRight"); SwerveModule m_frontRight = new SwerveModule(FRDrivePWM, FRDriveDir, FRDriveEncInt, FRDriveEncDir, FRSteeringPWM, FRSteeringDir, FRSteeringEncA, FRSteeringEncB, FRI2CAddr, "FrontRight"); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { Logger.log("DriveSubsystem", 3, "DriveSubsystem()"); } @Override public void periodic() { // This method will be called once per scheduler run Logger.log("DriveSubsystem", -1, "periodic()"); } } |
In the next section we will tune the PID for the Steering motors.