RSeries astromech firmware
ServoDispatchPrivate.h
Go to the documentation of this file.
1 #ifndef ServoDispatchPrivate_h
2 #define ServoDispatchPrivate_h
3 
4 #include "ServoDispatch.h"
5 #include "ServoDispatchPrivate.h"
6 
7 /************ static functions common to all instances ***********************/
8 
9 #ifndef ARDUINO_ARCH_ESP32
10 struct ServoDispatchISR
12 {
13 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
14 #define USE_TIMER5
15 #define USE_TIMER1
16 #define USE_TIMER3
17 #define USE_TIMER4
18 enum Timer16Order { kTimer16_5, kTimer16_1, kTimer16_3, kTimer16_4, kNumTimers16 };
19 #elif defined(__AVR_ATmega32U4__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) || \
20  defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
21 #define USE_TIMER3
22 #define USE_TIMER1
23 enum Timer16Order { kTimer16_3, kTimer16_1, kNumTimers16 };
24 #else // everything else
25 #define USE_TIMER1
26 enum Timer16Order { kTimer16_1, kNumTimers16 };
27 #endif
28 #define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer
29 #define MAX_SERVOS (kNumTimers16 * SERVOS_PER_TIMER)
30 #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel)
31 
33  struct PWMChannel
34  {
35  uint8_t pin:6;
36  volatile uint8_t isActive:1;
37  unsigned int ticks;
38  unsigned int value;
39  unsigned int target;
40  uint8_t speed;
41  };
43  struct Private
44  {
45  uint8_t ServoCount; // the total number of attached servos
46  volatile int8_t channel[kNumTimers16]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
47  PWMChannel pwm[MAX_SERVOS]; // static array of servo structures
48  };
49 
50  static Private* privates()
51  {
52  static Private priv;
53  return &priv;
54  }
55 
56  static unsigned int convertMicrosecToTicks(unsigned int microsec)
57  {
58  return (clockCyclesPerMicrosecond() * microsec) / 8;
59  }
60 
61  static unsigned int convertTicksToMicrosec(unsigned int ticks)
62  {
63  return ((unsigned)ticks * 8) / clockCyclesPerMicrosecond();
64  }
65 
66  static inline void handle_interrupts(Timer16Order timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
67  {
68  auto priv = privates();
69  volatile int8_t* timerChannel = &priv->channel[timer];
70  PWMChannel* servo = &priv->pwm[SERVO_INDEX(timer, *timerChannel)];
71  if (*timerChannel < 0)
72  {
73  *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
74  }
75  else
76  {
77  if (SERVO_INDEX(timer,*timerChannel) < priv->ServoCount && servo->isActive == true)
78  digitalWrite(servo->pin,LOW); // pulse this channel low if activated
79  }
80 
81  *timerChannel += 1; // increment to the next channel
82  servo = &priv->pwm[SERVO_INDEX(timer, *timerChannel)];
83  if (SERVO_INDEX(timer, *timerChannel) < priv->ServoCount && *timerChannel < SERVOS_PER_TIMER)
84  {
85  // Extension for slowmove
86  if (servo->speed)
87  {
88  // Increment ticks by speed until we reach the target.
89  // When the target is reached, speed is set to 0 to disable that code.
90  if (servo->target > servo->ticks)
91  {
92  servo->ticks += servo->speed;
93  if (servo->target <= servo->ticks)
94  {
95  servo->ticks = servo->target;
96  servo->speed = 0;
97  }
98  }
99  else
100  {
101  servo->ticks -= servo->speed;
102  if (servo->target >= servo->ticks)
103  {
104  servo->ticks = servo->target;
105  servo->speed = 0;
106  }
107  }
108  }
109  // End of Extension for slowmove
110 
111  // Todo
112  *OCRnA = *TCNTn + servo->ticks;
113  if (servo->isActive == true) // check if activated
114  digitalWrite(servo->pin,HIGH); // its an active channel so pulse it high
115  }
116  else
117  {
118  const unsigned kRefreshInterval = 20000; // time to refresh servos in microseconds
119  // finished all channels so wait for the refresh period to expire before starting over
120  unsigned long ticks = convertMicrosecToTicks(kRefreshInterval);
121  if ((unsigned)*TCNTn < ticks + 4) // allow a few ticks to ensure the next OCR1A not missed
122  *OCRnA = (unsigned int)ticks;
123  else
124  *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
125  *timerChannel = -1; // this will get incremented at the end of the refresh period to start again at the first channel
126  }
127  }
128 
129  static Timer16Order channelToTimer(byte channel)
130  {
131  return ((Timer16Order)(channel / SERVOS_PER_TIMER));
132  }
133 
134  static void initISR(int channel)
135  {
136  Timer16Order timer = channelToTimer(channel);
137  if (!isTimerActive(timer))
138  {
139  switch (timer)
140  {
141  #if defined (USE_TIMER1)
142  case kTimer16_1:
143  {
144  TCCR1A = 0; // normal counting mode
145  TCCR1B = _BV(CS11); // set prescaler of 8
146  TCNT1 = 0; // clear the timer count
147  #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
148  TIFR |= _BV(OCF1A); // clear any pending interrupts;
149  TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
150  #else
151  // here if not ATmega8 or ATmega128
152  TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
153  TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
154  #endif
155  // #if defined(WIRING)
156  // timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
157  // #endif
158  break;
159  }
160  #endif
161 
162  #if defined (USE_TIMER3)
163  case kTimer16_3:
164  {
165  TCCR3A = 0; // normal counting mode
166  TCCR3B = _BV(CS31); // set prescaler of 8
167  TCNT3 = 0; // clear the timer count
168  #if defined(__AVR_ATmega128__)
169  TIFR |= _BV(OCF3A); // clear any pending interrupts;
170  ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
171  #else
172  TIFR3 = _BV(OCF3A); // clear any pending interrupts;
173  TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
174  #endif
175  // #if defined(WIRING)
176  // timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
177  // #endif
178  break;
179  }
180  #endif
181 
182  #if defined (USE_TIMER4)
183  case kTimer16_4:
184  {
185  TCCR4A = 0; // normal counting mode
186  TCCR4B = _BV(CS41); // set prescaler of 8
187  TCNT4 = 0; // clear the timer count
188  TIFR4 = _BV(OCF4A); // clear any pending interrupts;
189  TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
190  break;
191  }
192  #endif
193 
194  #if defined (USE_TIMER5)
195  case kTimer16_5:
196  {
197  TCCR5A = 0; // normal counting mode
198  TCCR5B = _BV(CS51); // set prescaler of 8
199  TCNT5 = 0; // clear the timer count
200  TIFR5 = _BV(OCF5A); // clear any pending interrupts;
201  TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
202  break;
203  }
204  #endif
205  case kNumTimers16:
206  /* Dummy */
207  break;
208  }
209  }
210  auto priv = privates();
211  priv->pwm[channel].isActive = true;
212  }
213 
214  static boolean isTimerActive(Timer16Order timer)
215  {
216  auto priv = privates();
217  // returns true if any servo is active on this timer
218  for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++)
219  {
220  if (priv->pwm[SERVO_INDEX(timer,channel)].isActive == true)
221  return true;
222  }
223  return false;
224  }
225 };
226 
227 #if defined(USE_TIMER1)
228 /* {Secret} */
229 ISR (TIMER1_COMPA_vect)
230 {
231  ServoDispatchISR::handle_interrupts(ServoDispatchISR::kTimer16_1, &TCNT1, &OCR1A);
232 }
233 #endif
234 
235 #if defined(USE_TIMER3)
236 /* {Secret} */
237 ISR (TIMER3_COMPA_vect)
238 {
239  ServoDispatchISR::handle_interrupts(ServoDispatchISR::kTimer16_3, &TCNT3, &OCR3A);
240 }
241 #endif
242 
243 #if defined(USE_TIMER4)
244 /* {Secret} */
245 ISR (TIMER4_COMPA_vect)
246 {
247  ServoDispatchISR::handle_interrupts(ServoDispatchISR::kTimer16_4, &TCNT4, &OCR4A);
248 }
249 #endif
250 
251 #if defined(USE_TIMER5)
252 /* {Secret} */
253 ISR (TIMER5_COMPA_vect)
254 {
255  ServoDispatchISR::handle_interrupts(ServoDispatchISR::kTimer16_5, &TCNT5, &OCR5A);
256 }
257 #endif
258 
259 #undef USE_TIMER5
260 #undef USE_TIMER1
261 #undef USE_TIMER3
262 #undef USE_TIMER4
263 
264 #else /* ARDUINO_ARCH_ESP32 */
265 
266 #include "esp32-hal-ledc.h"
267 
269 class ServoDispatchESP32
270 {
271 private:
272  static constexpr unsigned kNumPWM = 16;
273 
275  class Private
276  {
277  public:
278  int PWMCount;
279  int timerCount[4];
280  ServoDispatchESP32* ChannelUsed[kNumPWM];
281  long timerFreqSet[4] = { -1, -1, -1, -1 };
282  bool explicateAllocationMode;
283  };
284 
285  static Private* privates()
286  {
287  static Private priv;
288  return &priv;
289  }
290 
291 public:
292  ServoDispatchESP32()
293  {
294  }
295 
296  virtual ~ServoDispatchESP32()
297  {
298  if (attached())
299  ledcDetachPin(fPin);
300  deallocate();
301  }
302 
303  void detachPin(int pin)
304  {
305  ledcDetachPin(pin);
306  deallocate();
307  }
308 
309  void attachPin(uint8_t pin, double freq, uint8_t resolution_bits = 10)
310  {
311  if (validPWM(pin))
312  setup(freq, resolution_bits);
313  attachPin(pin);
314  }
315 
316  inline bool attached()
317  {
318  return fAttached;
319  }
320 
321  void write(uint32_t duty)
322  {
323  fDuty = duty;
324  ledcWrite(getChannel(), duty);
325  }
326 
327  void writeScaled(float duty)
328  {
329  write(mapf(duty, 0.0, 1.0, 0, (float) ((1 << fResolutionBits) - 1)));
330  }
331 
332  void adjustFrequency(double freq, float dutyScaled = -1)
333  {
334  if (dutyScaled < 0)
335  dutyScaled = getDutyScaled();
336  writeScaled(dutyScaled);
337  auto priv = privates();
338  auto ChannelUsed = priv->ChannelUsed;
339  for (int i = 0; i < priv->timerCount[getTimer()]; i++)
340  {
341  int pwm = timerAndIndexToChannel(getTimer(), i);
342  if (ChannelUsed[pwm] != nullptr)
343  {
344  if (ChannelUsed[pwm]->fFreq != freq)
345  {
346  ChannelUsed[pwm]->adjustFrequencyLocal(freq, ChannelUsed[pwm]->getDutyScaled());
347  }
348  }
349  }
350  }
351 
352  uint32_t read()
353  {
354  return ledcRead(getChannel());
355  }
356 
357  double readFreq()
358  {
359  return fFreq;
360  }
361 
362  float getDutyScaled()
363  {
364  return mapf((float)fDuty, 0, (float) ((1 << fResolutionBits) - 1), 0.0, 1.0);
365  }
366 
367  static int timerAndIndexToChannel(int timerNum, int index)
368  {
369  int idx = 0;
370  for (int j = 0; j < kNumPWM; j++)
371  {
372  if (((j / 2) % 4) == timerNum)
373  {
374  if (idx == index)
375  return j;
376  idx++;
377  }
378  }
379  return -1;
380  }
381 
382  static void configureTimer(int timerNumber)
383  {
384  if (timerNumber >= 0 && timerNumber < 4)
385  {
386  auto priv = privates();
387  if (priv->explicateAllocationMode == false)
388  {
389  priv->explicateAllocationMode = true;
390  for (int i = 0; i < 4; i++)
391  priv->timerCount[i] = 4;
392  }
393  priv->timerCount[timerNumber] = 0;
394  }
395  }
396 
397  inline int getTimer()
398  {
399  return fTimerNum;
400  }
401 
402  inline int getChannel()
403  {
404  return fPWMChannel;
405  }
406 
407  inline int getPin()
408  {
409  return fPin;
410  }
411 
412  static bool validPWM(int pin)
413  {
414  #ifdef CONFIG_IDF_TARGET_ESP32
415  // Datasheet https://www.espressif.com/sites/default/files/documentation/esp32_datasheet_en.pdf,
416  // Pinout https://docs.espressif.com/projects/esp-idf/en/latest/esp32/_images/esp32-devkitC-v4-pinout.jpg
417  return (pin == 2 || pin == 4 || pin == 5) ||
418  (pin >= 12 && pin <= 19) || (pin >= 21 && pin <= 23) ||
419  (pin >= 25 && pin <= 27) || (pin == 32 || pin == 33);
420  #elif CONFIG_IDF_TARGET_ESP32S2
421  // Datasheet https://www.espressif.com/sites/default/files/documentation/esp32-s2_datasheet_en.pdf,
422  // Pinout https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/_images/esp32-s2_saola1-pinout.jpg
423  return (pin >= 1 && pin <= 21) || (pin >= 33 && pin <= 44);
424  #elif CONFIG_IDF_TARGET_ESP32S3
425  // Datasheet https://docs.espressif.com/projects/esp-idf/en/latest/esp32s3/api-reference/peripherals/gpio.html
426  // Pinout https://docs.espressif.com/projects/esp-idf/en/latest/esp32s3/_images/ESP32-S3_DevKitC-1_pinlayout_v1.1.jpg
427  if (pin >= 0 && pin <= 48)
428  {
429  // Strapping pin: GPIO0, GPIO3, GPIO45 and GPIO46 are strapping pins
430  // if (pin == 0 || pin == 3 || pin == 45 || pin == 46)
431  // return false;
432  // SPI0/1: GPIO26-32 are usually used for SPI flash and PSRAM and not recommended for other uses.
433  if (pin >= 26 && pin <= 32)
434  return false;
435  // When using Octal Flash or Octal PSRAM or both, GPIO33~37 are connected to SPIIO4 ~ SPIIO7 and SPIDQS.
436  if (pin >= 33 && pin <= 37)
437  return false;
438  // USB-JTAG: GPIO 19 and 20 are used by USB-JTAG by default.
439  return true;
440  }
441  return false;
442  #else
443  #error Unsupported ESP32 platform
444  return false;
445  #endif
446  }
447 
448  static int channelsRemaining()
449  {
450  return kNumPWM - privates()->PWMCount;
451  }
452 
453 private:
454  int fPin = -1;
455  double fFreq = -1;
456  int fTimerNum = -1;
457  uint32_t fDuty = 0;
458  int fPWMChannel = -1;
459  bool fAttached = false;
460  uint8_t fResolutionBits = 8;
461 
462  static inline float mapf(float x, float in_min, float in_max, float out_min, float out_max)
463  {
464  return (x > in_max) ? out_max : (x < in_min) ? out_min :
465  (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
466  }
467 
468  static double _ledcSetupTimerFreq(uint8_t chan, double freq, uint8_t bit_num)
469  {
470  return ledcSetup(chan, freq, bit_num);
471  }
472 
473  void attach(int pin)
474  {
475  fPin = pin;
476  fAttached = true;
477  }
478 
479  void adjustFrequencyLocal(double freq, float dutyScaled)
480  {
481  privates()->timerFreqSet[getTimer()] = (long) freq;
482  fFreq = freq;
483  if (attached())
484  {
485  ledcDetachPin(fPin);
486  // Remove the PWM during frequency adjust
487  _ledcSetupTimerFreq(getChannel(), freq, fResolutionBits);
488  writeScaled(dutyScaled);
489  ledcAttachPin(fPin, getChannel()); // re-attach the pin after frequency adjust
490  }
491  else
492  {
493  _ledcSetupTimerFreq(getChannel(), freq, fResolutionBits);
494  writeScaled(dutyScaled);
495  }
496  }
497 
498  double setup(double freq, uint8_t resolution_bits = 10)
499  {
500  auto priv = privates();
501  auto timerCount = priv->timerCount;
502  auto timerFreqSet = priv->timerFreqSet;
503  auto ChannelUsed = priv->ChannelUsed;
504 
505  long freqlocal = (long)freq;
506  if (fPWMChannel < 0)
507  {
508  bool found = false;
509  for (int i = 0; i < 4; i++)
510  {
511  bool freqAllocated = (timerFreqSet[i] == freqlocal || timerFreqSet[i] == -1);
512  if (freqAllocated && timerCount[i] < 4)
513  {
514  if (timerFreqSet[i] == -1)
515  timerFreqSet[i] = freqlocal;
516 
517  fTimerNum = i;
518  for (int index = 0; index < 4; index++)
519  {
520  int myTimerNumber = timerAndIndexToChannel(fTimerNum, index);
521  if (myTimerNumber >= 0 && ChannelUsed[myTimerNumber] == nullptr)
522  {
523  fPWMChannel = myTimerNumber;
524  ChannelUsed[fPWMChannel] = this;
525  timerCount[fTimerNum]++;
526  priv->PWMCount++;
527  fFreq = freq;
528  found = true;
529  break;
530  }
531  }
532  }
533  }
534  if (!found)
535  return 0;
536  }
537  for (int i = 0; i < priv->timerCount[getTimer()]; i++)
538  {
539  int pwm = timerAndIndexToChannel(getTimer(), i);
540 
541  if (pwm == fPWMChannel || ChannelUsed[pwm] == nullptr ||
542  ChannelUsed[pwm]->getTimer() != getTimer())
543  {
544  continue;
545  }
546  double diff = abs(ChannelUsed[pwm]->fFreq - freq);
547  if (abs(diff) > 0.1)
548  {
549  DEBUG_PRINTLN("WARNING: PWM channel conflict");
550  ChannelUsed[pwm]->fFreq = freq;
551  }
552  }
553 
554  fResolutionBits = resolution_bits;
555  if (attached())
556  {
557  ledcDetachPin(fPin);
558  double val = ledcSetup(getChannel(), freq, resolution_bits);
559  attachPin(fPin);
560  return val;
561  }
562  return ledcSetup(getChannel(), freq, resolution_bits);
563  }
564 
565  void attachPin(uint8_t pin)
566  {
567  if (!validPWM(pin))
568  {
569  DEBUG_PRINTLN("PWM pin not valid!");
570  return;
571  }
572  attach(pin);
573  ledcAttachPin(pin, getChannel());
574  }
575 
576  void deallocate()
577  {
578  if (fPWMChannel < 0)
579  return;
580  auto priv = privates();
581  if (--priv->timerCount[getTimer()] == 0)
582  {
583  priv->timerFreqSet[getTimer()] = -1; // last pwn closed out
584  }
585  fTimerNum = -1;
586  fAttached = false;
587  priv->ChannelUsed[fPWMChannel] = nullptr;
588  fPWMChannel = -1;
589  priv->PWMCount--;
590  }
591 };
592 
593 #endif
594 #endif
ServoDispatch.h
DEBUG_PRINTLN
#define DEBUG_PRINTLN(s)
Definition: ReelTwo.h:188
ServoDispatchPrivate.h
SERVOS_PER_TIMER
#define SERVOS_PER_TIMER
Definition: ServoDispatchPrivate.h:28
SERVO_INDEX
#define SERVO_INDEX(_timer, _channel)
Definition: ServoDispatchPrivate.h:30
MAX_SERVOS
#define MAX_SERVOS
Definition: ServoDispatchPrivate.h:29
ISR
ISR(TIMER1_COMPA_vect)
Definition: ServoDispatchPrivate.h:229