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.math.system.plant;
006
007import edu.wpi.first.math.system.plant.proto.DCMotorProto;
008import edu.wpi.first.math.system.plant.struct.DCMotorStruct;
009import edu.wpi.first.math.util.Units;
010import edu.wpi.first.util.protobuf.ProtobufSerializable;
011import edu.wpi.first.util.struct.StructSerializable;
012
013/** Holds the constants for a DC motor. */
014public class DCMotor implements ProtobufSerializable, StructSerializable {
015  /** Voltage at which the motor constants were measured. */
016  public final double nominalVoltage;
017
018  /** Torque when stalled in Newton-meters. */
019  public final double stallTorque;
020
021  /** Current draw when stalled in amps. */
022  public final double stallCurrent;
023
024  /** Current draw under no load in amps. */
025  public final double freeCurrent;
026
027  /** Angular velocity under no load in radians per second. */
028  public final double freeSpeed;
029
030  /** Motor internal resistance in Ohms. */
031  public final double R;
032
033  /** Motor velocity constant in (rad/s)/V. */
034  public final double Kv;
035
036  /** Motor torque constant in Newton-meters per amp. */
037  public final double Kt;
038
039  /** DCMotor protobuf for serialization. */
040  public static final DCMotorProto proto = new DCMotorProto();
041
042  /** DCMotor struct for serialization. */
043  public static final DCMotorStruct struct = new DCMotorStruct();
044
045  /**
046   * Constructs a DC motor.
047   *
048   * @param nominalVoltage Voltage at which the motor constants were measured.
049   * @param stallTorque Torque when stalled.
050   * @param stallCurrent Current draw when stalled.
051   * @param freeCurrent Current draw under no load.
052   * @param freeSpeed Angular velocity under no load.
053   * @param numMotors Number of motors in a gearbox.
054   */
055  public DCMotor(
056      double nominalVoltage,
057      double stallTorque,
058      double stallCurrent,
059      double freeCurrent,
060      double freeSpeed,
061      int numMotors) {
062    this.nominalVoltage = nominalVoltage;
063    this.stallTorque = stallTorque * numMotors;
064    this.stallCurrent = stallCurrent * numMotors;
065    this.freeCurrent = freeCurrent * numMotors;
066    this.freeSpeed = freeSpeed;
067
068    this.R = nominalVoltage / this.stallCurrent;
069    this.Kv = freeSpeed / (nominalVoltage - R * this.freeCurrent);
070    this.Kt = this.stallTorque / this.stallCurrent;
071  }
072
073  /**
074   * Calculate current drawn by motor with given speed and input voltage.
075   *
076   * @param speed The current angular velocity of the motor.
077   * @param voltageInput The voltage being applied to the motor.
078   * @return The estimated current.
079   */
080  public double getCurrent(double speed, double voltageInput) {
081    return -1.0 / Kv / R * speed + 1.0 / R * voltageInput;
082  }
083
084  /**
085   * Calculate current drawn by motor for a given torque.
086   *
087   * @param torque The torque produced by the motor in Newton-meters.
088   * @return The current drawn by the motor.
089   */
090  public double getCurrent(double torque) {
091    return torque / Kt;
092  }
093
094  /**
095   * Calculate torque produced by the motor with a given current.
096   *
097   * @param current The current drawn by the motor in amps.
098   * @return The torque output in Newton-meters.
099   */
100  public double getTorque(double current) {
101    return current * Kt;
102  }
103
104  /**
105   * Calculate the voltage provided to the motor for a given torque and angular velocity.
106   *
107   * @param torque The torque produced by the motor in Newton-meters.
108   * @param speed The current angular velocity of the motor in radians per second.
109   * @return The voltage of the motor.
110   */
111  public double getVoltage(double torque, double speed) {
112    return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
113  }
114
115  /**
116   * Calculates the angular speed produced by the motor at a given torque and input voltage.
117   *
118   * @param torque The torque produced by the motor in Newton-meters.
119   * @param voltageInput The voltage applied to the motor.
120   * @return The angular speed of the motor.
121   */
122  public double getSpeed(double torque, double voltageInput) {
123    return voltageInput * Kv - 1.0 / Kt * torque * R * Kv;
124  }
125
126  /**
127   * Returns a copy of this motor with the given gearbox reduction applied.
128   *
129   * @param gearboxReduction The gearbox reduction.
130   * @return A motor with the gearbox reduction applied.
131   */
132  public DCMotor withReduction(double gearboxReduction) {
133    return new DCMotor(
134        nominalVoltage,
135        stallTorque * gearboxReduction,
136        stallCurrent,
137        freeCurrent,
138        freeSpeed / gearboxReduction,
139        1);
140  }
141
142  /**
143   * Return a gearbox of CIM motors.
144   *
145   * @param numMotors Number of motors in the gearbox.
146   * @return A gearbox of CIM motors.
147   */
148  public static DCMotor getCIM(int numMotors) {
149    return new DCMotor(
150        12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
151  }
152
153  /**
154   * Return a gearbox of 775Pro motors.
155   *
156   * @param numMotors Number of motors in the gearbox.
157   * @return A gearbox of 775Pro motors.
158   */
159  public static DCMotor getVex775Pro(int numMotors) {
160    return new DCMotor(
161        12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
162  }
163
164  /**
165   * Return a gearbox of NEO motors.
166   *
167   * @param numMotors Number of motors in the gearbox.
168   * @return A gearbox of NEO motors.
169   */
170  public static DCMotor getNEO(int numMotors) {
171    return new DCMotor(
172        12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
173  }
174
175  /**
176   * Return a gearbox of MiniCIM motors.
177   *
178   * @param numMotors Number of motors in the gearbox.
179   * @return A gearbox of MiniCIM motors.
180   */
181  public static DCMotor getMiniCIM(int numMotors) {
182    return new DCMotor(
183        12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
184  }
185
186  /**
187   * Return a gearbox of Bag motors.
188   *
189   * @param numMotors Number of motors in the gearbox.
190   * @return A gearbox of Bag motors.
191   */
192  public static DCMotor getBag(int numMotors) {
193    return new DCMotor(
194        12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
195  }
196
197  /**
198   * Return a gearbox of Andymark RS775-125 motors.
199   *
200   * @param numMotors Number of motors in the gearbox.
201   * @return A gearbox of Andymark RS775-125 motors.
202   */
203  public static DCMotor getAndymarkRs775_125(int numMotors) {
204    return new DCMotor(
205        12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
206  }
207
208  /**
209   * Return a gearbox of Banebots RS775 motors.
210   *
211   * @param numMotors Number of motors in the gearbox.
212   * @return A gearbox of Banebots RS775 motors.
213   */
214  public static DCMotor getBanebotsRs775(int numMotors) {
215    return new DCMotor(
216        12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
217  }
218
219  /**
220   * Return a gearbox of Andymark 9015 motors.
221   *
222   * @param numMotors Number of motors in the gearbox.
223   * @return A gearbox of Andymark 9015 motors.
224   */
225  public static DCMotor getAndymark9015(int numMotors) {
226    return new DCMotor(
227        12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
228  }
229
230  /**
231   * Return a gearbox of Banebots RS 550 motors.
232   *
233   * @param numMotors Number of motors in the gearbox.
234   * @return A gearbox of Banebots RS 550 motors.
235   */
236  public static DCMotor getBanebotsRs550(int numMotors) {
237    return new DCMotor(
238        12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
239  }
240
241  /**
242   * Return a gearbox of NEO 550 motors.
243   *
244   * @param numMotors Number of motors in the gearbox.
245   * @return A gearbox of NEO 550 motors.
246   */
247  public static DCMotor getNeo550(int numMotors) {
248    return new DCMotor(
249        12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
250  }
251
252  /**
253   * Return a gearbox of Falcon 500 motors.
254   *
255   * @param numMotors Number of motors in the gearbox.
256   * @return A gearbox of Falcon 500 motors.
257   */
258  public static DCMotor getFalcon500(int numMotors) {
259    return new DCMotor(
260        12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
261  }
262
263  /**
264   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control) enabled.
265   *
266   * @param numMotors Number of motors in the gearbox.
267   * @return A gearbox of Falcon 500 FOC enabled motors.
268   */
269  public static DCMotor getFalcon500Foc(int numMotors) {
270    // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
271    return new DCMotor(
272        12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors);
273  }
274
275  /**
276   * Return a gearbox of Romi/TI_RSLK MAX motors.
277   *
278   * @param numMotors Number of motors in the gearbox.
279   * @return A gearbox of Romi/TI_RSLK MAX motors.
280   */
281  public static DCMotor getRomiBuiltIn(int numMotors) {
282    // From https://www.pololu.com/product/1520/specs
283    return new DCMotor(
284        4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
285  }
286
287  /**
288   * Return a gearbox of Kraken X60 brushless motors.
289   *
290   * @param numMotors Number of motors in the gearbox.
291   * @return a gearbox of Kraken X60 motors.
292   */
293  public static DCMotor getKrakenX60(int numMotors) {
294    // From https://store.ctr-electronics.com/announcing-kraken-x60/
295    return new DCMotor(
296        12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(6000), numMotors);
297  }
298
299  /**
300   * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented Control) enabled.
301   *
302   * @param numMotors Number of motors in the gearbox.
303   * @return A gearbox of Kraken X60 FOC enabled motors.
304   */
305  public static DCMotor getKrakenX60Foc(int numMotors) {
306    // From https://store.ctr-electronics.com/announcing-kraken-x60/
307    return new DCMotor(
308        12, 9.37, 483, 2, Units.rotationsPerMinuteToRadiansPerSecond(5800), numMotors);
309  }
310
311  /**
312   * Return a gearbox of Neo Vortex brushless motors.
313   *
314   * @param numMotors Number of motors in the gearbox.
315   * @return a gearbox of Neo Vortex motors.
316   */
317  public static DCMotor getNeoVortex(int numMotors) {
318    // From https://www.revrobotics.com/next-generation-spark-neo/
319    return new DCMotor(
320        12, 3.60, 211, 3.6, Units.rotationsPerMinuteToRadiansPerSecond(6784), numMotors);
321  }
322}