essai 1
This commit is contained in:
commit
3aac06ae17
|
@ -0,0 +1,6 @@
|
|||
void setup()
|
||||
{
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
}
|
|
@ -0,0 +1,563 @@
|
|||
// Adafruit Motor shield library
|
||||
// copyright Adafruit Industries LLC, 2009
|
||||
// this code is public domain, enjoy!
|
||||
|
||||
|
||||
#if (ARDUINO >= 100)
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include <avr/io.h>
|
||||
#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;
|
||||
#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;
|
||||
#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;
|
||||
#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 timer2A on PB3 (Arduino pin #11)
|
||||
OCR2B = s;
|
||||
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||
// on arduino mega, pin 11 is now PB5 (OC1A)
|
||||
OCR3C = 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;
|
||||
#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;
|
||||
#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) | _BV(WGM10); // fast PWM, turn on oc3a
|
||||
TCCR3B = (freq & 0x7) | _BV(WGM12);
|
||||
//TCCR4B = 1 | _BV(WGM12);
|
||||
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;
|
||||
#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 / (revsteps * 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
// Adafruit Motor shield library
|
||||
// copyright Adafruit Industries LLC, 2009
|
||||
// this code is public domain, enjoy!
|
||||
|
||||
#ifndef _AFMotor_h_
|
||||
#define _AFMotor_h_
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <avr/io.h>
|
||||
|
||||
//#define MOTORDEBUG 1
|
||||
|
||||
#define MICROSTEPS 16 // 8 or 16
|
||||
|
||||
#define MOTOR12_64KHZ _BV(CS20) // no prescale
|
||||
#define MOTOR12_8KHZ _BV(CS21) // divide by 8
|
||||
#define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32
|
||||
#define MOTOR12_1KHZ _BV(CS22) // divide by 64
|
||||
|
||||
#define MOTOR34_64KHZ _BV(CS00) // no prescale
|
||||
#define MOTOR34_8KHZ _BV(CS01) // divide by 8
|
||||
#define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64
|
||||
|
||||
#define MOTOR1_A 2
|
||||
#define MOTOR1_B 3
|
||||
#define MOTOR2_A 1
|
||||
#define MOTOR2_B 4
|
||||
#define MOTOR4_A 0
|
||||
#define MOTOR4_B 6
|
||||
#define MOTOR3_A 5
|
||||
#define MOTOR3_B 7
|
||||
|
||||
#define FORWARD 1
|
||||
#define BACKWARD 2
|
||||
#define BRAKE 3
|
||||
#define RELEASE 4
|
||||
|
||||
#define SINGLE 1
|
||||
#define DOUBLE 2
|
||||
#define INTERLEAVE 3
|
||||
#define MICROSTEP 4
|
||||
|
||||
/*
|
||||
#define LATCH 4
|
||||
#define LATCH_DDR DDRB
|
||||
#define LATCH_PORT PORTB
|
||||
|
||||
#define CLK_PORT PORTD
|
||||
#define CLK_DDR DDRD
|
||||
#define CLK 4
|
||||
|
||||
#define ENABLE_PORT PORTD
|
||||
#define ENABLE_DDR DDRD
|
||||
#define ENABLE 7
|
||||
|
||||
#define SER 0
|
||||
#define SER_DDR DDRB
|
||||
#define SER_PORT PORTB
|
||||
*/
|
||||
|
||||
// Arduino pin names
|
||||
#define MOTORLATCH 12
|
||||
#define MOTORCLK 4
|
||||
#define MOTORENABLE 7
|
||||
#define MOTORDATA 8
|
||||
|
||||
class AFMotorController
|
||||
{
|
||||
public:
|
||||
AFMotorController(void);
|
||||
void enable(void);
|
||||
friend class AF_DCMotor;
|
||||
void latch_tx(void);
|
||||
};
|
||||
|
||||
class AF_DCMotor
|
||||
{
|
||||
public:
|
||||
AF_DCMotor(uint8_t motornum, uint8_t freq = MOTOR34_8KHZ);
|
||||
void run(uint8_t);
|
||||
void setSpeed(uint8_t);
|
||||
|
||||
private:
|
||||
uint8_t motornum, pwmfreq;
|
||||
};
|
||||
|
||||
class AF_Stepper {
|
||||
public:
|
||||
AF_Stepper(uint16_t, uint8_t);
|
||||
void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE);
|
||||
void setSpeed(uint16_t);
|
||||
uint8_t onestep(uint8_t dir, uint8_t style);
|
||||
void release(void);
|
||||
uint16_t revsteps; // # steps per revolution
|
||||
uint8_t steppernum;
|
||||
uint32_t usperstep, steppingcounter;
|
||||
private:
|
||||
uint8_t currentstep;
|
||||
|
||||
};
|
||||
|
||||
uint8_t getlatchstate(void);
|
||||
|
||||
#endif
|
|
@ -0,0 +1,5 @@
|
|||
This is the August 12, 2009 Adafruit Motor shield firmware with basic Microstepping support. Works with all Arduinos and the Mega
|
||||
|
||||
For more information on the shield, please visit http://www.ladyada.net/make/mshield/
|
||||
|
||||
To install, click DOWNLOAD SOURCE in the top right corner, and see our tutorial at http://www.ladyada.net/library/arduino/libraries.html on Arduino Library installation
|
|
@ -0,0 +1,60 @@
|
|||
// Adafruit Motor shield library
|
||||
// copyright Adafruit Industries LLC, 2009
|
||||
// this code is public domain, enjoy!
|
||||
|
||||
#include <AFMotor.h>
|
||||
#include <ServoTimer1.h>
|
||||
|
||||
// DC motor on M2
|
||||
AF_DCMotor motor(2);
|
||||
// DC hobby servo
|
||||
ServoTimer1 servo1;
|
||||
// Stepper motor on M3+M4 48 steps per revolution
|
||||
AF_Stepper stepper(48, 2);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600); // set up Serial library at 9600 bps
|
||||
Serial.println("Motor party!");
|
||||
|
||||
// turn on servo
|
||||
servo1.attach(9);
|
||||
|
||||
// turn on motor #2
|
||||
motor.setSpeed(200);
|
||||
motor.run(RELEASE);
|
||||
}
|
||||
|
||||
int i;
|
||||
|
||||
// Test the DC motor, stepper and servo ALL AT ONCE!
|
||||
void loop() {
|
||||
motor.run(FORWARD);
|
||||
for (i=0; i<255; i++) {
|
||||
servo1.write(i);
|
||||
motor.setSpeed(i);
|
||||
stepper.step(1, FORWARD, INTERLEAVE);
|
||||
delay(3);
|
||||
}
|
||||
|
||||
for (i=255; i!=0; i--) {
|
||||
servo1.write(i-255);
|
||||
motor.setSpeed(i);
|
||||
stepper.step(1, BACKWARD, INTERLEAVE);
|
||||
delay(3);
|
||||
}
|
||||
|
||||
motor.run(BACKWARD);
|
||||
for (i=0; i<255; i++) {
|
||||
servo1.write(i);
|
||||
motor.setSpeed(i);
|
||||
delay(3);
|
||||
stepper.step(1, FORWARD, DOUBLE);
|
||||
}
|
||||
|
||||
for (i=255; i!=0; i--) {
|
||||
servo1.write(i-255);
|
||||
motor.setSpeed(i);
|
||||
stepper.step(1, BACKWARD, DOUBLE);
|
||||
delay(3);
|
||||
}
|
||||
}
|
|
@ -0,0 +1,52 @@
|
|||
// Adafruit Motor shield library
|
||||
// copyright Adafruit Industries LLC, 2009
|
||||
// this code is public domain, enjoy!
|
||||
|
||||
#include <AFMotor.h>
|
||||
|
||||
AF_DCMotor motor(4);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600); // set up Serial library at 9600 bps
|
||||
Serial.println("Motor test!");
|
||||
|
||||
// turn on motor
|
||||
motor.setSpeed(200);
|
||||
|
||||
motor.run(RELEASE);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
uint8_t i;
|
||||
|
||||
Serial.print("tick");
|
||||
|
||||
motor.run(FORWARD);
|
||||
for (i=0; i<255; i++) {
|
||||
motor.setSpeed(i);
|
||||
delay(10);
|
||||
}
|
||||
|
||||
for (i=255; i!=0; i--) {
|
||||
motor.setSpeed(i);
|
||||
delay(10);
|
||||
}
|
||||
|
||||
Serial.print("tock");
|
||||
|
||||
motor.run(BACKWARD);
|
||||
for (i=0; i<255; i++) {
|
||||
motor.setSpeed(i);
|
||||
delay(10);
|
||||
}
|
||||
|
||||
for (i=255; i!=0; i--) {
|
||||
motor.setSpeed(i);
|
||||
delay(10);
|
||||
}
|
||||
|
||||
|
||||
Serial.print("tech");
|
||||
motor.run(RELEASE);
|
||||
delay(1000);
|
||||
}
|
|
@ -0,0 +1,34 @@
|
|||
// Adafruit Motor shield library
|
||||
// copyright Adafruit Industries LLC, 2009
|
||||
// this code is public domain, enjoy!
|
||||
|
||||
#include <AFMotor.h>
|
||||
|
||||
// Connect a stepper motor with 48 steps per revolution (7.5 degree)
|
||||
// to motor port #2 (M3 and M4)
|
||||
AF_Stepper motor(48, 2);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600); // set up Serial library at 9600 bps
|
||||
Serial.println("Stepper test!");
|
||||
|
||||
motor.setSpeed(10); // 10 rpm
|
||||
}
|
||||
|
||||
void loop() {
|
||||
Serial.println("Single coil steps");
|
||||
motor.step(100, FORWARD, SINGLE);
|
||||
motor.step(100, BACKWARD, SINGLE);
|
||||
|
||||
Serial.println("Double coil steps");
|
||||
motor.step(100, FORWARD, DOUBLE);
|
||||
motor.step(100, BACKWARD, DOUBLE);
|
||||
|
||||
Serial.println("Interleave coil steps");
|
||||
motor.step(100, FORWARD, INTERLEAVE);
|
||||
motor.step(100, BACKWARD, INTERLEAVE);
|
||||
|
||||
Serial.println("Micrsostep steps");
|
||||
motor.step(100, FORWARD, MICROSTEP);
|
||||
motor.step(100, BACKWARD, MICROSTEP);
|
||||
}
|
|
@ -0,0 +1,35 @@
|
|||
#######################################
|
||||
# Syntax Coloring Map for AFMotor
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
AF_DCMotor KEYWORD1
|
||||
AF_Stepper KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
enable KEYWORD2
|
||||
run KEYWORD2
|
||||
setSpeed KEYWORD2
|
||||
step KEYWORD2
|
||||
onestep KEYWORD2
|
||||
release KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
MICROSTEPPING LITERAL1
|
||||
FORWARD LITERAL1
|
||||
BACKWARD LITERAL1
|
||||
BRAKE LITERAL1
|
||||
RELEASE LITERAL1
|
||||
SINGLE LITERAL1
|
||||
DOUBLE LITERAL1
|
||||
INTERLEAVE LITERAL1
|
||||
MICROSTEP LITERAL1
|
|
@ -0,0 +1,624 @@
|
|||
// AccelStepper.cpp
|
||||
//
|
||||
// Copyright (C) 2009-2013 Mike McCauley
|
||||
// $Id: AccelStepper.cpp,v 1.17 2013/08/02 01:53:21 mikem Exp mikem $
|
||||
|
||||
#include "AccelStepper.h"
|
||||
|
||||
#if 0
|
||||
// Some debugging assistance
|
||||
void dump(uint8_t* p, int l)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < l; i++)
|
||||
{
|
||||
Serial.print(p[i], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println("");
|
||||
}
|
||||
#endif
|
||||
|
||||
void AccelStepper::moveTo(long absolute)
|
||||
{
|
||||
if (_targetPos != absolute)
|
||||
{
|
||||
_targetPos = absolute;
|
||||
computeNewSpeed();
|
||||
// compute new n?
|
||||
}
|
||||
}
|
||||
|
||||
void AccelStepper::move(long relative)
|
||||
{
|
||||
moveTo(_currentPos + relative);
|
||||
}
|
||||
|
||||
// Implements steps according to the current step interval
|
||||
// You must call this at least once per step
|
||||
// returns true if a step occurred
|
||||
boolean AccelStepper::runSpeed()
|
||||
{
|
||||
// Dont do anything unless we actually have a step interval
|
||||
if (!_stepInterval)
|
||||
return false;
|
||||
|
||||
unsigned long time = micros();
|
||||
// Gymnastics to detect wrapping of either the nextStepTime and/or the current time
|
||||
unsigned long nextStepTime = _lastStepTime + _stepInterval;
|
||||
if ( ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
|
||||
|| ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime))))
|
||||
|
||||
{
|
||||
if (_direction == DIRECTION_CW)
|
||||
{
|
||||
// Clockwise
|
||||
_currentPos += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Anticlockwise
|
||||
_currentPos -= 1;
|
||||
}
|
||||
step(_currentPos);
|
||||
|
||||
_lastStepTime = time;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
long AccelStepper::distanceToGo()
|
||||
{
|
||||
return _targetPos - _currentPos;
|
||||
}
|
||||
|
||||
long AccelStepper::targetPosition()
|
||||
{
|
||||
return _targetPos;
|
||||
}
|
||||
|
||||
long AccelStepper::currentPosition()
|
||||
{
|
||||
return _currentPos;
|
||||
}
|
||||
|
||||
// Useful during initialisations or after initial positioning
|
||||
// Sets speed to 0
|
||||
void AccelStepper::setCurrentPosition(long position)
|
||||
{
|
||||
_targetPos = _currentPos = position;
|
||||
_n = 0;
|
||||
_stepInterval = 0;
|
||||
}
|
||||
|
||||
void AccelStepper::computeNewSpeed()
|
||||
{
|
||||
long distanceTo = distanceToGo(); // +ve is clockwise from curent location
|
||||
|
||||
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
|
||||
|
||||
if (distanceTo == 0 && stepsToStop <= 1)
|
||||
{
|
||||
// We are at the target and its time to stop
|
||||
_stepInterval = 0;
|
||||
_speed = 0.0;
|
||||
_n = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (distanceTo > 0)
|
||||
{
|
||||
// We are anticlockwise from the target
|
||||
// Need to go clockwise from here, maybe decelerate now
|
||||
if (_n > 0)
|
||||
{
|
||||
// Currently accelerating, need to decel now? Or maybe going the wrong way?
|
||||
if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
|
||||
_n = -stepsToStop; // Start deceleration
|
||||
}
|
||||
else if (_n < 0)
|
||||
{
|
||||
// Currently decelerating, need to accel again?
|
||||
if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
|
||||
_n = -_n; // Start accceleration
|
||||
}
|
||||
}
|
||||
else if (distanceTo < 0)
|
||||
{
|
||||
// We are clockwise from the target
|
||||
// Need to go anticlockwise from here, maybe decelerate
|
||||
if (_n > 0)
|
||||
{
|
||||
// Currently accelerating, need to decel now? Or maybe going the wrong way?
|
||||
if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
|
||||
_n = -stepsToStop; // Start deceleration
|
||||
}
|
||||
else if (_n < 0)
|
||||
{
|
||||
// Currently decelerating, need to accel again?
|
||||
if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
|
||||
_n = -_n; // Start accceleration
|
||||
}
|
||||
}
|
||||
|
||||
// Need to accelerate or decelerate
|
||||
if (_n == 0)
|
||||
{
|
||||
// First step from stopped
|
||||
_cn = _c0;
|
||||
_direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
|
||||
_cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
|
||||
_cn = max(_cn, _cmin);
|
||||
}
|
||||
_n++;
|
||||
_stepInterval = _cn;
|
||||
_speed = 1000000.0 / _cn;
|
||||
if (_direction == DIRECTION_CCW)
|
||||
_speed = -_speed;
|
||||
|
||||
#if 0
|
||||
Serial.println(_speed);
|
||||
Serial.println(_acceleration);
|
||||
Serial.println(_cn);
|
||||
Serial.println(_c0);
|
||||
Serial.println(_n);
|
||||
Serial.println(_stepInterval);
|
||||
Serial.println(distanceTo);
|
||||
Serial.println(stepsToStop);
|
||||
Serial.println("-----");
|
||||
#endif
|
||||
}
|
||||
|
||||
// Run the motor to implement speed and acceleration in order to proceed to the target position
|
||||
// You must call this at least once per step, preferably in your main loop
|
||||
// If the motor is in the desired position, the cost is very small
|
||||
// returns true if the motor is still running to the target position.
|
||||
boolean AccelStepper::run()
|
||||
{
|
||||
if (runSpeed())
|
||||
computeNewSpeed();
|
||||
return _speed != 0.0 || distanceToGo() != 0;
|
||||
}
|
||||
|
||||
AccelStepper::AccelStepper(uint8_t interface, uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, bool enable)
|
||||
{
|
||||
_interface = interface;
|
||||
_currentPos = 0;
|
||||
_targetPos = 0;
|
||||
_speed = 0.0;
|
||||
_maxSpeed = 1.0;
|
||||
_acceleration = 1.0;
|
||||
_sqrt_twoa = 1.0;
|
||||
_stepInterval = 0;
|
||||
_minPulseWidth = 1;
|
||||
_enablePin = 0xff;
|
||||
_lastStepTime = 0;
|
||||
_pin[0] = pin1;
|
||||
_pin[1] = pin2;
|
||||
_pin[2] = pin3;
|
||||
_pin[3] = pin4;
|
||||
|
||||
// NEW
|
||||
_n = 0;
|
||||
_c0 = 0.0;
|
||||
_cn = 0.0;
|
||||
_cmin = 1.0;
|
||||
_direction = DIRECTION_CCW;
|
||||
|
||||
int i;
|
||||
for (i = 0; i < 4; i++)
|
||||
_pinInverted[i] = 0;
|
||||
if (enable)
|
||||
enableOutputs();
|
||||
}
|
||||
|
||||
AccelStepper::AccelStepper(void (*forward)(), void (*backward)())
|
||||
{
|
||||
_interface = 0;
|
||||
_currentPos = 0;
|
||||
_targetPos = 0;
|
||||
_speed = 0.0;
|
||||
_maxSpeed = 1.0;
|
||||
_acceleration = 1.0;
|
||||
_sqrt_twoa = 1.0;
|
||||
_stepInterval = 0;
|
||||
_minPulseWidth = 1;
|
||||
_enablePin = 0xff;
|
||||
_lastStepTime = 0;
|
||||
_pin[0] = 0;
|
||||
_pin[1] = 0;
|
||||
_pin[2] = 0;
|
||||
_pin[3] = 0;
|
||||
_forward = forward;
|
||||
_backward = backward;
|
||||
|
||||
// NEW
|
||||
_n = 0;
|
||||
_c0 = 0.0;
|
||||
_cn = 0.0;
|
||||
_cmin = 1.0;
|
||||
_direction = DIRECTION_CCW;
|
||||
|
||||
int i;
|
||||
for (i = 0; i < 4; i++)
|
||||
_pinInverted[i] = 0;
|
||||
}
|
||||
|
||||
void AccelStepper::setMaxSpeed(float speed)
|
||||
{
|
||||
if (_maxSpeed != speed)
|
||||
{
|
||||
_maxSpeed = speed;
|
||||
_cmin = 1000000.0 / speed;
|
||||
// Recompute _n from current speed and adjust speed if accelerating or cruising
|
||||
if (_n > 0)
|
||||
{
|
||||
_n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
|
||||
computeNewSpeed();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AccelStepper::setAcceleration(float acceleration)
|
||||
{
|
||||
if (acceleration == 0.0)
|
||||
return;
|
||||
if (_acceleration != acceleration)
|
||||
{
|
||||
// Recompute _n per Equation 17
|
||||
_n = _n * (_acceleration / acceleration);
|
||||
// New c0 per Equation 7
|
||||
_c0 = sqrt(2.0 / acceleration) * 1000000.0;
|
||||
_acceleration = acceleration;
|
||||
computeNewSpeed();
|
||||
}
|
||||
}
|
||||
|
||||
void AccelStepper::setSpeed(float speed)
|
||||
{
|
||||
if (speed == _speed)
|
||||
return;
|
||||
speed = constrain(speed, -_maxSpeed, _maxSpeed);
|
||||
if (speed == 0.0)
|
||||
_stepInterval = 0;
|
||||
else
|
||||
{
|
||||
_stepInterval = fabs(1000000.0 / speed);
|
||||
_direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW;
|
||||
}
|
||||
_speed = speed;
|
||||
}
|
||||
|
||||
float AccelStepper::speed()
|
||||
{
|
||||
return _speed;
|
||||
}
|
||||
|
||||
// Subclasses can override
|
||||
void AccelStepper::step(long step)
|
||||
{
|
||||
switch (_interface)
|
||||
{
|
||||
case FUNCTION:
|
||||
step0(step);
|
||||
break;
|
||||
|
||||
case DRIVER:
|
||||
step1(step);
|
||||
break;
|
||||
|
||||
case FULL2WIRE:
|
||||
step2(step);
|
||||
break;
|
||||
|
||||
case FULL3WIRE:
|
||||
step3(step);
|
||||
break;
|
||||
|
||||
case FULL4WIRE:
|
||||
step4(step);
|
||||
break;
|
||||
|
||||
case HALF3WIRE:
|
||||
step6(step);
|
||||
break;
|
||||
|
||||
case HALF4WIRE:
|
||||
step8(step);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// You might want to override this to implement eg serial output
|
||||
// bit 0 of the mask corresponds to _pin[0]
|
||||
// bit 1 of the mask corresponds to _pin[1]
|
||||
// ....
|
||||
void AccelStepper::setOutputPins(uint8_t mask)
|
||||
{
|
||||
uint8_t numpins = 2;
|
||||
if (_interface == FULL4WIRE || _interface == HALF4WIRE)
|
||||
numpins = 4;
|
||||
uint8_t i;
|
||||
for (i = 0; i < numpins; i++)
|
||||
digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i]));
|
||||
}
|
||||
|
||||
// 0 pin step function (ie for functional usage)
|
||||
void AccelStepper::step0(long step)
|
||||
{
|
||||
if (_speed > 0)
|
||||
_forward();
|
||||
else
|
||||
_backward();
|
||||
}
|
||||
|
||||
// 1 pin step function (ie for stepper drivers)
|
||||
// This is passed the current step number (0 to 7)
|
||||
// Subclasses can override
|
||||
void AccelStepper::step1(long step)
|
||||
{
|
||||
// _pin[0] is step, _pin[1] is direction
|
||||
setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses
|
||||
setOutputPins(_direction ? 0b11 : 0b01); // step HIGH
|
||||
// Caution 200ns setup time
|
||||
// Delay the minimum allowed pulse width
|
||||
delayMicroseconds(_minPulseWidth);
|
||||
setOutputPins(_direction ? 0b10 : 0b00); // step LOW
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 2 pin step function
|
||||
// This is passed the current step number (0 to 7)
|
||||
// Subclasses can override
|
||||
void AccelStepper::step2(long step)
|
||||
{
|
||||
switch (step & 0x3)
|
||||
{
|
||||
case 0: /* 01 */
|
||||
setOutputPins(0b10);
|
||||
break;
|
||||
|
||||
case 1: /* 11 */
|
||||
setOutputPins(0b11);
|
||||
break;
|
||||
|
||||
case 2: /* 10 */
|
||||
setOutputPins(0b01);
|
||||
break;
|
||||
|
||||
case 3: /* 00 */
|
||||
setOutputPins(0b00);
|
||||
break;
|
||||
}
|
||||
}
|
||||
// 3 pin step function
|
||||
// This is passed the current step number (0 to 7)
|
||||
// Subclasses can override
|
||||
void AccelStepper::step3(long step)
|
||||
{
|
||||
switch (step % 3)
|
||||
{
|
||||
case 0: // 100
|
||||
setOutputPins(0b100);
|
||||
break;
|
||||
|
||||
case 1: // 001
|
||||
setOutputPins(0b001);
|
||||
break;
|
||||
|
||||
case 2: //010
|
||||
setOutputPins(0b010);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// 4 pin step function for half stepper
|
||||
// This is passed the current step number (0 to 7)
|
||||
// Subclasses can override
|
||||
void AccelStepper::step4(long step)
|
||||
{
|
||||
switch (step & 0x3)
|
||||
{
|
||||
case 0: // 1010
|
||||
setOutputPins(0b0101);
|
||||
break;
|
||||
|
||||
case 1: // 0110
|
||||
setOutputPins(0b0110);
|
||||
break;
|
||||
|
||||
case 2: //0101
|
||||
setOutputPins(0b1010);
|
||||
break;
|
||||
|
||||
case 3: //1001
|
||||
setOutputPins(0b1001);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 3 pin half step function
|
||||
// This is passed the current step number (0 to 7)
|
||||
// Subclasses can override
|
||||
void AccelStepper::step6(long step)
|
||||
{
|
||||
switch (step % 6)
|
||||
{
|
||||
case 0: // 100
|
||||
setOutputPins(0b100);
|
||||
break;
|
||||
|
||||
case 1: // 101
|
||||
setOutputPins(0b101);
|
||||
break;
|
||||
|
||||
case 2: // 001
|
||||
setOutputPins(0b001);
|
||||
break;
|
||||
|
||||
case 3: // 011
|
||||
setOutputPins(0b011);
|
||||
break;
|
||||
|
||||
case 4: // 010
|
||||
setOutputPins(0b010);
|
||||
break;
|
||||
|
||||
case 5: // 011
|
||||
setOutputPins(0b110);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// 4 pin half step function
|
||||
// This is passed the current step number (0 to 7)
|
||||
// Subclasses can override
|
||||
void AccelStepper::step8(long step)
|
||||
{
|
||||
switch (step & 0x7)
|
||||
{
|
||||
case 0: // 1000
|
||||
setOutputPins(0b0001);
|
||||
break;
|
||||
|
||||
case 1: // 1010
|
||||
setOutputPins(0b0101);
|
||||
break;
|
||||
|
||||
case 2: // 0010
|
||||
setOutputPins(0b0100);
|
||||
break;
|
||||
|
||||
case 3: // 0110
|
||||
setOutputPins(0b0110);
|
||||
break;
|
||||
|
||||
case 4: // 0100
|
||||
setOutputPins(0b0010);
|
||||
break;
|
||||
|
||||
case 5: //0101
|
||||
setOutputPins(0b1010);
|
||||
break;
|
||||
|
||||
case 6: // 0001
|
||||
setOutputPins(0b1000);
|
||||
break;
|
||||
|
||||
case 7: //1001
|
||||
setOutputPins(0b1001);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Prevents power consumption on the outputs
|
||||
void AccelStepper::disableOutputs()
|
||||
{
|
||||
if (! _interface) return;
|
||||
|
||||
setOutputPins(0); // Handles inversion automatically
|
||||
if (_enablePin != 0xff)
|
||||
digitalWrite(_enablePin, LOW ^ _enableInverted);
|
||||
}
|
||||
|
||||
void AccelStepper::enableOutputs()
|
||||
{
|
||||
if (! _interface)
|
||||
return;
|
||||
|
||||
pinMode(_pin[0], OUTPUT);
|
||||
pinMode(_pin[1], OUTPUT);
|
||||
if (_interface == FULL4WIRE || _interface == HALF4WIRE)
|
||||
{
|
||||
pinMode(_pin[2], OUTPUT);
|
||||
pinMode(_pin[3], OUTPUT);
|
||||
}
|
||||
|
||||
if (_enablePin != 0xff)
|
||||
{
|
||||
pinMode(_enablePin, OUTPUT);
|
||||
digitalWrite(_enablePin, HIGH ^ _enableInverted);
|
||||
}
|
||||
}
|
||||
|
||||
void AccelStepper::setMinPulseWidth(unsigned int minWidth)
|
||||
{
|
||||
_minPulseWidth = minWidth;
|
||||
}
|
||||
|
||||
void AccelStepper::setEnablePin(uint8_t enablePin)
|
||||
{
|
||||
_enablePin = enablePin;
|
||||
|
||||
// This happens after construction, so init pin now.
|
||||
if (_enablePin != 0xff)
|
||||
{
|
||||
pinMode(_enablePin, OUTPUT);
|
||||
digitalWrite(_enablePin, HIGH ^ _enableInverted);
|
||||
}
|
||||
}
|
||||
|
||||
void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert)
|
||||
{
|
||||
_pinInverted[0] = stepInvert;
|
||||
_pinInverted[1] = directionInvert;
|
||||
_enableInverted = enableInvert;
|
||||
}
|
||||
|
||||
void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
|
||||
{
|
||||
_pinInverted[0] = pin1Invert;
|
||||
_pinInverted[1] = pin2Invert;
|
||||
_pinInverted[2] = pin3Invert;
|
||||
_pinInverted[3] = pin4Invert;
|
||||
_enableInverted = enableInvert;
|
||||
}
|
||||
|
||||
// Blocks until the target position is reached and stopped
|
||||
void AccelStepper::runToPosition()
|
||||
{
|
||||
while (run())
|
||||
;
|
||||
}
|
||||
|
||||
boolean AccelStepper::runSpeedToPosition()
|
||||
{
|
||||
if (_targetPos == _currentPos)
|
||||
return false;
|
||||
if (_targetPos >_currentPos)
|
||||
_direction = DIRECTION_CW;
|
||||
else
|
||||
_direction = DIRECTION_CCW;
|
||||
return runSpeed();
|
||||
}
|
||||
|
||||
// Blocks until the new target position is reached
|
||||
void AccelStepper::runToNewPosition(long position)
|
||||
{
|
||||
moveTo(position);
|
||||
runToPosition();
|
||||
}
|
||||
|
||||
void AccelStepper::stop()
|
||||
{
|
||||
if (_speed != 0.0)
|
||||
{
|
||||
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding)
|
||||
if (_speed > 0)
|
||||
move(stepsToStop);
|
||||
else
|
||||
move(-stepsToStop);
|
||||
}
|
||||
}
|
|
@ -0,0 +1,620 @@
|
|||
// AccelStepper.h
|
||||
//
|
||||
/// \mainpage AccelStepper library for Arduino
|
||||
///
|
||||
/// This is the Arduino AccelStepper library.
|
||||
/// It provides an object-oriented interface for 2, 3 or 4 pin stepper motors.
|
||||
///
|
||||
/// The standard Arduino IDE includes the Stepper library
|
||||
/// (http://arduino.cc/en/Reference/Stepper) for stepper motors. It is
|
||||
/// perfectly adequate for simple, single motor applications.
|
||||
///
|
||||
/// AccelStepper significantly improves on the standard Arduino Stepper library in several ways:
|
||||
/// \li Supports acceleration and deceleration
|
||||
/// \li Supports multiple simultaneous steppers, with independent concurrent stepping on each stepper
|
||||
/// \li API functions never delay() or block
|
||||
/// \li Supports 2, 3 and 4 wire steppers, plus 3 and 4 wire half steppers.
|
||||
/// \li Supports alternate stepping functions to enable support of AFMotor (https://github.com/adafruit/Adafruit-Motor-Shield-library)
|
||||
/// \li Supports stepper drivers such as the Sparkfun EasyDriver (based on 3967 driver chip)
|
||||
/// \li Very slow speeds are supported
|
||||
/// \li Extensive API
|
||||
/// \li Subclass support
|
||||
///
|
||||
/// The latest version of this documentation can be downloaded from
|
||||
/// http://www.airspayce.com/mikem/arduino/AccelStepper
|
||||
/// The version of the package that this documentation refers to can be downloaded
|
||||
/// from http://www.airspayce.com/mikem/arduino/AccelStepper/AccelStepper-1.38.zip
|
||||
///
|
||||
/// Example Arduino programs are included to show the main modes of use.
|
||||
///
|
||||
/// You can also find online help and discussion at http://groups.google.com/group/accelstepper
|
||||
/// Please use that group for all questions and discussions on this topic.
|
||||
/// Do not contact the author directly, unless it is to discuss commercial licensing.
|
||||
///
|
||||
/// Tested on Arduino Diecimila and Mega with arduino-0018 & arduino-0021
|
||||
/// on OpenSuSE 11.1 and avr-libc-1.6.1-1.15,
|
||||
/// cross-avr-binutils-2.19-9.1, cross-avr-gcc-4.1.3_20080612-26.5.
|
||||
///
|
||||
/// \par Installation
|
||||
/// Install in the usual way: unzip the distribution zip file to the libraries
|
||||
/// sub-folder of your sketchbook.
|
||||
///
|
||||
/// \par Theory
|
||||
/// This code uses speed calculations as described in
|
||||
/// "Generate stepper-motor speed profiles in real time" by David Austin
|
||||
/// http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf
|
||||
/// with the exception that AccelStepper uses steps per second rather than radians per second
|
||||
/// (because we dont know the step angle of the motor)
|
||||
/// An initial step interval is calculated for the first step, based on the desired acceleration
|
||||
/// On subsequent steps, shorter step intervals are calculated based
|
||||
/// on the previous step until max speed is achieved.
|
||||
///
|
||||
/// This software is Copyright (C) 2010 Mike McCauley. Use is subject to license
|
||||
/// conditions. The main licensing options available are GPL V2 or Commercial:
|
||||
///
|
||||
/// \par Open Source Licensing GPL V2
|
||||
/// This is the appropriate option if you want to share the source code of your
|
||||
/// application with everyone you distribute it to, and you also want to give them
|
||||
/// the right to share who uses it. If you wish to use this software under Open
|
||||
/// Source Licensing, you must contribute all your source code to the open source
|
||||
/// community in accordance with the GPL Version 2 when your application is
|
||||
/// distributed. See http://www.gnu.org/copyleft/gpl.html
|
||||
///
|
||||
/// \par Commercial Licensing
|
||||
/// This is the appropriate option if you are creating proprietary applications
|
||||
/// and you are not prepared to distribute and share the source code of your
|
||||
/// application. Contact info@airspayce.com for details.
|
||||
///
|
||||
/// \par Revision History
|
||||
/// \version 1.0 Initial release
|
||||
///
|
||||
/// \version 1.1 Added speed() function to get the current speed.
|
||||
/// \version 1.2 Added runSpeedToPosition() submitted by Gunnar Arndt.
|
||||
/// \version 1.3 Added support for stepper drivers (ie with Step and Direction inputs) with _pins == 1
|
||||
/// \version 1.4 Added functional contructor to support AFMotor, contributed by Limor, with example sketches.
|
||||
/// \version 1.5 Improvements contributed by Peter Mousley: Use of microsecond steps and other speed improvements
|
||||
/// to increase max stepping speed to about 4kHz. New option for user to set the min allowed pulse width.
|
||||
/// Added checks for already running at max speed and skip further calcs if so.
|
||||
/// \version 1.6 Fixed a problem with wrapping of microsecond stepping that could cause stepping to hang.
|
||||
/// Reported by Sandy Noble.
|
||||
/// Removed redundant _lastRunTime member.
|
||||
/// \version 1.7 Fixed a bug where setCurrentPosition() did not always work as expected.
|
||||
/// Reported by Peter Linhart.
|
||||
/// \version 1.8 Added support for 4 pin half-steppers, requested by Harvey Moon
|
||||
/// \version 1.9 setCurrentPosition() now also sets motor speed to 0.
|
||||
/// \version 1.10 Builds on Arduino 1.0
|
||||
/// \version 1.11 Improvments from Michael Ellison:
|
||||
/// Added optional enable line support for stepper drivers
|
||||
/// Added inversion for step/direction/enable lines for stepper drivers
|
||||
/// \version 1.12 Announce Google Group
|
||||
/// \version 1.13 Improvements to speed calculation. Cost of calculation is now less in the worst case,
|
||||
/// and more or less constant in all cases. This should result in slightly beter high speed performance, and
|
||||
/// reduce anomalous speed glitches when other steppers are accelerating.
|
||||
/// However, its hard to see how to replace the sqrt() required at the very first step from 0 speed.
|
||||
/// \version 1.14 Fixed a problem with compiling under arduino 0021 reported by EmbeddedMan
|
||||
/// \version 1.15 Fixed a problem with runSpeedToPosition which did not correctly handle
|
||||
/// running backwards to a smaller target position. Added examples
|
||||
/// \version 1.16 Fixed some cases in the code where abs() was used instead of fabs().
|
||||
/// \version 1.17 Added example ProportionalControl
|
||||
/// \version 1.18 Fixed a problem: If one calls the funcion runSpeed() when Speed is zero, it makes steps
|
||||
/// without counting. reported by Friedrich, Klappenbach.
|
||||
/// \version 1.19 Added MotorInterfaceType and symbolic names for the number of pins to use
|
||||
/// for the motor interface. Updated examples to suit.
|
||||
/// Replaced individual pin assignment variables _pin1, _pin2 etc with array _pin[4].
|
||||
/// _pins member changed to _interface.
|
||||
/// Added _pinInverted array to simplify pin inversion operations.
|
||||
/// Added new function setOutputPins() which sets the motor output pins.
|
||||
|