001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007
008 package edu.wpi.first.wpilibj;
009
010 import edu.wpi.first.wpilibj.communication.UsageReporting;
011 import edu.wpi.first.wpilibj.fpga.tDIO;
012 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
013 import edu.wpi.first.wpilibj.tables.ITable;
014 import edu.wpi.first.wpilibj.tables.ITableListener;
015 import edu.wpi.first.wpilibj.util.AllocationException;
016 import edu.wpi.first.wpilibj.util.CheckedAllocationException;
017
018 /**
019 * Class implements the PWM generation in the FPGA.
020 * Values supplied as arguments for PWM outputs range from -1.0 to 1.0. They are mapped
021 * to the hardware dependent values, in this case 0-255 for the FPGA.
022 * Changes are immediately sent to the FPGA, and the update occurs at the next
023 * FPGA cycle. There is no delay.
024 *
025 * As of revision 0.1.4 of the FPGA, the FPGA interprets the 0-255 values as follows:
026 * 255 = full "forward"
027 * 254 to 129 = linear scaling from "full forward" to "center"
028 * 128 = center value
029 * 127 to 2 = linear scaling from "center" to "full reverse"
030 * 1 = full "reverse"
031 * 0 = disabled (i.e. PWM output is held low)
032 */
033 public class PWM extends SensorBase implements LiveWindowSendable {
034
035 private static Resource allocated = new Resource((tDIO.kNumSystems * kPwmChannels));
036
037 /**
038 * Represents the amount to multiply the minimum servo-pulse pwm period by.
039 */
040 public static class PeriodMultiplier {
041
042 /**
043 * The integer value representing this enumeration
044 */
045 public final int value;
046 static final int k1X_val = 1;
047 static final int k2X_val = 2;
048 static final int k4X_val = 4;
049 /**
050 * Period Multiplier: don't skip pulses
051 */
052 public static final PeriodMultiplier k1X = new PeriodMultiplier(k1X_val);
053 /**
054 * Period Multiplier: skip every other pulse
055 */
056 public static final PeriodMultiplier k2X = new PeriodMultiplier(k2X_val);
057 /**
058 * Period Multiplier: skip three out of four pulses
059 */
060 public static final PeriodMultiplier k4X = new PeriodMultiplier(k4X_val);
061
062 private PeriodMultiplier(int value) {
063 this.value = value;
064 }
065 }
066 private int m_channel;
067 private DigitalModule m_module;
068 /**
069 * kDefaultPwmPeriod is in ms
070 *
071 * - 20ms periods (50 Hz) are the "safest" setting in that this works for all devices
072 * - 20ms periods seem to be desirable for Vex Motors
073 * - 20ms periods are the specified period for HS-322HD servos, but work reliably down
074 * to 10.0 ms; starting at about 8.5ms, the servo sometimes hums and get hot;
075 * by 5.0ms the hum is nearly continuous
076 * - 10ms periods work well for Victor 884
077 * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed controllers.
078 * Due to the shipping firmware on the Jaguar, we can't run the update period less
079 * than 5.05 ms.
080 *
081 * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period scaling is implemented as an
082 * output squelch to get longer periods for old devices.
083 */
084 protected static final double kDefaultPwmPeriod = 5.05;
085 /**
086 * kDefaultPwmCenter is the PWM range center in ms
087 */
088 protected static final double kDefaultPwmCenter = 1.5;
089 /**
090 * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint
091 */
092 protected static final int kDefaultPwmStepsDown = 128;
093 public static final int kPwmDisabled = 0;
094 boolean m_eliminateDeadband;
095 int m_maxPwm;
096 int m_deadbandMaxPwm;
097 int m_centerPwm;
098 int m_deadbandMinPwm;
099 int m_minPwm;
100
101 /**
102 * Initialize PWMs given an module and channel.
103 *
104 * This method is private and is the common path for all the constructors for creating PWM
105 * instances. Checks module and channel value ranges and allocates the appropriate channel.
106 * The allocation is only done to help users ensure that they don't double assign channels.
107 */
108 private void initPWM(final int moduleNumber, final int channel) {
109 checkPWMModule(moduleNumber);
110 checkPWMChannel(channel);
111 try {
112 allocated.allocate((moduleNumber - 1) * kPwmChannels + channel - 1);
113 } catch (CheckedAllocationException e) {
114 throw new AllocationException(
115 "PWM channel " + channel + " on module " + moduleNumber + " is already allocated");
116 }
117 m_channel = channel;
118 m_module = DigitalModule.getInstance(moduleNumber);
119 m_module.setPWM(m_channel, kPwmDisabled);
120 m_eliminateDeadband = false;
121
122 UsageReporting.report(UsageReporting.kResourceType_PWM, channel, moduleNumber-1);
123 }
124
125 /**
126 * Allocate a PWM given a module and channel.
127 * Allocate a PWM using a module and channel number.
128 *
129 * @param moduleNumber The module number of the digital module to use.
130 * @param channel The PWM channel on the digital module.
131 */
132 public PWM(final int moduleNumber, final int channel) {
133 initPWM(moduleNumber, channel);
134 }
135
136 /**
137 * Allocate a PWM in the default module given a channel.
138 *
139 * Using a default module allocate a PWM given the channel number.
140 *
141 * @param channel The PWM channel on the digital module.
142 */
143 public PWM(final int channel) {
144 initPWM(getDefaultDigitalModule(), channel);
145 }
146
147 /**
148 * Free the PWM channel.
149 *
150 * Free the resource associated with the PWM channel and set the value to 0.
151 */
152 public void free() {
153 m_module.setPWM(m_channel, kPwmDisabled);
154 allocated.free((m_module.getModuleNumber() - 1) * kPwmChannels + m_channel - 1);
155 }
156
157 /**
158 * Optionally eliminate the deadband from a speed controller.
159 * @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate
160 * the deadband in the middle of the range. Otherwise, keep the full range without
161 * modifying any values.
162 */
163 public void enableDeadbandElimination(boolean eliminateDeadband) {
164 m_eliminateDeadband = eliminateDeadband;
165 }
166
167 /**
168 * Set the bounds on the PWM values.
169 * This sets the bounds on the PWM values for a particular each type of controller. The values
170 * determine the upper and lower speeds as well as the deadband bracket.
171 * @deprecated Recommended to set bounds in ms using {@link #setBounds(double, double, double, double, double)}
172 * @param max The Minimum pwm value
173 * @param deadbandMax The high end of the deadband range
174 * @param center The center speed (off)
175 * @param deadbandMin The low end of the deadband range
176 * @param min The minimum pwm value
177 */
178 public void setBounds(final int max, final int deadbandMax, final int center, final int deadbandMin, final int min) {
179 m_maxPwm = max;
180 m_deadbandMaxPwm = deadbandMax;
181 m_centerPwm = center;
182 m_deadbandMinPwm = deadbandMin;
183 m_minPwm = min;
184 }
185
186 /**
187 * Set the bounds on the PWM pulse widths.
188 * This sets the bounds on the PWM values for a particular type of controller. The values
189 * determine the upper and lower speeds as well as the deadband bracket.
190 * @param max The max PWM pulse width in ms
191 * @param deadbandMax The high end of the deadband range pulse width in ms
192 * @param center The center (off) pulse width in ms
193 * @param deadbandMin The low end of the deadband pulse width in ms
194 * @param min The minimum pulse width in ms
195 */
196 void setBounds(double max, double deadbandMax, double center, double deadbandMin, double min)
197 {
198 double loopTime = m_module.getLoopTiming()/(kSystemClockTicksPerMicrosecond*1e3);
199
200 m_maxPwm = (int)((max-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
201 m_deadbandMaxPwm = (int)((deadbandMax-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
202 m_centerPwm = (int)((center-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
203 m_deadbandMinPwm = (int)((deadbandMin-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
204 m_minPwm = (int)((min-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
205 }
206
207 /**
208 * Gets the module number associated with the PWM Object.
209 *
210 * @return The module's number.
211 */
212 public int getModuleNumber(){
213 return m_module.getModuleNumber();
214 }
215
216 /**
217 * Gets the channel number associated with the PWM Object.
218 *
219 * @return The channel number.
220 */
221 public int getChannel(){
222 return m_channel;
223 }
224
225 /**
226 * Set the PWM value based on a position.
227 *
228 * This is intended to be used by servos.
229 *
230 * @pre SetMaxPositivePwm() called.
231 * @pre SetMinNegativePwm() called.
232 *
233 * @param pos The position to set the servo between 0.0 and 1.0.
234 */
235 public void setPosition(double pos) {
236 if (pos < 0.0) {
237 pos = 0.0;
238 } else if (pos > 1.0) {
239 pos = 1.0;
240 }
241
242 int rawValue;
243 // note, need to perform the multiplication below as floating point before converting to int
244 rawValue = (int) ((pos * (double)getFullRangeScaleFactor()) + getMinNegativePwm());
245
246 // send the computed pwm value to the FPGA
247 setRaw(rawValue);
248 }
249
250 /**
251 * Get the PWM value in terms of a position.
252 *
253 * This is intended to be used by servos.
254 *
255 * @pre SetMaxPositivePwm() called.
256 * @pre SetMinNegativePwm() called.
257 *
258 * @return The position the servo is set to between 0.0 and 1.0.
259 */
260 public double getPosition() {
261 int value = getRaw();
262 if (value < getMinNegativePwm()) {
263 return 0.0;
264 } else if (value > getMaxPositivePwm()) {
265 return 1.0;
266 } else {
267 return (double)(value - getMinNegativePwm()) / (double)getFullRangeScaleFactor();
268 }
269 }
270
271 /**
272 * Set the PWM value based on a speed.
273 *
274 * This is intended to be used by speed controllers.
275 *
276 * @pre SetMaxPositivePwm() called.
277 * @pre SetMinPositivePwm() called.
278 * @pre SetCenterPwm() called.
279 * @pre SetMaxNegativePwm() called.
280 * @pre SetMinNegativePwm() called.
281 *
282 * @param speed The speed to set the speed controller between -1.0 and 1.0.
283 */
284 final void setSpeed(double speed) {
285 // clamp speed to be in the range 1.0 >= speed >= -1.0
286 if (speed < -1.0) {
287 speed = -1.0;
288 } else if (speed > 1.0) {
289 speed = 1.0;
290 }
291
292 // calculate the desired output pwm value by scaling the speed appropriately
293 int rawValue;
294 if (speed == 0.0) {
295 rawValue = getCenterPwm();
296 } else if (speed > 0.0) {
297 rawValue = (int) (speed * ((double)getPositiveScaleFactor()) +
298 ((double)getMinPositivePwm()) + 0.5);
299 } else {
300 rawValue = (int) (speed * ((double)getNegativeScaleFactor()) +
301 ((double)getMaxNegativePwm()) + 0.5);
302 }
303
304 // send the computed pwm value to the FPGA
305 setRaw(rawValue);
306 }
307
308 /**
309 * Get the PWM value in terms of speed.
310 *
311 * This is intended to be used by speed controllers.
312 *
313 * @pre SetMaxPositivePwm() called.
314 * @pre SetMinPositivePwm() called.
315 * @pre SetMaxNegativePwm() called.
316 * @pre SetMinNegativePwm() called.
317 *
318 * @return The most recently set speed between -1.0 and 1.0.
319 */
320 public double getSpeed() {
321 int value = getRaw();
322 if (value > getMaxPositivePwm()) {
323 return 1.0;
324 } else if (value < getMinNegativePwm()) {
325 return -1.0;
326 } else if (value > getMinPositivePwm()) {
327 return (double) (value - getMinPositivePwm()) / (double)getPositiveScaleFactor();
328 } else if (value < getMaxNegativePwm()) {
329 return (double) (value - getMaxNegativePwm()) / (double)getNegativeScaleFactor();
330 } else {
331 return 0.0;
332 }
333 }
334
335 /**
336 * Set the PWM value directly to the hardware.
337 *
338 * Write a raw value to a PWM channel.
339 *
340 * @param value Raw PWM value. Range 0 - 255.
341 */
342 public void setRaw(int value) {
343 m_module.setPWM(m_channel, value);
344 }
345
346 /**
347 * Get the PWM value directly from the hardware.
348 *
349 * Read a raw value from a PWM channel.
350 *
351 * @return Raw PWM control value. Range: 0 - 255.
352 */
353 public int getRaw() {
354 return m_module.getPWM(m_channel);
355 }
356
357 /**
358 * Slow down the PWM signal for old devices.
359 *
360 * @param mult The period multiplier to apply to this channel
361 */
362 public void setPeriodMultiplier(PeriodMultiplier mult) {
363 switch (mult.value) {
364 case PeriodMultiplier.k4X_val:
365 m_module.setPWMPeriodScale(m_channel, 3); // Squelch 3 out of 4 outputs
366 break;
367 case PeriodMultiplier.k2X_val:
368 m_module.setPWMPeriodScale(m_channel, 1); // Squelch 1 out of 2 outputs
369 break;
370 case PeriodMultiplier.k1X_val:
371 m_module.setPWMPeriodScale(m_channel, 0); // Don't squelch any outputs
372 break;
373 default:
374 //Cannot hit this, limited by PeriodMultiplier enum
375 }
376 }
377
378 private int getMaxPositivePwm() {
379 return m_maxPwm;
380 }
381
382 ;
383
384 private int getMinPositivePwm() {
385 return m_eliminateDeadband ? m_deadbandMaxPwm : m_centerPwm + 1;
386 }
387
388 ;
389
390 private int getCenterPwm() {
391 return m_centerPwm;
392 }
393
394 ;
395
396 private int getMaxNegativePwm() {
397 return m_eliminateDeadband ? m_deadbandMinPwm : m_centerPwm - 1;
398 }
399
400 ;
401
402 private int getMinNegativePwm() {
403 return m_minPwm;
404 }
405
406 ;
407
408 private int getPositiveScaleFactor() {
409 return getMaxPositivePwm() - getMinPositivePwm();
410 } ///< The scale for positive speeds.
411
412 private int getNegativeScaleFactor() {
413 return getMaxNegativePwm() - getMinNegativePwm();
414 } ///< The scale for negative speeds.
415
416 private int getFullRangeScaleFactor() {
417 return getMaxPositivePwm() - getMinNegativePwm();
418 } ///< The scale for positions.
419
420 /*
421 * Live Window code, only does anything if live window is activated.
422 */
423 public String getSmartDashboardType(){
424 return "Speed Controller";
425 }
426 private ITable m_table;
427 private ITableListener m_table_listener;
428
429 /**
430 * {@inheritDoc}
431 */
432 public void initTable(ITable subtable) {
433 m_table = subtable;
434 updateTable();
435 }
436
437 /**
438 * {@inheritDoc}
439 */
440 public void updateTable() {
441 if (m_table != null) {
442 m_table.putNumber("Value", getSpeed());
443 }
444 }
445
446 /**
447 * {@inheritDoc}
448 */
449 public ITable getTable(){
450 return m_table;
451 }
452
453 /**
454 * {@inheritDoc}
455 */
456 public void startLiveWindowMode() {
457 setSpeed(0); // Stop for safety
458 m_table_listener = new ITableListener() {
459 public void valueChanged(ITable itable, String key, Object value, boolean bln) {
460 setSpeed(((Double) value).doubleValue());
461 }
462 };
463 m_table.addTableListener("Value", m_table_listener, true);
464 }
465
466 /**
467 * {@inheritDoc}
468 */
469 public void stopLiveWindowMode() {
470 setSpeed(0); // Stop for safety
471 // TODO: Broken, should only remove the listener from "Value" only.
472 m_table.removeTableListener(m_table_listener);
473 }
474 }