Source code

001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.drive;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.hal.FRCNetComm.tInstances;
010import edu.wpi.first.hal.FRCNetComm.tResourceType;
011import edu.wpi.first.hal.HAL;
012import edu.wpi.first.math.MathUtil;
013import edu.wpi.first.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import edu.wpi.first.wpilibj.motorcontrol.MotorController;
017
018/**
019 * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
020 * base, "tank drive", or West Coast Drive.
021 *
022 * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
023 * (e.g., 6WD or 8WD). This class takes a MotorController per side. For four and six motor
024 * drivetrains, construct and pass in {@link
025 * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup} instances as follows.
026 *
027 * <p>Four motor drivetrain:
028 *
029 * <pre><code>
030 * public class Robot {
031 *   MotorController m_frontLeft = new PWMVictorSPX(1);
032 *   MotorController m_rearLeft = new PWMVictorSPX(2);
033 *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_rearLeft);
034 *
035 *   MotorController m_frontRight = new PWMVictorSPX(3);
036 *   MotorController m_rearRight = new PWMVictorSPX(4);
037 *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_rearRight);
038 *
039 *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
040 * }
041 * </code></pre>
042 *
043 * <p>Six motor drivetrain:
044 *
045 * <pre><code>
046 * public class Robot {
047 *   MotorController m_frontLeft = new PWMVictorSPX(1);
048 *   MotorController m_midLeft = new PWMVictorSPX(2);
049 *   MotorController m_rearLeft = new PWMVictorSPX(3);
050 *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
051 *
052 *   MotorController m_frontRight = new PWMVictorSPX(4);
053 *   MotorController m_midRight = new PWMVictorSPX(5);
054 *   MotorController m_rearRight = new PWMVictorSPX(6);
055 *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_midRight, m_rearRight);
056 *
057 *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
058 * }
059 * </code></pre>
060 *
061 * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
062 *
063 * <p>Drive base diagram:
064 *
065 * <pre>
066 * |_______|
067 * | |   | |
068 *   |   |
069 * |_|___|_|
070 * |       |
071 * </pre>
072 *
073 * <p>Each drive function provides different inverse kinematic relations for a differential drive
074 * robot.
075 *
076 * <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
077 * frame). The positive X axis points ahead, the positive Y axis points to the left, and the
078 * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
079 * around the Z axis is positive.
080 *
081 * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
082 * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
083 * value can be changed with {@link #setDeadband}.
084 *
085 * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The tankDrive, arcadeDrive,
086 * or curvatureDrive methods should be called periodically to avoid Motor Safety timeouts.
087 */
088public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
089  private static int instances;
090
091  private final MotorController m_leftMotor;
092  private final MotorController m_rightMotor;
093
094  private boolean m_reported;
095
096  /**
097   * Wheel speeds for a differential drive.
098   *
099   * <p>Uses normalized voltage [-1.0..1.0].
100   */
101  @SuppressWarnings("MemberName")
102  public static class WheelSpeeds {
103    public double left;
104    public double right;
105
106    /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
107    public WheelSpeeds() {}
108
109    /**
110     * Constructs a WheelSpeeds.
111     *
112     * @param left The left speed [-1.0..1.0].
113     * @param right The right speed [-1.0..1.0].
114     */
115    public WheelSpeeds(double left, double right) {
116      this.left = left;
117      this.right = right;
118    }
119  }
120
121  /**
122   * Construct a DifferentialDrive.
123   *
124   * <p>To pass multiple motors per side, use a {@link
125   * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}. If a motor needs to be inverted, do
126   * so before passing it in.
127   *
128   * @param leftMotor Left motor.
129   * @param rightMotor Right motor.
130   */
131  public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
132    requireNonNullParam(leftMotor, "leftMotor", "DifferentialDrive");
133    requireNonNullParam(rightMotor, "rightMotor", "DifferentialDrive");
134
135    m_leftMotor = leftMotor;
136    m_rightMotor = rightMotor;
137    SendableRegistry.addChild(this, m_leftMotor);
138    SendableRegistry.addChild(this, m_rightMotor);
139    instances++;
140    SendableRegistry.addLW(this, "DifferentialDrive", instances);
141  }
142
143  @Override
144  public void close() {
145    SendableRegistry.remove(this);
146  }
147
148  /**
149   * Arcade drive method for differential drive platform. The calculated values will be squared to
150   * decrease sensitivity at low speeds.
151   *
152   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
153   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
154   *     positive.
155   */
156  public void arcadeDrive(double xSpeed, double zRotation) {
157    arcadeDrive(xSpeed, zRotation, true);
158  }
159
160  /**
161   * Arcade drive method for differential drive platform.
162   *
163   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
164   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
165   *     positive.
166   * @param squareInputs If set, decreases the input sensitivity at low speeds.
167   */
168  public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
169    if (!m_reported) {
170      HAL.report(
171          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialArcade, 2);
172      m_reported = true;
173    }
174
175    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
176    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
177
178    var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
179
180    m_leftMotor.set(speeds.left * m_maxOutput);
181    m_rightMotor.set(speeds.right * m_maxOutput);
182
183    feed();
184  }
185
186  /**
187   * Curvature drive method for differential drive platform.
188   *
189   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
190   * heading change. This makes the robot more controllable at high speeds.
191   *
192   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
193   * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
194   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
195   *     maneuvers. zRotation will control turning rate instead of curvature.
196   */
197  public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
198    if (!m_reported) {
199      HAL.report(
200          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialCurvature, 2);
201      m_reported = true;
202    }
203
204    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
205    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
206
207    var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
208
209    m_leftMotor.set(speeds.left * m_maxOutput);
210    m_rightMotor.set(speeds.right * m_maxOutput);
211
212    feed();
213  }
214
215  /**
216   * Tank drive method for differential drive platform. The calculated values will be squared to
217   * decrease sensitivity at low speeds.
218   *
219   * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is positive.
220   * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
221   *     positive.
222   */
223  public void tankDrive(double leftSpeed, double rightSpeed) {
224    tankDrive(leftSpeed, rightSpeed, true);
225  }
226
227  /**
228   * Tank drive method for differential drive platform.
229   *
230   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
231   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
232   *     positive.
233   * @param squareInputs If set, decreases the input sensitivity at low speeds.
234   */
235  public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
236    if (!m_reported) {
237      HAL.report(
238          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialTank, 2);
239      m_reported = true;
240    }
241
242    leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
243    rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
244
245    var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
246
247    m_leftMotor.set(speeds.left * m_maxOutput);
248    m_rightMotor.set(speeds.right * m_maxOutput);
249
250    feed();
251  }
252
253  /**
254   * Arcade drive inverse kinematics for differential drive platform.
255   *
256   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
257   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
258   *     positive.
259   * @param squareInputs If set, decreases the input sensitivity at low speeds.
260   * @return Wheel speeds [-1.0..1.0].
261   */
262  public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
263    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
264    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
265
266    // Square the inputs (while preserving the sign) to increase fine control
267    // while permitting full power.
268    if (squareInputs) {
269      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
270      zRotation = Math.copySign(zRotation * zRotation, zRotation);
271    }
272
273    double leftSpeed = xSpeed - zRotation;
274    double rightSpeed = xSpeed + zRotation;
275
276    // Find the maximum possible value of (throttle + turn) along the vector
277    // that the joystick is pointing, then desaturate the wheel speeds
278    double greaterInput = Math.max(Math.abs(xSpeed), Math.abs(zRotation));
279    double lesserInput = Math.min(Math.abs(xSpeed), Math.abs(zRotation));
280    if (greaterInput == 0.0) {
281      return new WheelSpeeds(0.0, 0.0);
282    }
283    double saturatedInput = (greaterInput + lesserInput) / greaterInput;
284    leftSpeed /= saturatedInput;
285    rightSpeed /= saturatedInput;
286
287    return new WheelSpeeds(leftSpeed, rightSpeed);
288  }
289
290  /**
291   * Curvature drive inverse kinematics for differential drive platform.
292   *
293   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
294   * heading change. This makes the robot more controllable at high speeds.
295   *
296   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
297   * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
298   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
299   *     maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
300   * @return Wheel speeds [-1.0..1.0].
301   */
302  public static WheelSpeeds curvatureDriveIK(
303      double xSpeed, double zRotation, boolean allowTurnInPlace) {
304    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
305    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
306
307    double leftSpeed;
308    double rightSpeed;
309
310    if (allowTurnInPlace) {
311      leftSpeed = xSpeed - zRotation;
312      rightSpeed = xSpeed + zRotation;
313    } else {
314      leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
315      rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
316    }
317
318    // Desaturate wheel speeds
319    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
320    if (maxMagnitude > 1.0) {
321      leftSpeed /= maxMagnitude;
322      rightSpeed /= maxMagnitude;
323    }
324
325    return new WheelSpeeds(leftSpeed, rightSpeed);
326  }
327
328  /**
329   * Tank drive inverse kinematics for differential drive platform.
330   *
331   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
332   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
333   *     positive.
334   * @param squareInputs If set, decreases the input sensitivity at low speeds.
335   * @return Wheel speeds [-1.0..1.0].
336   */
337  public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
338    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
339    rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
340
341    // Square the inputs (while preserving the sign) to increase fine control
342    // while permitting full power.
343    if (squareInputs) {
344      leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
345      rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
346    }
347
348    return new WheelSpeeds(leftSpeed, rightSpeed);
349  }
350
351  @Override
352  public void stopMotor() {
353    m_leftMotor.stopMotor();
354    m_rightMotor.stopMotor();
355    feed();
356  }
357
358  @Override
359  public String getDescription() {
360    return "DifferentialDrive";
361  }
362
363  @Override
364  public void initSendable(SendableBuilder builder) {
365    builder.setSmartDashboardType("DifferentialDrive");
366    builder.setActuator(true);
367    builder.setSafeState(this::stopMotor);
368    builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
369    builder.addDoubleProperty(
370        "Right Motor Speed", () -> m_rightMotor.get(), x -> m_rightMotor.set(x));
371  }
372}