bendns' repos / ArduinoStuff
arduino stuffs
Diffstat (limited to 'libraries/AFMotor/AFMotor.cpp')
-rwxr-xr-xlibraries/AFMotor/AFMotor.cpp598
1 files changed, 0 insertions, 598 deletions
diff --git a/libraries/AFMotor/AFMotor.cpp b/libraries/AFMotor/AFMotor.cpp
deleted file mode 100755
index d3c90c6..0000000
--- a/libraries/AFMotor/AFMotor.cpp
+++ /dev/null
@@ -1,598 +0,0 @@
-// 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;
-}
-