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.tCounter;
012 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
013 import edu.wpi.first.wpilibj.tables.ITable;
014 import edu.wpi.first.wpilibj.util.AllocationException;
015 import edu.wpi.first.wpilibj.util.BoundaryException;
016 import edu.wpi.first.wpilibj.util.CheckedAllocationException;
017
018 /**
019 * Class for counting the number of ticks on a digital input channel.
020 * This is a general purpose class for counting repetitive events. It can return the number
021 * of counts, the period of the most recent cycle, and detect when the signal being counted
022 * has stopped by supplying a maximum cycle time.
023 */
024 public class Counter extends SensorBase implements CounterBase, LiveWindowSendable, PIDSource {
025
026 /**
027 * Mode determines how and what the counter counts
028 */
029 public static class Mode {
030
031 /**
032 * The integer value representing this enumeration
033 */
034 public final int value;
035 static final int kTwoPulse_val = 0;
036 static final int kSemiperiod_val = 1;
037 static final int kPulseLength_val = 2;
038 static final int kExternalDirection_val = 3;
039 /**
040 * mode: two pulse
041 */
042 public static final Mode kTwoPulse = new Mode(kTwoPulse_val);
043 /**
044 * mode: semi period
045 */
046 public static final Mode kSemiperiod = new Mode(kSemiperiod_val);
047 /**
048 * mode: pulse length
049 */
050 public static final Mode kPulseLength = new Mode(kPulseLength_val);
051 /**
052 * mode: external direction
053 */
054 public static final Mode kExternalDirection = new Mode(kExternalDirection_val);
055
056 private Mode(int value) {
057 this.value = value;
058 }
059 }
060 private DigitalSource m_upSource; ///< What makes the counter count up.
061 private DigitalSource m_downSource; ///< What makes the counter count down.
062 private boolean m_allocatedUpSource;
063 private boolean m_allocatedDownSource;
064 private tCounter m_counter; ///< The FPGA counter object.
065 private int m_index; ///< The index of this counter.
066 private static Resource counters = new Resource(tCounter.kNumSystems);
067 private PIDSourceParameter m_pidSource;
068 private double m_distancePerPulse; // distance of travel for each tick
069
070 private void initCounter(final Mode mode) {
071 m_allocatedUpSource = false;
072 m_allocatedDownSource = false;
073
074 try {
075 m_index = counters.allocate();
076 } catch (CheckedAllocationException e) {
077 throw new AllocationException("No counters left to be allocated");
078 }
079
080 m_counter = new tCounter(m_index);
081 m_counter.writeConfig_Mode(mode.value);
082 m_upSource = null;
083 m_downSource = null;
084 m_counter.writeTimerConfig_AverageSize(1);
085
086 UsageReporting.report(UsageReporting.kResourceType_Counter, m_index, mode.value);
087 }
088
089 /**
090 * Create an instance of a counter where no sources are selected.
091 * Then they all must be selected by calling functions to specify the upsource and the downsource
092 * independently.
093 */
094 public Counter() {
095 initCounter(Mode.kTwoPulse);
096 }
097
098 /**
099 * Create an instance of a counter from a Digital Input.
100 * This is used if an existing digital input is to be shared by multiple other objects such
101 * as encoders.
102 * @param source the digital source to count
103 */
104 public Counter(DigitalSource source) {
105 if (source == null)
106 throw new NullPointerException("Source given was null");
107 initCounter(Mode.kTwoPulse);
108 setUpSource(source);
109 }
110
111 /**
112 * Create an instance of a Counter object.
113 * Create an up-Counter instance given a channel. The default digital module is assumed.
114 * @param channel the digital input channel to count
115 */
116 public Counter(int channel) {
117 initCounter(Mode.kTwoPulse);
118 setUpSource(channel);
119 }
120
121 /**
122 * Create an instance of a Counter object.
123 * Create an instance of an up-Counter given a digital module and a channel.
124 * @param slot The cRIO chassis slot for the digital module used
125 * @param channel The channel in the digital module
126 */
127 public Counter(int slot, int channel) {
128 initCounter(Mode.kTwoPulse);
129 setUpSource(slot, channel);
130 }
131
132 /**
133 * Create an instance of a Counter object.
134 * Create an instance of a simple up-Counter given an analog trigger.
135 * Use the trigger state output from the analog trigger.
136 * @param encodingType which edges to count
137 * @param upSource first source to count
138 * @param downSource second source for direction
139 * @param inverted true to invert the count
140 */
141 public Counter(EncodingType encodingType, DigitalSource upSource, DigitalSource downSource, boolean inverted) {
142 initCounter(Mode.kExternalDirection);
143 if (encodingType != EncodingType.k1X && encodingType != EncodingType.k2X) {
144 throw new RuntimeException("Counters only support 1X and 2X quadreature decoding!");
145 }
146 if (upSource == null)
147 throw new NullPointerException("Up Source given was null");
148 setUpSource(upSource);
149 if (downSource == null)
150 throw new NullPointerException("Down Source given was null");
151 setDownSource(downSource);
152
153 if (encodingType == null)
154 throw new NullPointerException("Encoding type given was null");
155
156 if (encodingType == EncodingType.k1X) {
157 setUpSourceEdge(true, false);
158 } else {
159 setUpSourceEdge(true, true);
160 }
161
162 setDownSourceEdge(inverted, true);
163 }
164
165 /**
166 * Create an instance of a Counter object.
167 * Create an instance of a simple up-Counter given an analog trigger.
168 * Use the trigger state output from the analog trigger.
169 * @param trigger the analog trigger to count
170 */
171 public Counter(AnalogTrigger trigger) {
172 initCounter(Mode.kTwoPulse);
173 setUpSource(trigger.createOutput(AnalogTriggerOutput.Type.kTypeState));
174 }
175
176 public void free() {
177 setUpdateWhenEmpty(true);
178
179 clearUpSource();
180 clearDownSource();
181 m_counter.Release();
182
183 m_upSource = null;
184 m_downSource = null;
185 m_counter = null;
186 counters.free(m_index);
187 }
188
189 /**
190 * Set the up source for the counter as digital input channel and slot.
191 * @param slot the location of the digital module to use
192 * @param channel the digital port to count
193 */
194 public void setUpSource(int slot, int channel) {
195 setUpSource(new DigitalInput(slot, channel));
196 m_allocatedUpSource = true;
197 }
198
199 /**
200 * Set the upsource for the counter as a digital input channel.
201 * The slot will be the default digital module slot.
202 * @param channel the digital port to count
203 */
204 public void setUpSource(int channel) {
205 setUpSource(new DigitalInput(channel));
206 m_allocatedUpSource = true;
207 }
208
209 /**
210 * Set the source object that causes the counter to count up.
211 * Set the up counting DigitalSource.
212 * @param source the digital source to count
213 */
214 public void setUpSource(DigitalSource source) {
215 if (m_upSource != null && m_allocatedUpSource) {
216 m_upSource.free();
217 m_allocatedUpSource = false;
218 }
219 m_upSource = source;
220 m_counter.writeConfig_UpSource_Module(source.getModuleForRouting());
221 m_counter.writeConfig_UpSource_Channel(source.getChannelForRouting());
222 m_counter.writeConfig_UpSource_AnalogTrigger(source.getAnalogTriggerForRouting());
223
224 if (m_counter.readConfig_Mode() == Mode.kTwoPulse.value ||
225 m_counter.readConfig_Mode() == Mode.kExternalDirection.value) {
226 setUpSourceEdge(true, false);
227 }
228 m_counter.strobeReset();
229 }
230
231 /**
232 * Set the up counting source to be an analog trigger.
233 * @param analogTrigger The analog trigger object that is used for the Up Source
234 * @param triggerType The analog trigger output that will trigger the counter.
235 */
236 public void setUpSource(AnalogTrigger analogTrigger, AnalogTriggerOutput.Type triggerType) {
237 setUpSource(analogTrigger.createOutput(triggerType));
238 m_allocatedUpSource = true;
239 }
240
241 /**
242 * Set the edge sensitivity on an up counting source.
243 * Set the up source to either detect rising edges or falling edges.
244 * @param risingEdge true to count rising edge
245 * @param fallingEdge true to count falling edge
246 */
247 public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
248 if (m_upSource == null) throw new RuntimeException(
249 "Up Source must be set before setting the edge!");
250 m_counter.writeConfig_UpRisingEdge(risingEdge);
251 m_counter.writeConfig_UpFallingEdge(fallingEdge);
252 }
253
254 /**
255 * Disable the up counting source to the counter.
256 */
257 public void clearUpSource() {
258 if (m_upSource != null && m_allocatedUpSource) {
259 m_upSource.free();
260 m_allocatedUpSource = false;
261 }
262 m_upSource = null;
263
264 boolean state = m_counter.readConfig_Enable();
265 m_counter.writeConfig_Enable(false);
266 m_counter.writeConfig_UpFallingEdge(false);
267 m_counter.writeConfig_UpRisingEdge(false);
268 // Index 0 of digital is always 0.
269 m_counter.writeConfig_UpSource_Channel(0);
270 m_counter.writeConfig_UpSource_AnalogTrigger(false);
271 m_counter.writeConfig_Enable(state);
272 }
273
274 /**
275 * Set the down counting source to be a digital input channel.
276 * The slot will be set to the default digital module slot.
277 * @param channel the digital port to count
278 */
279 public void setDownSource(int channel) {
280 setDownSource(new DigitalInput(channel));
281 m_allocatedDownSource = true;
282 }
283
284 /**
285 * Set the down counting source to be a digital input slot and channel.
286 * @param slot the location of the digital module to use
287 * @param channel the digital port to count
288 */
289 public void setDownSource(int slot, int channel) {
290 setDownSource(new DigitalInput(slot, channel));
291 m_allocatedDownSource = true;
292 }
293
294 /**
295 * Set the source object that causes the counter to count down.
296 * Set the down counting DigitalSource.
297 * @param source the digital source to count
298 */
299 public void setDownSource(DigitalSource source) {
300 if (m_downSource != null && m_allocatedDownSource) {
301 m_downSource.free();
302 m_allocatedDownSource = false;
303 }
304 int mode = m_counter.readConfig_Mode();
305 if(mode != Mode.kTwoPulse_val && mode != Mode.kExternalDirection_val) {
306 throw new RuntimeException(
307 "Down Source only supported in TwoPulse and ExternalDirection modes!");
308 }
309 m_downSource = source;
310 m_counter.writeConfig_DownSource_Module(source.getModuleForRouting());
311 m_counter.writeConfig_DownSource_Channel(source.getChannelForRouting());
312 m_counter.writeConfig_DownSource_AnalogTrigger(source.getAnalogTriggerForRouting());
313
314 setDownSourceEdge(true, false);
315 m_counter.strobeReset();
316 }
317
318 /**
319 * Set the down counting source to be an analog trigger.
320 * @param analogTrigger The analog trigger object that is used for the Down Source
321 * @param triggerType The analog trigger output that will trigger the counter.
322 */
323 public void setDownSource(AnalogTrigger analogTrigger, AnalogTriggerOutput.Type triggerType) {
324 setDownSource(analogTrigger.createOutput(triggerType));
325 m_allocatedDownSource = true;
326 }
327
328 /**
329 * Set the edge sensitivity on a down counting source.
330 * Set the down source to either detect rising edges or falling edges.
331 * @param risingEdge true to count the rising edge
332 * @param fallingEdge true to count the falling edge
333 */
334 public void setDownSourceEdge(boolean risingEdge, boolean fallingEdge) {
335 if (m_downSource == null) throw new RuntimeException(
336 " Down Source must be set before setting the edge!");
337 m_counter.writeConfig_DownRisingEdge(risingEdge);
338 m_counter.writeConfig_DownFallingEdge(fallingEdge);
339 }
340
341 /**
342 * Disable the down counting source to the counter.
343 */
344 public void clearDownSource() {
345 if (m_downSource != null && m_allocatedDownSource) {
346 m_downSource.free();
347 m_allocatedDownSource = false;
348 }
349 m_downSource = null;
350
351 boolean state = m_counter.readConfig_Enable();
352 m_counter.writeConfig_Enable(false);
353 m_counter.writeConfig_DownFallingEdge(false);
354 m_counter.writeConfig_DownRisingEdge(false);
355 // Index 0 of digital is always 0.
356 m_counter.writeConfig_DownSource_Channel(0);
357 m_counter.writeConfig_DownSource_AnalogTrigger(false);
358 m_counter.writeConfig_Enable(state);
359 }
360
361 /**
362 * Set standard up / down counting mode on this counter.
363 * Up and down counts are sourced independently from two inputs.
364 */
365 public void setUpDownCounterMode() {
366 m_counter.writeConfig_Mode(Mode.kTwoPulse.value);
367 }
368
369 /**
370 * Set external direction mode on this counter.
371 * Counts are sourced on the Up counter input.
372 * The Down counter input represents the direction to count.
373 */
374 public void setExternalDirectionMode() {
375 m_counter.writeConfig_Mode(Mode.kExternalDirection.value);
376 }
377
378 /**
379 * Set Semi-period mode on this counter.
380 * Counts up on both rising and falling edges.
381 * @param highSemiPeriod true to count up on both rising and falling
382 */
383 public void setSemiPeriodMode(boolean highSemiPeriod) {
384 m_counter.writeConfig_Mode(Mode.kSemiperiod.value);
385 m_counter.writeConfig_UpRisingEdge(highSemiPeriod);
386 setUpdateWhenEmpty(false);
387 }
388
389 /**
390 * Configure the counter to count in up or down based on the length of the input pulse.
391 * This mode is most useful for direction sensitive gear tooth sensors.
392 * @param threshold The pulse length beyond which the counter counts the opposite direction. Units are seconds.
393 */
394 public void setPulseLengthMode(double threshold) {
395 m_counter.writeConfig_Mode(Mode.kPulseLength.value);
396 m_counter.writeConfig_PulseLengthThreshold((short) ((threshold * 1.0e6) * kSystemClockTicksPerMicrosecond));
397 }
398
399 /**
400 * Start the Counter counting.
401 * This enables the counter and it starts accumulating counts from the associated
402 * input channel. The counter value is not reset on starting, and still has the previous value.
403 */
404 public void start() {
405 m_counter.writeConfig_Enable(true);
406 }
407
408 /**
409 * Read the current counter value.
410 * Read the value at this instant. It may still be running, so it reflects the current value. Next
411 * time it is read, it might have a different value.
412 */
413 public int get() {
414 return m_counter.readOutput_Value();
415 }
416
417 /**
418 * Read the current scaled counter value.
419 * Read the value at this instant, scaled by the distance per pulse (defaults to 1).
420 * @return
421 */
422 public double getDistance() {
423 return m_counter.readOutput_Value() * m_distancePerPulse;
424 }
425
426 /**
427 * Reset the Counter to zero.
428 * Set the counter value to zero. This doesn't effect the running state of the counter, just sets
429 * the current value to zero.
430 */
431 public void reset() {
432 m_counter.strobeReset();
433 }
434
435 /**
436 * Stop the Counter.
437 * Stops the counting but doesn't effect the current value.
438 */
439 public void stop() {
440 m_counter.writeConfig_Enable(false);
441 }
442
443 /**
444 * Set the maximum period where the device is still considered "moving".
445 * Sets the maximum period where the device is considered moving. This value is used to determine
446 * the "stopped" state of the counter using the GetStopped method.
447 * @param maxPeriod The maximum period where the counted device is considered moving in
448 * seconds.
449 */
450 public void setMaxPeriod(double maxPeriod) {
451 m_counter.writeTimerConfig_StallPeriod((int) (maxPeriod * 1.0e6));
452 }
453
454 /**
455 * Select whether you want to continue updating the event timer output when there are no samples captured.
456 * The output of the event timer has a buffer of periods that are averaged and posted to
457 * a register on the FPGA. When the timer detects that the event source has stopped
458 * (based on the MaxPeriod) the buffer of samples to be averaged is emptied. If you
459 * enable the update when empty, you will be notified of the stopped source and the event
460 * time will report 0 samples. If you disable update when empty, the most recent average
461 * will remain on the output until a new sample is acquired. You will never see 0 samples
462 * output (except when there have been no events since an FPGA reset) and you will likely not
463 * see the stopped bit become true (since it is updated at the end of an average and there are
464 * no samples to average).
465 * @param enabled true to continue updating
466 */
467 public void setUpdateWhenEmpty(boolean enabled) {
468 m_counter.writeTimerConfig_UpdateWhenEmpty(enabled);
469 }
470
471 /**
472 * Determine if the clock is stopped.
473 * Determine if the clocked input is stopped based on the MaxPeriod value set using the
474 * SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and counter) are
475 * assumed to be stopped and it returns true.
476 * @return Returns true if the most recent counter period exceeds the MaxPeriod value set by
477 * SetMaxPeriod.
478 */
479 public boolean getStopped() {
480 return m_counter.readTimerOutput_Stalled();
481 }
482
483 /**
484 * The last direction the counter value changed.
485 * @return The last direction the counter value changed.
486 */
487 public boolean getDirection() {
488 boolean value = m_counter.readOutput_Direction();
489 return value;
490 }
491
492 /**
493 * Set the Counter to return reversed sensing on the direction.
494 * This allows counters to change the direction they are counting in the case of 1X and 2X
495 * quadrature encoding only. Any other counter mode isn't supported.
496 * @param reverseDirection true if the value counted should be negated.
497 */
498 public void setReverseDirection(boolean reverseDirection) {
499 if (m_counter.readConfig_Mode() == Mode.kExternalDirection.value) {
500 if (reverseDirection) {
501 setDownSourceEdge(true, true);
502 } else {
503 setDownSourceEdge(false, true);
504 }
505 }
506 }
507
508 /**
509 * Get the Period of the most recent count.
510 * Returns the time interval of the most recent count. This can be used for velocity calculations
511 * to determine shaft speed.
512 * @returns The period of the last two pulses in units of seconds.
513 */
514 public double getPeriod() {
515 double period;
516 if (m_counter.readTimerOutput_Stalled()) {
517 return Double.POSITIVE_INFINITY;
518 } else {
519 period = (double) m_counter.readTimerOutput_Period() / (double) m_counter.readTimerOutput_Count();
520 }
521 return period / 1.0e6;
522 }
523
524 /**
525 * Get the current rate of the Counter.
526 * Read the current rate of the counter accounting for the distance per pulse value.
527 * The default value for distance per pulse (1) yields units of pulses per second.
528 * @return The rate in units/sec
529 */
530 public double getRate() {
531 return m_distancePerPulse / getPeriod();
532 }
533
534 /**
535 * Set the Samples to Average which specifies the number of samples of the timer to
536 * average when calculating the period. Perform averaging to account for
537 * mechanical imperfections or as oversampling to increase resolution.
538 * @param samplesToAverage The number of samples to average from 1 to 127.
539 */
540 public void setSamplesToAverage (int samplesToAverage) {
541 BoundaryException.assertWithinBounds(samplesToAverage, 1, 127);
542 m_counter.writeTimerConfig_AverageSize(samplesToAverage);
543 }
544
545 /**
546 * Get the Samples to Average which specifies the number of samples of the timer to
547 * average when calculating the period. Perform averaging to account for
548 * mechanical imperfections or as oversampling to increase resolution.
549 * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
550 */
551 public int getSamplesToAverage()
552 {
553 return m_counter.readTimerConfig_AverageSize();
554 }
555
556 /**
557 * Set the distance per pulse for this counter.
558 * This sets the multiplier used to determine the distance driven based on the count value
559 * from the encoder. Set this value based on the Pulses per Revolution and factor in any
560 * gearing reductions. This distance can be in any units you like, linear or angular.
561 *
562 * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
563 */
564 public void setDistancePerPulse(double distancePerPulse) {
565 m_distancePerPulse = distancePerPulse;
566 }
567
568 /**
569 * Set which parameter of the encoder you are using as a process control variable.
570 * The counter class supports the rate and distance parameters.
571 * @param pidSource An enum to select the parameter.
572 */
573 public void setPIDSourceParameter(PIDSourceParameter pidSource) {
574 BoundaryException.assertWithinBounds(pidSource.value, 0, 1);
575 m_pidSource = pidSource;
576 }
577
578 public double pidGet() {
579 switch (m_pidSource.value) {
580 case PIDSourceParameter.kDistance_val:
581 return getDistance();
582 case PIDSourceParameter.kRate_val:
583 return getRate();
584 default:
585 return 0.0;
586 }
587 }
588
589 /**
590 * Live Window code, only does anything if live window is activated.
591 */
592 public String getSmartDashboardType(){
593 return "Counter";
594 }
595 private ITable m_table;
596
597 /**
598 * {@inheritDoc}
599 */
600 public void initTable(ITable subtable) {
601 m_table = subtable;
602 updateTable();
603 }
604
605 /**
606 * {@inheritDoc}
607 */
608 public ITable getTable(){
609 return m_table;
610 }
611
612 /**
613 * {@inheritDoc}
614 */
615 public void updateTable() {
616 if (m_table != null) {
617 m_table.putNumber("Value", m_counter.readOutput_Value());
618 }
619 }
620
621 /**
622 * {@inheritDoc}
623 */
624 public void startLiveWindowMode() {}
625
626 /**
627 * {@inheritDoc}
628 */
629 public void stopLiveWindowMode() {}
630 }