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.simulation; 006 007import edu.wpi.first.math.VecBuilder; 008import edu.wpi.first.math.numbers.N1; 009import edu.wpi.first.math.numbers.N2; 010import edu.wpi.first.math.system.LinearSystem; 011import edu.wpi.first.math.system.plant.DCMotor; 012import edu.wpi.first.math.system.plant.LinearSystemId; 013import edu.wpi.first.math.util.Units; 014import edu.wpi.first.wpilibj.RobotController; 015 016/** Represents a simulated DC motor mechanism. */ 017public class DCMotorSim extends LinearSystemSim<N2, N1, N2> { 018 // Gearbox for the DC motor. 019 private final DCMotor m_gearbox; 020 021 // The gearing from the motors to the output. 022 private final double m_gearing; 023 024 /** 025 * Creates a simulated DC motor mechanism. 026 * 027 * @param plant The linear system representing the DC motor. This system can be created with 028 * @param gearbox The type of and number of motors in the DC motor gearbox. 029 * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). 030 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 031 * noise is desired. If present must have 2 elements. The first element is for position. The 032 * second element is for velocity. 033 */ 034 public DCMotorSim( 035 LinearSystem<N2, N1, N2> plant, 036 DCMotor gearbox, 037 double gearing, 038 double... measurementStdDevs) { 039 super(plant, measurementStdDevs); 040 m_gearbox = gearbox; 041 m_gearing = gearing; 042 } 043 044 /** 045 * Creates a simulated DC motor mechanism. 046 * 047 * @param gearbox The type of and number of motors in the DC motor gearbox. 048 * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). 049 * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the 050 * {@link #DCMotorSim(LinearSystem, DCMotor, double, double...)} constructor. 051 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 052 * noise is desired. If present must have 2 elements. The first element is for position. The 053 * second element is for velocity. 054 */ 055 public DCMotorSim( 056 DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) { 057 super( 058 LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs); 059 m_gearbox = gearbox; 060 m_gearing = gearing; 061 } 062 063 /** 064 * Sets the state of the DC motor. 065 * 066 * @param angularPositionRad The new position in radians. 067 * @param angularVelocityRadPerSec The new velocity in radians per second. 068 */ 069 public void setState(double angularPositionRad, double angularVelocityRadPerSec) { 070 setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec)); 071 } 072 073 /** 074 * Returns the DC motor position. 075 * 076 * @return The DC motor position. 077 */ 078 public double getAngularPositionRad() { 079 return getOutput(0); 080 } 081 082 /** 083 * Returns the DC motor position in rotations. 084 * 085 * @return The DC motor position in rotations. 086 */ 087 public double getAngularPositionRotations() { 088 return Units.radiansToRotations(getAngularPositionRad()); 089 } 090 091 /** 092 * Returns the DC motor velocity. 093 * 094 * @return The DC motor velocity. 095 */ 096 public double getAngularVelocityRadPerSec() { 097 return getOutput(1); 098 } 099 100 /** 101 * Returns the DC motor velocity in RPM. 102 * 103 * @return The DC motor velocity in RPM. 104 */ 105 public double getAngularVelocityRPM() { 106 return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); 107 } 108 109 /** 110 * Returns the DC motor current draw. 111 * 112 * @return The DC motor current draw. 113 */ 114 public double getCurrentDrawAmps() { 115 // I = V / R - omega / (Kv * R) 116 // Reductions are output over input, so a reduction of 2:1 means the motor is spinning 117 // 2x faster than the output 118 return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0)) 119 * Math.signum(m_u.get(0, 0)); 120 } 121 122 /** 123 * Sets the input voltage for the DC motor. 124 * 125 * @param volts The input voltage. 126 */ 127 public void setInputVoltage(double volts) { 128 setInput(volts); 129 clampInput(RobotController.getBatteryVoltage()); 130 } 131}