It is time to create the ArcadeDrive command which will allow you to drive the robot using a joystick or XBox controller. As you might guess, the ArcadeDrive command for a swerve robot is a fair bit more complicated that that of the Minibot, but, fear not, as most of the heavy lifting is provided by wpilib.
We are going to start by using the following drive function which is provided by wpilib:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
/** * Method to drive the robot using joystick info. * * @param xSpeed Speed of the robot in the x direction (forward). * @param ySpeed Speed of the robot in the y direction (sideways). * @param rot Angular rate of the robot. * @param fieldRelative Whether the provided x and y speeds are relative to the * field. */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { SwerveModuleState[] swerveModuleStates = m_kinematics.toSwerveModuleStates( ChassisSpeeds.discretize( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( xSpeed, ySpeed, rot, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeed, ySpeed, rot), periodSeconds)); setModuleStates(swerveModuleStates); } |
This takes as input the desired speed in the x and y directions (in meters/sec) and the desired angular rate of rotation (in radians/sec) and sets the module states to get the robot to drive and rotate as required. This function also takes a boolean fieldRelative which specifies whether the speeds are relative to the current orientation of the robot or absolute with respect to the field.
Now to make this work we are going to need to implement a number of things. The first thing we see is that it uses a variable m_kinematics which is an instance of SwerveDriveKinematics and is declared like this:
|
1 2 3 4 5 6 7 |
private final Translation2d m_frontLeftLocation = new Translation2d(0.05842, 0.05842); private final Translation2d m_backLeftLocation = new Translation2d(-0.05842, 0.05842); private final Translation2d m_backRightLocation = new Translation2d(-0.05842, -0.05842); private final Translation2d m_frontRightLocation = new Translation2d(0.05842, -0.05842); private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( m_frontLeftLocation, m_backLeftLocation, m_backRightLocation, m_frontRightLocation); |
The constructor for this class takes the physical positions (in meters) of the swerve wheels with respect to the center of the robot. Pay close attention to the order of these parameters. There will be a number of places where you will see a list such as this and the order must always be the same.
Now in order to move relative to the field, we will need to know the current orientation of the robot. To do this we will need to add a Gyro:
|
1 |
private final Gyro m_gyro = new Gyro(); |
Note that, for the moment, we will be computing the robot’s orientation solely from the gyro. Later, when we add a camera, we will want to incorporate the camera information to correct for gyro drift and this call will need to be changed. Also be sure to reset the gyro at startup by adding this to the DriveSubsystem constructor:
|
1 |
m_gyro.reset(0); |
Note that once you create an instance of the gyro, there will be a gyro initialization process that happens on startup. The output to the console will look like:
|
1 2 3 4 5 6 7 8 9 10 |
3655:Robot(3):robotInit() 3699:ExampleSubsystem(3):ExampleSubsystem() 3704:Gyro(1):Wait for initialization... 4709:Gyro(1):Waiting for gyro: state = 0 5713:Gyro(1):Waiting for gyro: state = 0 6716:Gyro(1):Waiting for gyro: state = 0 7720:Gyro(1):Waiting for gyro: state = 0 8724:Gyro(1):Waiting for gyro: state = 0 9729:Gyro(1):Waiting for gyro: state = 1 11511:DriveSubsystem(3):DriveSubsystem() |
You will need to wait until the initialization is complete before you can connect to the robot. Occasionally this initialization will fail and you will see a state of -1:
|
1 2 3 4 5 6 |
3406:ExampleSubsystem(3):ExampleSubsystem() 3410:Gyro(1):Wait for initialization... 4412:Gyro(1):Waiting for gyro: state = 0 5416:Gyro(1):Waiting for gyro: state = -1 5416:Gyro(1):Init failed, retrying... 6420:Gyro(1):Waiting for gyro: state = 0 |
The system will try and retry the initialization but this always fails so if this happens you need to stop your program and restart it.
Now the function toSwerveModuleStates performs the complicated calculations required to set the position and velocity of all of the four wheels in order to move in the desired way. The output of this function is an array of SwerveModuleState with one value for each of the wheels. Note that the order of these states will be the same as the order you used when you created the SwerveDriveKinematics instance.
We then need to create a function that will apply these states to all four wheels:
|
1 2 3 4 5 6 7 |
private void setModuleStates(SwerveModuleState[] swerveModuleStates) { SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, k_maxDriveSpeedMetersPerSecond); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_backLeft.setDesiredState(swerveModuleStates[1]); m_backRight.setDesiredState(swerveModuleStates[2]); m_frontRight.setDesiredState(swerveModuleStates[3]); } |
The call to desaturateWheelSpeeds guarantees that we don’t try and run any of the wheels faster than they can go. We then call setDesiredState for each of the modules. Once again note the order of these. Naturally we need to implement setDesiredState in our SwerveModule class:
|
1 2 3 4 5 6 7 8 9 10 11 12 |
public void setDesiredState(SwerveModuleState desiredState) { Rotation2d encoderRotation = Rotation2d.fromDegrees(getSteeringPosition()); // Optimize the reference state to avoid spinning further than 90 degrees SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation); state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos(); setDriveSpeedInMetersPerSecond(state.speedMetersPerSecond); setSteeringPosition(state.angle.getDegrees()); } |
Note the call to optimize. This function optimizes the angle and speed by reversing the direction and adding 180 degrees if the amount the wheel must be rotated exceeds 90 degrees. This guarantees that the maximum amount the wheel will have to rotate is 90 degrees.
ArcadeDrive
We are now ready to create the ArcadeDrive command. It is a little complicated so I am going to give you the entire command and then I will discuss the details:
|
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 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 |
/*----------------------------------------------------------------------------*/ /* 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 frc.robot.commands; import java.util.function.DoubleSupplier; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; import robotCore.RobotCoreBase; /** * An example command that uses an example subsystem. */ public class ArcadeDrive extends Command { private final DriveSubsystem m_subsystem; private final DoubleSupplier m_x; private final DoubleSupplier m_y; private final DoubleSupplier m_turn; private final Boolean m_fieldRelative; private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3); private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3); private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); /** * Creates a new ArcadeDrive. * * @param subsystem The subsystem used by this command. */ public ArcadeDrive(DriveSubsystem subsystem, DoubleSupplier x, DoubleSupplier y, DoubleSupplier turn, boolean fieldRelative) { Logger.log("ArcadeDrive", 3, "ArcadeDrive()"); m_subsystem = subsystem; m_x = x; m_y = y; m_turn = turn; m_fieldRelative = fieldRelative; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { Logger.log("ArcadeDrive", 2, "initialize()"); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Logger.log("ArcadeDrive", -1, "execute()"); double x = m_x.getAsDouble(); double y = m_y.getAsDouble(); double turn = m_turn.getAsDouble(); // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. final var xSpeed = -m_xspeedLimiter.calculate(MathUtil.applyDeadband(y, 0.02)) * DriveSubsystem.k_maxDriveSpeedMetersPerSecond; // Get the y speed or sideways/strafe speed. We are inverting this because // we want a positive value when we pull to the left. Xbox controllers // return positive values when you pull to the right by default. final var ySpeed = -m_yspeedLimiter.calculate(MathUtil.applyDeadband(x, 0.02)) * DriveSubsystem.k_maxDriveSpeedMetersPerSecond; // Get the rate of angular rotation. We are inverting this because we want a // positive value when we pull to the left (remember, CCW is positive in // mathematics). Xbox controllers return positive values when you pull to // the right by default. double rot = -m_rotLimiter.calculate(MathUtil.applyDeadband(turn, 0.20)) * DriveSubsystem.k_maxAngularSpeed; if ((xSpeed != 0) || (ySpeed != 0) || (rot != 0)) { m_subsystem.drive(xSpeed, ySpeed, rot, m_fieldRelative, ((TimedRobot) RobotCoreBase.getInstance()).getPeriod()); } else { m_subsystem.stop(); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { Logger.log("ArcadeDrive", 2, String.format("end(%b)", interrupted)); } // Returns true when the command should end. @Override public boolean isFinished() { Logger.log("ArcadeDrive", -1, "isFinished()"); return false; } } |
For the constructor we pass in the DriveSubsystem, of course, but we also pass in double suppliers which provide the joystick x, y, and rotation values. These will be direct values from the joystick so they will all range from -1 to +1.
The execute function is where everything happens. We first obtain the current values for the joystick x, y, and rotation settings. We then call the function applyDeadband which will ignore the input if it is within the specified dead zone. I am using 0.02 for the x and y and 0.20 for the rotation. I am using the larger value for the rotation because I am using the twist function (i.e. Z) of the Joystick (as opposed to an XBox controller) and it is very difficult not to slightly twist the joystick when moving the robot and I don’t want it to turn unless I twist it significantly.
We then take this result and pass it into SlewRateLimiter to limit the rate at which the values can change.
This result still represents a value from -1 to +1 and we need to convert this to meters/sec for the x and y and radians/sec for the rotation. We do this by multiplying them by the appropriate conversion factor k_maxDriveSpeedMetersPerSecond or k_maxAngularSpeed. Note that we have chosen 2 radians/sec for the maximum angular speed. I believe that the robot can actually turn faster than that (I have not actually measured it) but turning faster than that makes the robot more difficult to control. You can, of course, change this number to suit your needs.
We are then ready to call the drive function we created in the DriveSubsystem. When testing this I found when the input was all zero, the motors were a bit twitchy so when the inputs are all zero, I stop all the motors instead.
Maintain orientation
You probably will find that as you drive the robot around it’s orientation is not maintained (even when you are not rotating it with the joystick). This is because the precision of the motors and sensors are not perfect. This is particularly apparent when using these cheap motors and 3d printed gears, but you will find the same behavior with the larger FRC robots.
You can fix this issues by using the gyro to maintain the current orientation. The basic idea is that when you are not commanding the robot to rotate, you record the current heading. Then as it moves you measure the difference between the current gyro heading and the desired heading. Then you automatically add a rotate vector that is proportional to the difference between the desired heading and the current heading. This will cause the robot to automatically correct itself. You should create a constant that multiplies the heading difference (e.g. k_p) and adjust it as you would tune the P of a PID loop so that it turns to the correct heading as fast as possible without the robot oscellating.
The implementation of this feature is left as an exercise for the reader.