bendns' repos / ArduinoStuff
arduino stuffs
Diffstat (limited to 'libraries/AFMotor/AFMotor.cpp')
-rwxr-xr-xlibraries/AFMotor/AFMotor.cpp598
1 files changed, 598 insertions, 0 deletions
diff --git a/libraries/AFMotor/AFMotor.cpp b/libraries/AFMotor/AFMotor.cpp
new file mode 100755
index 0000000..d3c90c6
--- /dev/null
+++ b/libraries/AFMotor/AFMotor.cpp
@@ -0,0 +1,598 @@
+// Adafruit Motor shield library
+// copyright Adafruit Industries LLC, 2009
+// this code is public domain, enjoy!
+
+// added Leonardo support - Michael Margolis, 24 July 2012
+
+#include <avr/io.h>
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#include "AFMotor.h"
+
+static uint8_t latch_state;
+
+#if (MICROSTEPS == 8)
+uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255};
+#elif (MICROSTEPS == 16)
+uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255};
+#endif
+
+AFMotorController::AFMotorController(void) {
+}
+
+void AFMotorController::enable(void) {
+ // setup the latch
+ /*
+ LATCH_DDR |= _BV(LATCH);
+ ENABLE_DDR |= _BV(ENABLE);
+ CLK_DDR |= _BV(CLK);
+ SER_DDR |= _BV(SER);
+ */
+ pinMode(MOTORLATCH, OUTPUT);
+ pinMode(MOTORENABLE, OUTPUT);
+ pinMode(MOTORDATA, OUTPUT);
+ pinMode(MOTORCLK, OUTPUT);
+
+ latch_state = 0;
+
+ latch_tx(); // "reset"
+
+ //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs!
+ digitalWrite(MOTORENABLE, LOW);
+}
+
+
+void AFMotorController::latch_tx(void) {
+ uint8_t i;
+
+ //LATCH_PORT &= ~_BV(LATCH);
+ digitalWrite(MOTORLATCH, LOW);
+
+ //SER_PORT &= ~_BV(SER);
+ digitalWrite(MOTORDATA, LOW);
+
+ for (i=0; i<8; i++) {
+ //CLK_PORT &= ~_BV(CLK);
+ digitalWrite(MOTORCLK, LOW);
+
+ if (latch_state & _BV(7-i)) {
+ //SER_PORT |= _BV(SER);
+ digitalWrite(MOTORDATA, HIGH);
+ } else {
+ //SER_PORT &= ~_BV(SER);
+ digitalWrite(MOTORDATA, LOW);
+ }
+ //CLK_PORT |= _BV(CLK);
+ digitalWrite(MOTORCLK, HIGH);
+ }
+ //LATCH_PORT |= _BV(LATCH);
+ digitalWrite(MOTORLATCH, HIGH);
+}
+
+static AFMotorController MC;
+
+
+/******************************************
+ MOTORS
+******************************************/
+inline void initPWM1(uint8_t freq) {
+
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer2A on PB3 (Arduino pin #11)
+ TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a
+ TCCR2B = freq & 0x7;
+ OCR2A = 0;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 11 is now PB5 (OC1A)
+ TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a
+ TCCR1B = (freq & 0x7) | _BV(WGM12);
+ OCR1A = 0;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM on timer0A (Arduino pin #11 on leo)
+ TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A
+ OCR0A = 0;
+
+#else
+ #error "This chip is not supported!"
+#endif
+ pinMode(11, OUTPUT);
+}
+
+inline void setPWM1(uint8_t s) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer2A on PB3 (Arduino pin #11)
+ OCR2A = s;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 11 is now PB5 (OC1A)
+ OCR1A = s;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM on timer0A (Arduino pin #11 pn leo)
+ OCR0A = s;
+#else
+ #error "This chip is not supported!"
+#endif
+}
+
+inline void initPWM2(uint8_t freq) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer2B (pin 3)
+ TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b
+ TCCR2B = freq & 0x7;
+ OCR2B = 0;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 3 is now PE5 (OC3C)
+ TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c
+ TCCR3B = (freq & 0x7) | _BV(WGM12);
+ OCR3C = 0;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM from timer0B (pin 3 on leo)
+ TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0B
+ OCR0B = 0;
+#else
+ #error "This chip is not supported!"
+#endif
+
+ pinMode(3, OUTPUT);
+}
+
+inline void setPWM2(uint8_t s) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer2B (pin 3)
+ OCR2B = s;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ OCR3C = s;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM from timer0B (pin 3 pin Leo)
+ OCR0B = s;
+#else
+ #error "This chip is not supported!"
+#endif
+}
+
+inline void initPWM3(uint8_t freq) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer0A / PD6 (pin 6)
+ TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A
+ //TCCR0B = freq & 0x7;
+ OCR0A = 0;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 6 is now PH3 (OC4A)
+ TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a
+ TCCR4B = (freq & 0x7) | _BV(WGM12);
+ //TCCR4B = 1 | _BV(WGM12);
+ OCR4A = 0;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM from timer4D (leo pin 6)
+ TCCR4B = _BV(CS42) | _BV(CS41) ; //| _BV(CS40);
+ TCCR4C = _BV(PWM4D) |_BV(COM4D1);
+ TCCR4D = _BV(WGM40);
+
+#else
+ #error "This chip is not supported!"
+#endif
+ pinMode(6, OUTPUT);
+}
+
+inline void setPWM3(uint8_t s) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer0A on PB3 (Arduino pin #6)
+ OCR0A = s;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 6 is now PH3 (OC4A)
+ OCR4A = s;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM from timer4D (leo pin 6)
+ OCR4D = s;
+#else
+ #error "This chip is not supported!"
+#endif
+}
+
+
+
+inline void initPWM4(uint8_t freq) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer0B / PD5 (pin 5)
+ TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a
+ //TCCR0B = freq & 0x7;
+ OCR0B = 0;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 5 is now PE3 (OC3A)
+ TCCR3A = _BV(COM1A1) ; // turn on oc3a
+ TCCR3B = (freq & 0x7) | _BV(WGM12);
+ //TCCR4B = 1 | _BV(WGM12);
+ OCR3A = 0;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 8 aug 2012)
+ // use PWM from timer3A (leo pin 5)
+ TCCR3B = 0;
+ TCCR3B |= _BV(CS31) | _BV(CS30);
+ TCCR3A = 0;
+ TCCR3A |= _BV(COM3A1) | _BV(WGM30); //turn on oc3a
+ OCR3A = 0;
+#else
+ #error "This chip is not supported!"
+#endif
+ pinMode(5, OUTPUT);
+}
+
+inline void setPWM4(uint8_t s) {
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega328P__)
+ // use PWM from timer0A on PB3 (Arduino pin #6)
+ OCR0B = s;
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+ // on arduino mega, pin 6 is now PH3 (OC4A)
+ OCR3A = s;
+#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012)
+ // use PWM from timer3A (leo pin 5)
+ OCR3A = s;
+#else
+ #error "This chip is not supported!"
+#endif
+}
+
+AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) {
+ motornum = num;
+ pwmfreq = freq;
+
+ MC.enable();
+
+ switch (num) {
+ case 1:
+ latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0
+ MC.latch_tx();
+ initPWM1(freq);
+ break;
+ case 2:
+ latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0
+ MC.latch_tx();
+ initPWM2(freq);
+ break;
+ case 3:
+ latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0
+ MC.latch_tx();
+ initPWM3(freq);
+ break;
+ case 4:
+ latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0
+ MC.latch_tx();
+ initPWM4(freq);
+ break;
+ }
+}
+
+void AF_DCMotor::run(uint8_t cmd) {
+ uint8_t a, b;
+ switch (motornum) {
+ case 1:
+ a = MOTOR1_A; b = MOTOR1_B; break;
+ case 2:
+ a = MOTOR2_A; b = MOTOR2_B; break;
+ case 3:
+ a = MOTOR3_A; b = MOTOR3_B; break;
+ case 4:
+ a = MOTOR4_A; b = MOTOR4_B; break;
+ default:
+ return;
+ }
+
+ switch (cmd) {
+ case FORWARD:
+ latch_state |= _BV(a);
+ latch_state &= ~_BV(b);
+ MC.latch_tx();
+ break;
+ case BACKWARD:
+ latch_state &= ~_BV(a);
+ latch_state |= _BV(b);
+ MC.latch_tx();
+ break;
+ case RELEASE:
+ latch_state &= ~_BV(a);
+ latch_state &= ~_BV(b);
+ MC.latch_tx();
+ break;
+ }
+}
+
+void AF_DCMotor::setSpeed(uint8_t speed) {
+ switch (motornum) {
+ case 1:
+ setPWM1(speed); break;
+ case 2:
+ setPWM2(speed); break;
+ case 3:
+ setPWM3(speed); break;
+ case 4:
+ setPWM4(speed); break;
+ }
+}
+
+/******************************************
+ STEPPERS
+******************************************/
+
+AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
+ MC.enable();
+
+ revsteps = steps;
+ steppernum = num;
+ currentstep = 0;
+
+ if (steppernum == 1) {
+ latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
+ ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
+ MC.latch_tx();
+
+ // enable both H bridges
+ pinMode(11, OUTPUT);
+ pinMode(3, OUTPUT);
+ digitalWrite(11, HIGH);
+ digitalWrite(3, HIGH);
+
+ // use PWM for microstepping support
+ initPWM1(MOTOR12_64KHZ);
+ initPWM2(MOTOR12_64KHZ);
+ setPWM1(255);
+ setPWM2(255);
+
+ } else if (steppernum == 2) {
+ latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
+ ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
+ MC.latch_tx();
+
+ // enable both H bridges
+ pinMode(5, OUTPUT);
+ pinMode(6, OUTPUT);
+ digitalWrite(5, HIGH);
+ digitalWrite(6, HIGH);
+
+ // use PWM for microstepping support
+ // use PWM for microstepping support
+ initPWM3(1);
+ initPWM4(1);
+ setPWM3(255);
+ setPWM4(255);
+ }
+}
+
+void AF_Stepper::setSpeed(uint16_t rpm) {
+ usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
+ steppingcounter = 0;
+}
+
+void AF_Stepper::release(void) {
+ if (steppernum == 1) {
+ latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
+ ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
+ MC.latch_tx();
+ } else if (steppernum == 2) {
+ latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
+ ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
+ MC.latch_tx();
+ }
+}
+
+void AF_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) {
+ uint32_t uspers = usperstep;
+ uint8_t ret = 0;
+
+ if (style == INTERLEAVE) {
+ uspers /= 2;
+ }
+ else if (style == MICROSTEP) {
+ uspers /= MICROSTEPS;
+ steps *= MICROSTEPS;
+#ifdef MOTORDEBUG
+ Serial.print("steps = "); Serial.println(steps, DEC);
+#endif
+ }
+
+ while (steps--) {
+ ret = onestep(dir, style);
+ delay(uspers/1000); // in ms
+ steppingcounter += (uspers % 1000);
+ if (steppingcounter >= 1000) {
+ delay(1);
+ steppingcounter -= 1000;
+ }
+ }
+ if (style == MICROSTEP) {
+ while ((ret != 0) && (ret != MICROSTEPS)) {
+ ret = onestep(dir, style);
+ delay(uspers/1000); // in ms
+ steppingcounter += (uspers % 1000);
+ if (steppingcounter >= 1000) {
+ delay(1);
+ steppingcounter -= 1000;
+ }
+ }
+ }
+}
+
+uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) {
+ uint8_t a, b, c, d;
+ uint8_t ocrb, ocra;
+
+ ocra = ocrb = 255;
+
+ if (steppernum == 1) {
+ a = _BV(MOTOR1_A);
+ b = _BV(MOTOR2_A);
+ c = _BV(MOTOR1_B);
+ d = _BV(MOTOR2_B);
+ } else if (steppernum == 2) {
+ a = _BV(MOTOR3_A);
+ b = _BV(MOTOR4_A);
+ c = _BV(MOTOR3_B);
+ d = _BV(MOTOR4_B);
+ } else {
+ return 0;
+ }
+
+ // next determine what sort of stepping procedure we're up to
+ if (style == SINGLE) {
+ if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird
+ if (dir == FORWARD) {
+ currentstep += MICROSTEPS/2;
+ }
+ else {
+ currentstep -= MICROSTEPS/2;
+ }
+ } else { // go to the next even step
+ if (dir == FORWARD) {
+ currentstep += MICROSTEPS;
+ }
+ else {
+ currentstep -= MICROSTEPS;
+ }
+ }
+ } else if (style == DOUBLE) {
+ if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird
+ if (dir == FORWARD) {
+ currentstep += MICROSTEPS/2;
+ } else {
+ currentstep -= MICROSTEPS/2;
+ }
+ } else { // go to the next odd step
+ if (dir == FORWARD) {
+ currentstep += MICROSTEPS;
+ } else {
+ currentstep -= MICROSTEPS;
+ }
+ }
+ } else if (style == INTERLEAVE) {
+ if (dir == FORWARD) {
+ currentstep += MICROSTEPS/2;
+ } else {
+ currentstep -= MICROSTEPS/2;
+ }
+ }
+
+ if (style == MICROSTEP) {
+ if (dir == FORWARD) {
+ currentstep++;
+ } else {
+ // BACKWARDS
+ currentstep--;
+ }
+
+ currentstep += MICROSTEPS*4;
+ currentstep %= MICROSTEPS*4;
+
+ ocra = ocrb = 0;
+ if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) {
+ ocra = microstepcurve[MICROSTEPS - currentstep];
+ ocrb = microstepcurve[currentstep];
+ } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) {
+ ocra = microstepcurve[currentstep - MICROSTEPS];
+ ocrb = microstepcurve[MICROSTEPS*2 - currentstep];
+ } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) {
+ ocra = microstepcurve[MICROSTEPS*3 - currentstep];
+ ocrb = microstepcurve[currentstep - MICROSTEPS*2];
+ } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) {
+ ocra = microstepcurve[currentstep - MICROSTEPS*3];
+ ocrb = microstepcurve[MICROSTEPS*4 - currentstep];
+ }
+ }
+
+ currentstep += MICROSTEPS*4;
+ currentstep %= MICROSTEPS*4;
+
+#ifdef MOTORDEBUG
+ Serial.print("current step: "); Serial.println(currentstep, DEC);
+ Serial.print(" pwmA = "); Serial.print(ocra, DEC);
+ Serial.print(" pwmB = "); Serial.println(ocrb, DEC);
+#endif
+
+ if (steppernum == 1) {
+ setPWM1(ocra);
+ setPWM2(ocrb);
+ } else if (steppernum == 2) {
+ setPWM3(ocra);
+ setPWM4(ocrb);
+ }
+
+
+ // release all
+ latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0
+
+ //Serial.println(step, DEC);
+ if (style == MICROSTEP) {
+ if ((currentstep >= 0) && (currentstep < MICROSTEPS))
+ latch_state |= a | b;
+ if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
+ latch_state |= b | c;
+ if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
+ latch_state |= c | d;
+ if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
+ latch_state |= d | a;
+ } else {
+ switch (currentstep/(MICROSTEPS/2)) {
+ case 0:
+ latch_state |= a; // energize coil 1 only
+ break;
+ case 1:
+ latch_state |= a | b; // energize coil 1+2
+ break;
+ case 2:
+ latch_state |= b; // energize coil 2 only
+ break;
+ case 3:
+ latch_state |= b | c; // energize coil 2+3
+ break;
+ case 4:
+ latch_state |= c; // energize coil 3 only
+ break;
+ case 5:
+ latch_state |= c | d; // energize coil 3+4
+ break;
+ case 6:
+ latch_state |= d; // energize coil 4 only
+ break;
+ case 7:
+ latch_state |= d | a; // energize coil 1+4
+ break;
+ }
+ }
+
+
+ MC.latch_tx();
+ return currentstep;
+}
+