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 java.util.Objects.requireNonNull;
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.SpeedController;
017
018/**
019 * A class for driving Killough drive platforms.
020 *
021 * <p>Killough drives are triangular with one omni wheel on each corner.
022 *
023 * <p>Drive base diagram:
024 *
025 * <pre>
026 *  /_____\
027 * / \   / \
028 *    \ /
029 *    ---
030 * </pre>
031 *
032 * <p>Each drive() function provides different inverse kinematic relations for a Killough drive. The
033 * default wheel vectors are parallel to their respective opposite sides, but can be overridden. See
034 * the constructor for more information.
035 *
036 * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
037 * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
038 *
039 * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
040 * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
041 * positive.
042 */
043@SuppressWarnings("removal")
044public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable {
045  public static final double kDefaultLeftMotorAngle = 60.0;
046  public static final double kDefaultRightMotorAngle = 120.0;
047  public static final double kDefaultBackMotorAngle = 270.0;
048
049  private static int instances;
050
051  private SpeedController m_leftMotor;
052  private SpeedController m_rightMotor;
053  private SpeedController m_backMotor;
054
055  private Vector2d m_leftVec;
056  private Vector2d m_rightVec;
057  private Vector2d m_backVec;
058
059  private boolean m_reported;
060
061  @SuppressWarnings("MemberName")
062  public static class WheelSpeeds {
063    public double left;
064    public double right;
065    public double back;
066
067    /** Constructs a WheelSpeeds with zeroes for left, right, and back speeds. */
068    public WheelSpeeds() {}
069
070    /**
071     * Constructs a WheelSpeeds.
072     *
073     * @param left The left speed.
074     * @param right The right speed.
075     * @param back The back speed.
076     */
077    public WheelSpeeds(double left, double right, double back) {
078      this.left = left;
079      this.right = right;
080      this.back = back;
081    }
082  }
083
084  /**
085   * Construct a Killough drive with the given motors and default motor angles.
086   *
087   * <p>The default motor angles make the wheels on each corner parallel to their respective
088   * opposite sides.
089   *
090   * <p>If a motor needs to be inverted, do so before passing it in.
091   *
092   * @param leftMotor The motor on the left corner.
093   * @param rightMotor The motor on the right corner.
094   * @param backMotor The motor on the back corner.
095   */
096  public KilloughDrive(
097      SpeedController leftMotor, SpeedController rightMotor, SpeedController backMotor) {
098    this(
099        leftMotor,
100        rightMotor,
101        backMotor,
102        kDefaultLeftMotorAngle,
103        kDefaultRightMotorAngle,
104        kDefaultBackMotorAngle);
105  }
106
107  /**
108   * Construct a Killough drive with the given motors.
109   *
110   * <p>Angles are measured in degrees clockwise from the positive X axis.
111   *
112   * @param leftMotor The motor on the left corner.
113   * @param rightMotor The motor on the right corner.
114   * @param backMotor The motor on the back corner.
115   * @param leftMotorAngle The angle of the left wheel's forward direction of travel.
116   * @param rightMotorAngle The angle of the right wheel's forward direction of travel.
117   * @param backMotorAngle The angle of the back wheel's forward direction of travel.
118   */
119  public KilloughDrive(
120      SpeedController leftMotor,
121      SpeedController rightMotor,
122      SpeedController backMotor,
123      double leftMotorAngle,
124      double rightMotorAngle,
125      double backMotorAngle) {
126    requireNonNull(leftMotor, "Left motor cannot be null");
127    requireNonNull(rightMotor, "Right motor cannot be null");
128    requireNonNull(backMotor, "Back motor cannot be null");
129
130    m_leftMotor = leftMotor;
131    m_rightMotor = rightMotor;
132    m_backMotor = backMotor;
133    m_leftVec =
134        new Vector2d(
135            Math.cos(leftMotorAngle * (Math.PI / 180.0)),
136            Math.sin(leftMotorAngle * (Math.PI / 180.0)));
137    m_rightVec =
138        new Vector2d(
139            Math.cos(rightMotorAngle * (Math.PI / 180.0)),
140            Math.sin(rightMotorAngle * (Math.PI / 180.0)));
141    m_backVec =
142        new Vector2d(
143            Math.cos(backMotorAngle * (Math.PI / 180.0)),
144            Math.sin(backMotorAngle * (Math.PI / 180.0)));
145    SendableRegistry.addChild(this, m_leftMotor);
146    SendableRegistry.addChild(this, m_rightMotor);
147    SendableRegistry.addChild(this, m_backMotor);
148    instances++;
149    SendableRegistry.addLW(this, "KilloughDrive", instances);
150  }
151
152  @Override
153  public void close() {
154    SendableRegistry.remove(this);
155  }
156
157  /**
158   * Drive method for Killough platform.
159   *
160   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
161   * from its angle or rotation rate.
162   *
163   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
164   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
165   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
166   *     positive.
167   */
168  @SuppressWarnings("ParameterName")
169  public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
170    driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
171  }
172
173  /**
174   * Drive method for Killough platform.
175   *
176   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
177   * from its angle or rotation rate.
178   *
179   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
180   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
181   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
182   *     positive.
183   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
184   *     to implement field-oriented controls.
185   */
186  @SuppressWarnings("ParameterName")
187  public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
188    if (!m_reported) {
189      HAL.report(
190          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughCartesian, 3);
191      m_reported = true;
192    }
193
194    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
195    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
196
197    var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
198
199    m_leftMotor.set(speeds.left * m_maxOutput);
200    m_rightMotor.set(speeds.right * m_maxOutput);
201    m_backMotor.set(speeds.back * m_maxOutput);
202
203    feed();
204  }
205
206  /**
207   * Drive method for Killough platform.
208   *
209   * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
210   * drives (translation) is independent from its angle or rotation rate.
211   *
212   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
213   * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
214   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
215   *     positive.
216   */
217  @SuppressWarnings("ParameterName")
218  public void drivePolar(double magnitude, double angle, double zRotation) {
219    if (!m_reported) {
220      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughPolar, 3);
221      m_reported = true;
222    }
223
224    driveCartesian(
225        magnitude * Math.sin(angle * (Math.PI / 180.0)),
226        magnitude * Math.cos(angle * (Math.PI / 180.0)),
227        zRotation,
228        0.0);
229  }
230
231  /**
232   * Cartesian inverse kinematics for Killough platform.
233   *
234   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
235   * from its angle or rotation rate.
236   *
237   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
238   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
239   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
240   *     positive.
241   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
242   *     to implement field-oriented controls.
243   * @return Wheel speeds.
244   */
245  @SuppressWarnings("ParameterName")
246  public WheelSpeeds driveCartesianIK(
247      double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
248    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
249    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
250
251    // Compensate for gyro angle.
252    Vector2d input = new Vector2d(ySpeed, xSpeed);
253    input.rotate(-gyroAngle);
254
255    double[] wheelSpeeds = new double[3];
256    wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation;
257    wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation;
258    wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation;
259
260    normalize(wheelSpeeds);
261
262    return new WheelSpeeds(
263        wheelSpeeds[MotorType.kLeft.value],
264        wheelSpeeds[MotorType.kRight.value],
265        wheelSpeeds[MotorType.kBack.value]);
266  }
267
268  @Override
269  public void stopMotor() {
270    m_leftMotor.stopMotor();
271    m_rightMotor.stopMotor();
272    m_backMotor.stopMotor();
273    feed();
274  }
275
276  @Override
277  public String getDescription() {
278    return "KilloughDrive";
279  }
280
281  @Override
282  public void initSendable(SendableBuilder builder) {
283    builder.setSmartDashboardType("KilloughDrive");
284    builder.setActuator(true);
285    builder.setSafeState(this::stopMotor);
286    builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
287    builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set);
288    builder.addDoubleProperty("Back Motor Speed", m_backMotor::get, m_backMotor::set);
289  }
290}