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.motorcontrol; 006 007import edu.wpi.first.hal.SimDevice; 008import edu.wpi.first.hal.SimDevice.Direction; 009import edu.wpi.first.hal.SimDouble; 010import edu.wpi.first.math.MathUtil; 011import edu.wpi.first.util.sendable.Sendable; 012import edu.wpi.first.util.sendable.SendableBuilder; 013import edu.wpi.first.util.sendable.SendableRegistry; 014import edu.wpi.first.wpilibj.MotorSafety; 015import edu.wpi.first.wpilibj.PWM; 016import edu.wpi.first.wpilibj.RobotController; 017import java.util.ArrayList; 018 019/** Common base class for all PWM Motor Controllers. */ 020@SuppressWarnings("removal") 021public abstract class PWMMotorController extends MotorSafety 022 implements MotorController, Sendable, AutoCloseable { 023 private boolean m_isInverted; 024 private final ArrayList<PWMMotorController> m_followers = new ArrayList<>(); 025 026 /** PWM instances for motor controller. */ 027 protected PWM m_pwm; 028 029 private SimDevice m_simDevice; 030 private SimDouble m_simSpeed; 031 032 private boolean m_eliminateDeadband; 033 private int m_minPwm; 034 private int m_deadbandMinPwm; 035 private int m_centerPwm; 036 private int m_deadbandMaxPwm; 037 private int m_maxPwm; 038 039 /** 040 * Constructor. 041 * 042 * @param name Name to use for SendableRegistry 043 * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are 044 * on the MXP port 045 */ 046 @SuppressWarnings("this-escape") 047 protected PWMMotorController(final String name, final int channel) { 048 m_pwm = new PWM(channel, false); 049 SendableRegistry.add(this, name, channel); 050 051 m_simDevice = SimDevice.create("PWMMotorController", channel); 052 if (m_simDevice != null) { 053 m_simSpeed = m_simDevice.createDouble("Speed", Direction.kOutput, 0.0); 054 m_pwm.setSimDevice(m_simDevice); 055 } 056 } 057 058 /** Free the resource associated with the PWM channel and set the value to 0. */ 059 @Override 060 public void close() { 061 SendableRegistry.remove(this); 062 m_pwm.close(); 063 064 if (m_simDevice != null) { 065 m_simDevice.close(); 066 m_simDevice = null; 067 m_simSpeed = null; 068 } 069 } 070 071 private int getMinPositivePwm() { 072 if (m_eliminateDeadband) { 073 return m_deadbandMaxPwm; 074 } else { 075 return m_centerPwm + 1; 076 } 077 } 078 079 private int getMaxNegativePwm() { 080 if (m_eliminateDeadband) { 081 return m_deadbandMinPwm; 082 } else { 083 return m_centerPwm - 1; 084 } 085 } 086 087 private int getPositiveScaleFactor() { 088 return m_maxPwm - getMinPositivePwm(); 089 } 090 091 private int getNegativeScaleFactor() { 092 return getMaxNegativePwm() - m_minPwm; 093 } 094 095 /** 096 * Takes a speed from -1 to 1, and outputs it in the microsecond format. 097 * 098 * @param speed the speed to output 099 */ 100 protected final void setSpeed(double speed) { 101 if (Double.isFinite(speed)) { 102 speed = MathUtil.clamp(speed, -1.0, 1.0); 103 } else { 104 speed = 0.0; 105 } 106 107 if (m_simSpeed != null) { 108 m_simSpeed.set(speed); 109 } 110 111 int rawValue; 112 if (speed == 0.0) { 113 rawValue = m_centerPwm; 114 } else if (speed > 0.0) { 115 rawValue = (int) Math.round(speed * getPositiveScaleFactor()) + getMinPositivePwm(); 116 } else { 117 rawValue = (int) Math.round(speed * getNegativeScaleFactor()) + getMaxNegativePwm(); 118 } 119 120 m_pwm.setPulseTimeMicroseconds(rawValue); 121 } 122 123 /** 124 * Gets the speed from -1 to 1, from the currently set pulse time. 125 * 126 * @return motor controller speed 127 */ 128 protected final double getSpeed() { 129 int rawValue = m_pwm.getPulseTimeMicroseconds(); 130 131 if (rawValue == 0) { 132 return 0.0; 133 } else if (rawValue > m_maxPwm) { 134 return 1.0; 135 } else if (rawValue < m_minPwm) { 136 return -1.0; 137 } else if (rawValue > getMinPositivePwm()) { 138 return (rawValue - getMinPositivePwm()) / (double) getPositiveScaleFactor(); 139 } else if (rawValue < getMaxNegativePwm()) { 140 return (rawValue - getMaxNegativePwm()) / (double) getNegativeScaleFactor(); 141 } else { 142 return 0.0; 143 } 144 } 145 146 /** 147 * Sets the bounds in microseconds for the controller. 148 * 149 * @param maxPwm maximum 150 * @param deadbandMaxPwm deadband max 151 * @param centerPwm center 152 * @param deadbandMinPwm deadmand min 153 * @param minPwm minimum 154 */ 155 protected final void setBoundsMicroseconds( 156 int maxPwm, int deadbandMaxPwm, int centerPwm, int deadbandMinPwm, int minPwm) { 157 m_maxPwm = maxPwm; 158 m_deadbandMaxPwm = deadbandMaxPwm; 159 m_centerPwm = centerPwm; 160 m_deadbandMinPwm = deadbandMinPwm; 161 m_minPwm = minPwm; 162 } 163 164 /** 165 * Set the PWM value. 166 * 167 * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the 168 * FPGA. 169 * 170 * @param speed The speed value between -1.0 and 1.0 to set. 171 */ 172 @Override 173 public void set(double speed) { 174 if (m_isInverted) { 175 speed = -speed; 176 } 177 setSpeed(speed); 178 179 for (var follower : m_followers) { 180 follower.set(speed); 181 } 182 183 feed(); 184 } 185 186 /** 187 * Get the recently set value of the PWM. This value is affected by the inversion property. 188 * 189 * @return The most recently set value for the PWM between -1.0 and 1.0. 190 */ 191 @Override 192 public double get() { 193 return getSpeed() * (m_isInverted ? -1.0 : 1.0); 194 } 195 196 /** 197 * Gets the voltage output of the motor controller, nominally between -12 V and 12 V. 198 * 199 * @return The voltage of the motor controller, nominally between -12 V and 12 V. 200 */ 201 public double getVoltage() { 202 return get() * RobotController.getBatteryVoltage(); 203 } 204 205 @Override 206 public void setInverted(boolean isInverted) { 207 m_isInverted = isInverted; 208 } 209 210 @Override 211 public boolean getInverted() { 212 return m_isInverted; 213 } 214 215 @Override 216 public void disable() { 217 m_pwm.setDisabled(); 218 219 if (m_simSpeed != null) { 220 m_simSpeed.set(0.0); 221 } 222 223 for (var follower : m_followers) { 224 follower.disable(); 225 } 226 } 227 228 @Override 229 public void stopMotor() { 230 // Don't use set(0) as that will feed the watch kitty 231 m_pwm.setPulseTimeMicroseconds(0); 232 233 for (var follower : m_followers) { 234 follower.stopMotor(); 235 } 236 } 237 238 @Override 239 public String getDescription() { 240 return "PWM " + getChannel(); 241 } 242 243 /** 244 * Gets the backing PWM handle. 245 * 246 * @return The pwm handle. 247 */ 248 public int getPwmHandle() { 249 return m_pwm.getHandle(); 250 } 251 252 /** 253 * Gets the PWM channel number. 254 * 255 * @return The channel number. 256 */ 257 public int getChannel() { 258 return m_pwm.getChannel(); 259 } 260 261 /** 262 * Optionally eliminate the deadband from a motor controller. 263 * 264 * @param eliminateDeadband If true, set the motor curve for the motor controller to eliminate the 265 * deadband in the middle of the range. Otherwise, keep the full range without modifying any 266 * values. 267 */ 268 public void enableDeadbandElimination(boolean eliminateDeadband) { 269 m_eliminateDeadband = eliminateDeadband; 270 } 271 272 /** 273 * Make the given PWM motor controller follow the output of this one. 274 * 275 * @param follower The motor controller follower. 276 */ 277 public void addFollower(PWMMotorController follower) { 278 m_followers.add(follower); 279 } 280 281 @Override 282 public void initSendable(SendableBuilder builder) { 283 builder.setSmartDashboardType("Motor Controller"); 284 builder.setActuator(true); 285 builder.addDoubleProperty("Value", this::get, this::set); 286 } 287}