2 #ifndef ARDUINO_ARCH_SAM 18 #if !defined(__AVR_ATmega32U4__) 19 #include "AFMotor.hpp" 23 static uint8_t latch_state;
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};
31 AFMotorController::AFMotorController(
void) {
32 TimerInitalized =
false;
35 void AFMotorController::enable(
void) {
43 pinMode(MOTORLATCH, OUTPUT);
44 pinMode(MOTORENABLE, OUTPUT);
45 pinMode(MOTORDATA, OUTPUT);
46 pinMode(MOTORCLK, OUTPUT);
53 digitalWrite(MOTORENABLE, LOW);
57 void AFMotorController::latch_tx(
void) {
61 digitalWrite(MOTORLATCH, LOW);
64 digitalWrite(MOTORDATA, LOW);
68 digitalWrite(MOTORCLK, LOW);
70 if (latch_state & _BV(7-i)) {
72 digitalWrite(MOTORDATA, HIGH);
75 digitalWrite(MOTORDATA, LOW);
78 digitalWrite(MOTORCLK, HIGH);
81 digitalWrite(MOTORLATCH, HIGH);
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__) 96 TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21);
99 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 101 TCCR1A |= _BV(COM1A1) | _BV(WGM10);
102 TCCR1B = (freq & 0x7) | _BV(WGM12);
104 #elif defined(__PIC32MX__) 105 #if defined(PIC32_USE_PIN9_FOR_M1_PWM) 109 if (!MC.TimerInitalized)
111 T2CON = 0x8000 | ((freq & 0x07) << 4);
114 MC.TimerInitalized =
true;
120 #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) 124 if (!MC.TimerInitalized)
126 T2CON = 0x8000 | ((freq & 0x07) << 4);
129 MC.TimerInitalized =
true;
137 digitalWrite(11, LOW);
140 #error "This chip is not supported!" 142 #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) 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__) 155 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 158 #elif defined(__PIC32MX__) 159 #if defined(PIC32_USE_PIN9_FOR_M1_PWM) 162 #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) 169 digitalWrite(11, HIGH);
173 digitalWrite(11, LOW);
177 #error "This chip is not supported!" 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__) 188 TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21);
191 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 193 TCCR3A |= _BV(COM1C1) | _BV(WGM10);
194 TCCR3B = (freq & 0x7) | _BV(WGM12);
196 #elif defined(__PIC32MX__) 197 if (!MC.TimerInitalized)
199 T2CON = 0x8000 | ((freq & 0x07) << 4);
202 MC.TimerInitalized =
true;
209 #error "This chip is not supported!" 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__) 223 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 226 #elif defined(__PIC32MX__) 230 #error "This chip is not supported!" 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__) 241 TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01);
244 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 246 TCCR4A |= _BV(COM1A1) | _BV(WGM10);
247 TCCR4B = (freq & 0x7) | _BV(WGM12);
250 #elif defined(__PIC32MX__) 251 if (!MC.TimerInitalized)
253 T2CON = 0x8000 | ((freq & 0x07) << 4);
256 MC.TimerInitalized =
true;
263 #error "This chip is not supported!" 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__) 276 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 279 #elif defined(__PIC32MX__) 283 #error "This chip is not supported!" 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__) 296 TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01);
299 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 301 TCCR3A |= _BV(COM1A1) | _BV(WGM10);
302 TCCR3B = (freq & 0x7) | _BV(WGM12);
305 #elif defined(__PIC32MX__) 306 if (!MC.TimerInitalized)
308 T2CON = 0x8000 | ((freq & 0x07) << 4);
311 MC.TimerInitalized =
true;
318 #error "This chip is not supported!" 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__) 331 #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 334 #elif defined(__PIC32MX__) 338 #error "This chip is not supported!" 342 AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) {
350 latch_state &= ~~_BV(MOTOR1_A) & ~~_BV(MOTOR1_B);
355 latch_state &= ~~_BV(MOTOR2_A) & ~~_BV(MOTOR2_B);
360 latch_state &= ~~_BV(MOTOR3_A) & ~~_BV(MOTOR3_B);
365 latch_state &= ~~_BV(MOTOR4_A) & ~~_BV(MOTOR4_B);
372 void AF_DCMotor::run(uint8_t cmd) {
376 a = MOTOR1_A; b = MOTOR1_B;
break;
378 a = MOTOR2_A; b = MOTOR2_B;
break;
380 a = MOTOR3_A; b = MOTOR3_B;
break;
382 a = MOTOR4_A; b = MOTOR4_B;
break;
389 latch_state |= _BV(a);
390 latch_state &= ~~_BV(b);
394 latch_state &= ~~_BV(a);
395 latch_state |= _BV(b);
399 latch_state &= ~~_BV(a);
400 latch_state &= ~~_BV(b);
406 void AF_DCMotor::setSpeed(uint8_t speed) {
409 setPWM1(speed);
break;
411 setPWM2(speed);
break;
413 setPWM3(speed);
break;
415 setPWM4(speed);
break;
423 AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
430 if (steppernum == 1) {
431 latch_state &= ~~_BV(MOTOR1_A) & ~~_BV(MOTOR1_B) &
432 ~~_BV(MOTOR2_A) & ~~_BV(MOTOR2_B);
438 digitalWrite(11, HIGH);
439 digitalWrite(3, HIGH);
442 initPWM1(STEPPER1_PWM_RATE);
443 initPWM2(STEPPER1_PWM_RATE);
447 }
else if (steppernum == 2) {
448 latch_state &= ~~_BV(MOTOR3_A) & ~~_BV(MOTOR3_B) &
449 ~~_BV(MOTOR4_A) & ~~_BV(MOTOR4_B);
455 digitalWrite(5, HIGH);
456 digitalWrite(6, HIGH);
460 initPWM3(STEPPER2_PWM_RATE);
461 initPWM4(STEPPER2_PWM_RATE);
467 void AF_Stepper::setSpeed(uint16_t rpm) {
468 usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
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);
477 }
else if (steppernum == 2) {
478 latch_state &= ~~_BV(MOTOR3_A) & ~~_BV(MOTOR3_B) &
479 ~~_BV(MOTOR4_A) & ~~_BV(MOTOR4_B);
484 void AF_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) {
485 uint32_t uspers = usperstep;
488 if (style == INTERLEAVE) {
491 else if (style == MICROSTEP) {
492 uspers /= MICROSTEPS;
495 Serial.print(
"steps = "); Serial.println(steps, DEC);
500 ret = onestep(dir, style);
502 steppingcounter += (uspers % 1000);
503 if (steppingcounter >= 1000) {
505 steppingcounter -= 1000;
508 if (style == MICROSTEP) {
509 while ((ret != 0) && (ret != MICROSTEPS)) {
510 ret = onestep(dir, style);
512 steppingcounter += (uspers % 1000);
513 if (steppingcounter >= 1000) {
515 steppingcounter -= 1000;
521 uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) {
527 if (steppernum == 1) {
532 }
else if (steppernum == 2) {
542 if (style == SINGLE) {
543 if ((currentstep/(MICROSTEPS/2)) % 2) {
544 if (dir == FORWARD) {
545 currentstep += MICROSTEPS/2;
548 currentstep -= MICROSTEPS/2;
551 if (dir == FORWARD) {
552 currentstep += MICROSTEPS;
555 currentstep -= MICROSTEPS;
558 }
else if (style == DOUBLE) {
559 if (! (currentstep/(MICROSTEPS/2) % 2)) {
560 if (dir == FORWARD) {
561 currentstep += MICROSTEPS/2;
563 currentstep -= MICROSTEPS/2;
566 if (dir == FORWARD) {
567 currentstep += MICROSTEPS;
569 currentstep -= MICROSTEPS;
572 }
else if (style == INTERLEAVE) {
573 if (dir == FORWARD) {
574 currentstep += MICROSTEPS/2;
576 currentstep -= MICROSTEPS/2;
580 if (style == MICROSTEP) {
581 if (dir == FORWARD) {
588 currentstep += MICROSTEPS*4;
589 currentstep %= MICROSTEPS*4;
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];
607 currentstep += MICROSTEPS*4;
608 currentstep %= MICROSTEPS*4;
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);
616 if (steppernum == 1) {
619 }
else if (steppernum == 2) {
626 latch_state &= ~~a & ~~b & ~~c & ~~d;
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;
639 switch (currentstep/(MICROSTEPS/2)) {
644 latch_state |= a | b;
650 latch_state |= b | c;
656 latch_state |= c | d;
662 latch_state |= d | a;