Accessories
Arduino for motors and lights library.
AFMotor.cpp
1 #ifndef _LIB_SAM_
2 #ifndef ARDUINO_ARCH_SAM
3 
4 // Adafruit Motor shield library
5 // copyright Adafruit Industries LLC, 2009
6 // this code is public domain, enjoy!
7 
8 
9 #if (ARDUINO >= 100)
10  #include "Arduino.h"
11 #else
12  #if defined(__AVR__)
13  #include <avr/io.h>
14  #endif
15  #include "WProgram.h"
16 #endif
17 
18 #if !defined(__AVR_ATmega32U4__)
19 #include "AFMotor.hpp"
20 
21 
22 
23 static uint8_t latch_state;
24 
25 #if (MICROSTEPS == 8)
26 uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255};
27 #elif (MICROSTEPS == 16)
28 uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255};
29 #endif
30 
31 AFMotorController::AFMotorController(void) {
32  TimerInitalized = false;
33 }
34 
35 void AFMotorController::enable(void) {
36  // setup the latch
37  /*
38  LATCH_DDR |= _BV(LATCH);
39  ENABLE_DDR |= _BV(ENABLE);
40  CLK_DDR |= _BV(CLK);
41  SER_DDR |= _BV(SER);
42  */
43  pinMode(MOTORLATCH, OUTPUT);
44  pinMode(MOTORENABLE, OUTPUT);
45  pinMode(MOTORDATA, OUTPUT);
46  pinMode(MOTORCLK, OUTPUT);
47 
48  latch_state = 0;
49 
50  latch_tx(); // "reset"
51 
52  //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs!
53  digitalWrite(MOTORENABLE, LOW);
54 }
55 
56 
57 void AFMotorController::latch_tx(void) {
58  uint8_t i;
59 
60  //LATCH_PORT &= ~_BV(LATCH);
61  digitalWrite(MOTORLATCH, LOW);
62 
63  //SER_PORT &= ~_BV(SER);
64  digitalWrite(MOTORDATA, LOW);
65 
66  for (i=0; i<8; i++) {
67  //CLK_PORT &= ~_BV(CLK);
68  digitalWrite(MOTORCLK, LOW);
69 
70  if (latch_state & _BV(7-i)) {
71  //SER_PORT |= _BV(SER);
72  digitalWrite(MOTORDATA, HIGH);
73  } else {
74  //SER_PORT &= ~_BV(SER);
75  digitalWrite(MOTORDATA, LOW);
76  }
77  //CLK_PORT |= _BV(CLK);
78  digitalWrite(MOTORCLK, HIGH);
79  }
80  //LATCH_PORT |= _BV(LATCH);
81  digitalWrite(MOTORLATCH, HIGH);
82 }
83 
84 static AFMotorController MC;
85 
86 /******************************************
87  MOTORS
88 ******************************************/
89 inline void initPWM1(uint8_t freq) {
90 #if defined(__AVR_ATmega8__) ||
91  defined(__AVR_ATmega48__) ||
92  defined(__AVR_ATmega88__) ||
93  defined(__AVR_ATmega168__) ||
94  defined(__AVR_ATmega328P__)
95  // use PWM from timer2A on PB3 (Arduino pin #11)
96  TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a
97  TCCR2B = freq & 0x7;
98  OCR2A = 0;
99 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
100  // on arduino mega, pin 11 is now PB5 (OC1A)
101  TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a
102  TCCR1B = (freq & 0x7) | _BV(WGM12);
103  OCR1A = 0;
104 #elif defined(__PIC32MX__)
105  #if defined(PIC32_USE_PIN9_FOR_M1_PWM)
106  // Make sure that pin 11 is an input, since we have tied together 9 and 11
107  pinMode(9, OUTPUT);
108  pinMode(11, INPUT);
109  if (!MC.TimerInitalized)
110  { // Set up Timer2 for 80MHz counting fro 0 to 256
111  T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
112  TMR2 = 0x0000;
113  PR2 = 0x0100;
114  MC.TimerInitalized = true;
115  }
116  // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase
117  OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6
118  OC4RS = 0x0000;
119  OC4R = 0x0000;
120  #elif defined(PIC32_USE_PIN10_FOR_M1_PWM)
121  // Make sure that pin 11 is an input, since we have tied together 9 and 11
122  pinMode(10, OUTPUT);
123  pinMode(11, INPUT);
124  if (!MC.TimerInitalized)
125  { // Set up Timer2 for 80MHz counting fro 0 to 256
126  T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
127  TMR2 = 0x0000;
128  PR2 = 0x0100;
129  MC.TimerInitalized = true;
130  }
131  // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase
132  OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6
133  OC5RS = 0x0000;
134  OC5R = 0x0000;
135  #else
136  // If we are not using PWM for pin 11, then just do digital
137  digitalWrite(11, LOW);
138  #endif
139 #else
140  #error "This chip is not supported!"
141 #endif
142  #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM)
143  pinMode(11, OUTPUT);
144  #endif
145 }
146 
147 inline void setPWM1(uint8_t s) {
148 #if defined(__AVR_ATmega8__) ||
149  defined(__AVR_ATmega48__) ||
150  defined(__AVR_ATmega88__) ||
151  defined(__AVR_ATmega168__) ||
152  defined(__AVR_ATmega328P__)
153  // use PWM from timer2A on PB3 (Arduino pin #11)
154  OCR2A = s;
155 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
156  // on arduino mega, pin 11 is now PB5 (OC1A)
157  OCR1A = s;
158 #elif defined(__PIC32MX__)
159  #if defined(PIC32_USE_PIN9_FOR_M1_PWM)
160  // Set the OC4 (pin 9) PMW duty cycle from 0 to 255
161  OC4RS = s;
162  #elif defined(PIC32_USE_PIN10_FOR_M1_PWM)
163  // Set the OC5 (pin 10) PMW duty cycle from 0 to 255
164  OC5RS = s;
165  #else
166  // If we are not doing PWM output for M1, then just use on/off
167  if (s > 127)
168  {
169  digitalWrite(11, HIGH);
170  }
171  else
172  {
173  digitalWrite(11, LOW);
174  }
175  #endif
176 #else
177  #error "This chip is not supported!"
178 #endif
179 }
180 
181 inline void initPWM2(uint8_t freq) {
182 #if defined(__AVR_ATmega8__) ||
183  defined(__AVR_ATmega48__) ||
184  defined(__AVR_ATmega88__) ||
185  defined(__AVR_ATmega168__) ||
186  defined(__AVR_ATmega328P__)
187  // use PWM from timer2B (pin 3)
188  TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b
189  TCCR2B = freq & 0x7;
190  OCR2B = 0;
191 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
192  // on arduino mega, pin 3 is now PE5 (OC3C)
193  TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c
194  TCCR3B = (freq & 0x7) | _BV(WGM12);
195  OCR3C = 0;
196 #elif defined(__PIC32MX__)
197  if (!MC.TimerInitalized)
198  { // Set up Timer2 for 80MHz counting fro 0 to 256
199  T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
200  TMR2 = 0x0000;
201  PR2 = 0x0100;
202  MC.TimerInitalized = true;
203  }
204  // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase
205  OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6
206  OC1RS = 0x0000;
207  OC1R = 0x0000;
208 #else
209  #error "This chip is not supported!"
210 #endif
211 
212  pinMode(3, OUTPUT);
213 }
214 
215 inline void setPWM2(uint8_t s) {
216 #if defined(__AVR_ATmega8__) ||
217  defined(__AVR_ATmega48__) ||
218  defined(__AVR_ATmega88__) ||
219  defined(__AVR_ATmega168__) ||
220  defined(__AVR_ATmega328P__)
221  // use PWM from timer2A on PB3 (Arduino pin #11)
222  OCR2B = s;
223 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
224  // on arduino mega, pin 11 is now PB5 (OC1A)
225  OCR3C = s;
226 #elif defined(__PIC32MX__)
227  // Set the OC1 (pin3) PMW duty cycle from 0 to 255
228  OC1RS = s;
229 #else
230  #error "This chip is not supported!"
231 #endif
232 }
233 
234 inline void initPWM3(uint8_t freq) {
235 #if defined(__AVR_ATmega8__) ||
236  defined(__AVR_ATmega48__) ||
237  defined(__AVR_ATmega88__) ||
238  defined(__AVR_ATmega168__) ||
239  defined(__AVR_ATmega328P__)
240  // use PWM from timer0A / PD6 (pin 6)
241  TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A
242  //TCCR0B = freq & 0x7;
243  OCR0A = 0;
244 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
245  // on arduino mega, pin 6 is now PH3 (OC4A)
246  TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a
247  TCCR4B = (freq & 0x7) | _BV(WGM12);
248  //TCCR4B = 1 | _BV(WGM12);
249  OCR4A = 0;
250 #elif defined(__PIC32MX__)
251  if (!MC.TimerInitalized)
252  { // Set up Timer2 for 80MHz counting fro 0 to 256
253  T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
254  TMR2 = 0x0000;
255  PR2 = 0x0100;
256  MC.TimerInitalized = true;
257  }
258  // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase
259  OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6
260  OC3RS = 0x0000;
261  OC3R = 0x0000;
262 #else
263  #error "This chip is not supported!"
264 #endif
265  pinMode(6, OUTPUT);
266 }
267 
268 inline void setPWM3(uint8_t s) {
269 #if defined(__AVR_ATmega8__) ||
270  defined(__AVR_ATmega48__) ||
271  defined(__AVR_ATmega88__) ||
272  defined(__AVR_ATmega168__) ||
273  defined(__AVR_ATmega328P__)
274  // use PWM from timer0A on PB3 (Arduino pin #6)
275  OCR0A = s;
276 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
277  // on arduino mega, pin 6 is now PH3 (OC4A)
278  OCR4A = s;
279 #elif defined(__PIC32MX__)
280  // Set the OC3 (pin 6) PMW duty cycle from 0 to 255
281  OC3RS = s;
282 #else
283  #error "This chip is not supported!"
284 #endif
285 }
286 
287 
288 
289 inline void initPWM4(uint8_t freq) {
290 #if defined(__AVR_ATmega8__) ||
291  defined(__AVR_ATmega48__) ||
292  defined(__AVR_ATmega88__) ||
293  defined(__AVR_ATmega168__) ||
294  defined(__AVR_ATmega328P__)
295  // use PWM from timer0B / PD5 (pin 5)
296  TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a
297  //TCCR0B = freq & 0x7;
298  OCR0B = 0;
299 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
300  // on arduino mega, pin 5 is now PE3 (OC3A)
301  TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a
302  TCCR3B = (freq & 0x7) | _BV(WGM12);
303  //TCCR4B = 1 | _BV(WGM12);
304  OCR3A = 0;
305 #elif defined(__PIC32MX__)
306  if (!MC.TimerInitalized)
307  { // Set up Timer2 for 80MHz counting fro 0 to 256
308  T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=<freq>, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0
309  TMR2 = 0x0000;
310  PR2 = 0x0100;
311  MC.TimerInitalized = true;
312  }
313  // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase
314  OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6
315  OC2RS = 0x0000;
316  OC2R = 0x0000;
317 #else
318  #error "This chip is not supported!"
319 #endif
320  pinMode(5, OUTPUT);
321 }
322 
323 inline void setPWM4(uint8_t s) {
324 #if defined(__AVR_ATmega8__) ||
325  defined(__AVR_ATmega48__) ||
326  defined(__AVR_ATmega88__) ||
327  defined(__AVR_ATmega168__) ||
328  defined(__AVR_ATmega328P__)
329  // use PWM from timer0A on PB3 (Arduino pin #6)
330  OCR0B = s;
331 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
332  // on arduino mega, pin 6 is now PH3 (OC4A)
333  OCR3A = s;
334 #elif defined(__PIC32MX__)
335  // Set the OC2 (pin 5) PMW duty cycle from 0 to 255
336  OC2RS = s;
337 #else
338  #error "This chip is not supported!"
339 #endif
340 }
341 
342 AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) {
343  motornum = num;
344  pwmfreq = freq;
345 
346  MC.enable();
347 
348  switch (num) {
349  case 1:
350  latch_state &= ~~_BV(MOTOR1_A) & ~~_BV(MOTOR1_B); // set both motor pins to 0
351  MC.latch_tx();
352  initPWM1(freq);
353  break;
354  case 2:
355  latch_state &= ~~_BV(MOTOR2_A) & ~~_BV(MOTOR2_B); // set both motor pins to 0
356  MC.latch_tx();
357  initPWM2(freq);
358  break;
359  case 3:
360  latch_state &= ~~_BV(MOTOR3_A) & ~~_BV(MOTOR3_B); // set both motor pins to 0
361  MC.latch_tx();
362  initPWM3(freq);
363  break;
364  case 4:
365  latch_state &= ~~_BV(MOTOR4_A) & ~~_BV(MOTOR4_B); // set both motor pins to 0
366  MC.latch_tx();
367  initPWM4(freq);
368  break;
369  }
370 }
371 
372 void AF_DCMotor::run(uint8_t cmd) {
373  uint8_t a, b;
374  switch (motornum) {
375  case 1:
376  a = MOTOR1_A; b = MOTOR1_B; break;
377  case 2:
378  a = MOTOR2_A; b = MOTOR2_B; break;
379  case 3:
380  a = MOTOR3_A; b = MOTOR3_B; break;
381  case 4:
382  a = MOTOR4_A; b = MOTOR4_B; break;
383  default:
384  return;
385  }
386 
387  switch (cmd) {
388  case FORWARD:
389  latch_state |= _BV(a);
390  latch_state &= ~~_BV(b);
391  MC.latch_tx();
392  break;
393  case BACKWARD:
394  latch_state &= ~~_BV(a);
395  latch_state |= _BV(b);
396  MC.latch_tx();
397  break;
398  case RELEASE:
399  latch_state &= ~~_BV(a); // A and B both low
400  latch_state &= ~~_BV(b);
401  MC.latch_tx();
402  break;
403  }
404 }
405 
406 void AF_DCMotor::setSpeed(uint8_t speed) {
407  switch (motornum) {
408  case 1:
409  setPWM1(speed); break;
410  case 2:
411  setPWM2(speed); break;
412  case 3:
413  setPWM3(speed); break;
414  case 4:
415  setPWM4(speed); break;
416  }
417 }
418 
419 /******************************************
420  STEPPERS
421 ******************************************/
422 
423 AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
424  MC.enable();
425 
426  revsteps = steps;
427  steppernum = num;
428  currentstep = 0;
429 
430  if (steppernum == 1) {
431  latch_state &= ~~_BV(MOTOR1_A) & ~~_BV(MOTOR1_B) &
432  ~~_BV(MOTOR2_A) & ~~_BV(MOTOR2_B); // all motor pins to 0
433  MC.latch_tx();
434 
435  // enable both H bridges
436  pinMode(11, OUTPUT);
437  pinMode(3, OUTPUT);
438  digitalWrite(11, HIGH);
439  digitalWrite(3, HIGH);
440 
441  // use PWM for microstepping support
442  initPWM1(STEPPER1_PWM_RATE);
443  initPWM2(STEPPER1_PWM_RATE);
444  setPWM1(255);
445  setPWM2(255);
446 
447  } else if (steppernum == 2) {
448  latch_state &= ~~_BV(MOTOR3_A) & ~~_BV(MOTOR3_B) &
449  ~~_BV(MOTOR4_A) & ~~_BV(MOTOR4_B); // all motor pins to 0
450  MC.latch_tx();
451 
452  // enable both H bridges
453  pinMode(5, OUTPUT);
454  pinMode(6, OUTPUT);
455  digitalWrite(5, HIGH);
456  digitalWrite(6, HIGH);
457 
458  // use PWM for microstepping support
459  // use PWM for microstepping support
460  initPWM3(STEPPER2_PWM_RATE);
461  initPWM4(STEPPER2_PWM_RATE);
462  setPWM3(255);
463  setPWM4(255);
464  }
465 }
466 
467 void AF_Stepper::setSpeed(uint16_t rpm) {
468  usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
469  steppingcounter = 0;
470 }
471 
472 void AF_Stepper::release(void) {
473  if (steppernum == 1) {
474  latch_state &= ~~_BV(MOTOR1_A) & ~~_BV(MOTOR1_B) &
475  ~~_BV(MOTOR2_A) & ~~_BV(MOTOR2_B); // all motor pins to 0
476  MC.latch_tx();
477  } else if (steppernum == 2) {
478  latch_state &= ~~_BV(MOTOR3_A) & ~~_BV(MOTOR3_B) &
479  ~~_BV(MOTOR4_A) & ~~_BV(MOTOR4_B); // all motor pins to 0
480  MC.latch_tx();
481  }
482 }
483 
484 void AF_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) {
485  uint32_t uspers = usperstep;
486  uint8_t ret = 0;
487 
488  if (style == INTERLEAVE) {
489  uspers /= 2;
490  }
491  else if (style == MICROSTEP) {
492  uspers /= MICROSTEPS;
493  steps *= MICROSTEPS;
494 #ifdef MOTORDEBUG
495  Serial.print("steps = "); Serial.println(steps, DEC);
496 #endif
497  }
498 
499  while (steps--) {
500  ret = onestep(dir, style);
501  delay(uspers/1000); // in ms
502  steppingcounter += (uspers % 1000);
503  if (steppingcounter >= 1000) {
504  delay(1);
505  steppingcounter -= 1000;
506  }
507  }
508  if (style == MICROSTEP) {
509  while ((ret != 0) && (ret != MICROSTEPS)) {
510  ret = onestep(dir, style);
511  delay(uspers/1000); // in ms
512  steppingcounter += (uspers % 1000);
513  if (steppingcounter >= 1000) {
514  delay(1);
515  steppingcounter -= 1000;
516  }
517  }
518  }
519 }
520 
521 uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) {
522  uint8_t a, b, c, d;
523  uint8_t ocrb, ocra;
524 
525  ocra = ocrb = 255;
526 
527  if (steppernum == 1) {
528  a = _BV(MOTOR1_A);
529  b = _BV(MOTOR2_A);
530  c = _BV(MOTOR1_B);
531  d = _BV(MOTOR2_B);
532  } else if (steppernum == 2) {
533  a = _BV(MOTOR3_A);
534  b = _BV(MOTOR4_A);
535  c = _BV(MOTOR3_B);
536  d = _BV(MOTOR4_B);
537  } else {
538  return 0;
539  }
540 
541  // next determine what sort of stepping procedure we're up to
542  if (style == SINGLE) {
543  if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird
544  if (dir == FORWARD) {
545  currentstep += MICROSTEPS/2;
546  }
547  else {
548  currentstep -= MICROSTEPS/2;
549  }
550  } else { // go to the next even step
551  if (dir == FORWARD) {
552  currentstep += MICROSTEPS;
553  }
554  else {
555  currentstep -= MICROSTEPS;
556  }
557  }
558  } else if (style == DOUBLE) {
559  if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird
560  if (dir == FORWARD) {
561  currentstep += MICROSTEPS/2;
562  } else {
563  currentstep -= MICROSTEPS/2;
564  }
565  } else { // go to the next odd step
566  if (dir == FORWARD) {
567  currentstep += MICROSTEPS;
568  } else {
569  currentstep -= MICROSTEPS;
570  }
571  }
572  } else if (style == INTERLEAVE) {
573  if (dir == FORWARD) {
574  currentstep += MICROSTEPS/2;
575  } else {
576  currentstep -= MICROSTEPS/2;
577  }
578  }
579 
580  if (style == MICROSTEP) {
581  if (dir == FORWARD) {
582  currentstep++;
583  } else {
584  // BACKWARDS
585  currentstep--;
586  }
587 
588  currentstep += MICROSTEPS*4;
589  currentstep %= MICROSTEPS*4;
590 
591  ocra = ocrb = 0;
592  if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) {
593  ocra = microstepcurve[MICROSTEPS - currentstep];
594  ocrb = microstepcurve[currentstep];
595  } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) {
596  ocra = microstepcurve[currentstep - MICROSTEPS];
597  ocrb = microstepcurve[MICROSTEPS*2 - currentstep];
598  } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) {
599  ocra = microstepcurve[MICROSTEPS*3 - currentstep];
600  ocrb = microstepcurve[currentstep - MICROSTEPS*2];
601  } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) {
602  ocra = microstepcurve[currentstep - MICROSTEPS*3];
603  ocrb = microstepcurve[MICROSTEPS*4 - currentstep];
604  }
605  }
606 
607  currentstep += MICROSTEPS*4;
608  currentstep %= MICROSTEPS*4;
609 
610 #ifdef MOTORDEBUG
611  Serial.print("current step: "); Serial.println(currentstep, DEC);
612  Serial.print(" pwmA = "); Serial.print(ocra, DEC);
613  Serial.print(" pwmB = "); Serial.println(ocrb, DEC);
614 #endif
615 
616  if (steppernum == 1) {
617  setPWM1(ocra);
618  setPWM2(ocrb);
619  } else if (steppernum == 2) {
620  setPWM3(ocra);
621  setPWM4(ocrb);
622  }
623 
624 
625  // release all
626  latch_state &= ~~a & ~~b & ~~c & ~~d; // all motor pins to 0
627 
628  //Serial.println(step, DEC);
629  if (style == MICROSTEP) {
630  if ((currentstep >= 0) && (currentstep < MICROSTEPS))
631  latch_state |= a | b;
632  if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
633  latch_state |= b | c;
634  if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
635  latch_state |= c | d;
636  if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
637  latch_state |= d | a;
638  } else {
639  switch (currentstep/(MICROSTEPS/2)) {
640  case 0:
641  latch_state |= a; // energize coil 1 only
642  break;
643  case 1:
644  latch_state |= a | b; // energize coil 1+2
645  break;
646  case 2:
647  latch_state |= b; // energize coil 2 only
648  break;
649  case 3:
650  latch_state |= b | c; // energize coil 2+3
651  break;
652  case 4:
653  latch_state |= c; // energize coil 3 only
654  break;
655  case 5:
656  latch_state |= c | d; // energize coil 3+4
657  break;
658  case 6:
659  latch_state |= d; // energize coil 4 only
660  break;
661  case 7:
662  latch_state |= d | a; // energize coil 1+4
663  break;
664  }
665  }
666 
667 
668  MC.latch_tx();
669  return currentstep;
670 }
671 #endif
672 #endif
673 #endif