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.util.Units;
008
009/** Holds the constants for a DC motor. */
010public class DCMotor {
011  public final double nominalVoltageVolts;
012  public final double stallTorqueNewtonMeters;
013  public final double stallCurrentAmps;
014  public final double freeCurrentAmps;
015  public final double freeSpeedRadPerSec;
016  public final double rOhms;
017  public final double KvRadPerSecPerVolt;
018  public final double KtNMPerAmp;
019
020  /**
021   * Constructs a DC motor.
022   *
023   * @param nominalVoltageVolts Voltage at which the motor constants were measured.
024   * @param stallTorqueNewtonMeters Current draw when stalled.
025   * @param stallCurrentAmps Current draw when stalled.
026   * @param freeCurrentAmps Current draw under no load.
027   * @param freeSpeedRadPerSec Angular velocity under no load.
028   * @param numMotors Number of motors in a gearbox.
029   */
030  public DCMotor(
031      double nominalVoltageVolts,
032      double stallTorqueNewtonMeters,
033      double stallCurrentAmps,
034      double freeCurrentAmps,
035      double freeSpeedRadPerSec,
036      int numMotors) {
037    this.nominalVoltageVolts = nominalVoltageVolts;
038    this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors;
039    this.stallCurrentAmps = stallCurrentAmps * numMotors;
040    this.freeCurrentAmps = freeCurrentAmps * numMotors;
041    this.freeSpeedRadPerSec = freeSpeedRadPerSec;
042
043    this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
044    this.KvRadPerSecPerVolt =
045        freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
046    this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
047  }
048
049  /**
050   * Estimate the current being drawn by this motor.
051   *
052   * @param speedRadiansPerSec The speed of the motor.
053   * @param voltageInputVolts The input voltage.
054   * @return The estimated current.
055   */
056  public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
057    return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
058  }
059
060  /**
061   * Calculate the torque produced by the motor for a given current.
062   *
063   * @param currentAmpere The current drawn by the motor.
064   * @return The torque produced.
065   */
066  public double getTorque(double currentAmpere) {
067    return currentAmpere * KtNMPerAmp;
068  }
069
070  /**
071   * Calculate the voltage provided to the motor at a given torque and angular velocity.
072   *
073   * @param torqueNm The torque produced by the motor.
074   * @param speedRadiansPerSec The speed of the motor.
075   * @return The voltage of the motor.
076   */
077  public double getVoltage(double torqueNm, double speedRadiansPerSec) {
078    return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm;
079  }
080
081  /**
082   * Calculate the speed of the motor at a given torque and input voltage.
083   *
084   * @param torqueNm The torque produced by the motor.
085   * @param voltageInputVolts The voltage applied to the motor.
086   * @return The speed of the motor.
087   */
088  public double getSpeed(double torqueNm, double voltageInputVolts) {
089    return voltageInputVolts * KvRadPerSecPerVolt
090        - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
091  }
092
093  /**
094   * Returns a copy of this motor with the given gearbox reduction applied.
095   *
096   * @param gearboxReduction The gearbox reduction.
097   * @return A motor with the gearbox reduction applied.
098   */
099  public DCMotor withReduction(double gearboxReduction) {
100    return new DCMotor(
101        nominalVoltageVolts,
102        stallTorqueNewtonMeters * gearboxReduction,
103        stallCurrentAmps,
104        freeCurrentAmps,
105        freeSpeedRadPerSec / gearboxReduction,
106        1);
107  }
108
109  /**
110   * Return a gearbox of CIM motors.
111   *
112   * @param numMotors Number of motors in the gearbox.
113   * @return A gearbox of CIM motors.
114   */
115  public static DCMotor getCIM(int numMotors) {
116    return new DCMotor(
117        12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
118  }
119
120  /**
121   * Return a gearbox of 775Pro motors.
122   *
123   * @param numMotors Number of motors in the gearbox.
124   * @return A gearbox of 775Pro motors.
125   */
126  public static DCMotor getVex775Pro(int numMotors) {
127    return new DCMotor(
128        12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
129  }
130
131  /**
132   * Return a gearbox of NEO motors.
133   *
134   * @param numMotors Number of motors in the gearbox.
135   * @return A gearbox of NEO motors.
136   */
137  public static DCMotor getNEO(int numMotors) {
138    return new DCMotor(
139        12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
140  }
141
142  /**
143   * Return a gearbox of MiniCIM motors.
144   *
145   * @param numMotors Number of motors in the gearbox.
146   * @return A gearbox of MiniCIM motors.
147   */
148  public static DCMotor getMiniCIM(int numMotors) {
149    return new DCMotor(
150        12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
151  }
152
153  /**
154   * Return a gearbox of Bag motors.
155   *
156   * @param numMotors Number of motors in the gearbox.
157   * @return A gearbox of Bag motors.
158   */
159  public static DCMotor getBag(int numMotors) {
160    return new DCMotor(
161        12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
162  }
163
164  /**
165   * Return a gearbox of Andymark RS775-125 motors.
166   *
167   * @param numMotors Number of motors in the gearbox.
168   * @return A gearbox of Andymark RS775-125 motors.
169   */
170  public static DCMotor getAndymarkRs775_125(int numMotors) {
171    return new DCMotor(
172        12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
173  }
174
175  /**
176   * Return a gearbox of Banebots RS775 motors.
177   *
178   * @param numMotors Number of motors in the gearbox.
179   * @return A gearbox of Banebots RS775 motors.
180   */
181  public static DCMotor getBanebotsRs775(int numMotors) {
182    return new DCMotor(
183        12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
184  }
185
186  /**
187   * Return a gearbox of Andymark 9015 motors.
188   *
189   * @param numMotors Number of motors in the gearbox.
190   * @return A gearbox of Andymark 9015 motors.
191   */
192  public static DCMotor getAndymark9015(int numMotors) {
193    return new DCMotor(
194        12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
195  }
196
197  /**
198   * Return a gearbox of Banebots RS 550 motors.
199   *
200   * @param numMotors Number of motors in the gearbox.
201   * @return A gearbox of Banebots RS 550 motors.
202   */
203  public static DCMotor getBanebotsRs550(int numMotors) {
204    return new DCMotor(
205        12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
206  }
207
208  /**
209   * Return a gearbox of NEO 550 motors.
210   *
211   * @param numMotors Number of motors in the gearbox.
212   * @return A gearbox of NEO 550 motors.
213   */
214  public static DCMotor getNeo550(int numMotors) {
215    return new DCMotor(
216        12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
217  }
218
219  /**
220   * Return a gearbox of Falcon 500 motors.
221   *
222   * @param numMotors Number of motors in the gearbox.
223   * @return A gearbox of Falcon 500 motors.
224   */
225  public static DCMotor getFalcon500(int numMotors) {
226    return new DCMotor(
227        12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
228  }
229
230  /**
231   * Return a gearbox of Romi/TI_RSLK MAX motors.
232   *
233   * @param numMotors Number of motors in the gearbox.
234   * @return A gearbox of Romi/TI_RSLK MAX motors.
235   */
236  public static DCMotor getRomiBuiltIn(int numMotors) {
237    // From https://www.pololu.com/product/1520/specs
238    return new DCMotor(
239        4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
240  }
241}