414 lines
10 KiB
C++
414 lines
10 KiB
C++
/******************************************************************
|
|
This is the library for the Adafruit Motor Shield V2 for Arduino.
|
|
It supports DC motors & Stepper motors with microstepping as well
|
|
as stacking-support. It is *not* compatible with the V1 library!
|
|
|
|
It will only work with https://www.adafruit.com/products/1483
|
|
|
|
Adafruit invests time and resources providing this open
|
|
source code, please support Adafruit and open-source hardware
|
|
by purchasing products from Adafruit!
|
|
|
|
Written by Limor Fried/Ladyada for Adafruit Industries.
|
|
BSD license, check license.txt for more information.
|
|
All text above must be included in any redistribution.
|
|
******************************************************************/
|
|
|
|
|
|
#if (ARDUINO >= 100)
|
|
#include "Arduino.h"
|
|
#else
|
|
#include "WProgram.h"
|
|
#endif
|
|
#include <Wire.h>
|
|
#include "Adafruit_MotorShield.h"
|
|
#include <Adafruit_PWMServoDriver.h>
|
|
#ifdef __AVR__
|
|
#define WIRE Wire
|
|
#else // Arduino Due
|
|
#define WIRE Wire1
|
|
#endif
|
|
|
|
|
|
#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
|
|
|
|
Adafruit_MotorShield::Adafruit_MotorShield(uint8_t addr) {
|
|
_addr = addr;
|
|
_pwm = Adafruit_PWMServoDriver(_addr);
|
|
}
|
|
|
|
void Adafruit_MotorShield::begin(uint16_t freq) {
|
|
// init PWM w/_freq
|
|
WIRE.begin();
|
|
_pwm.begin();
|
|
_freq = freq;
|
|
_pwm.setPWMFreq(_freq); // This is the maximum PWM frequency
|
|
for (uint8_t i=0; i<16; i++)
|
|
_pwm.setPWM(i, 0, 0);
|
|
}
|
|
|
|
void Adafruit_MotorShield::setPWM(uint8_t pin, uint16_t value) {
|
|
if (value > 4095) {
|
|
_pwm.setPWM(pin, 4096, 0);
|
|
} else
|
|
_pwm.setPWM(pin, 0, value);
|
|
}
|
|
void Adafruit_MotorShield::setPin(uint8_t pin, boolean value) {
|
|
if (value == LOW)
|
|
_pwm.setPWM(pin, 0, 0);
|
|
else
|
|
_pwm.setPWM(pin, 4096, 0);
|
|
}
|
|
|
|
Adafruit_DCMotor *Adafruit_MotorShield::getMotor(uint8_t num) {
|
|
if (num > 4) return NULL;
|
|
|
|
num--;
|
|
|
|
if (dcmotors[num].motornum == 0) {
|
|
// not init'd yet!
|
|
dcmotors[num].motornum = num;
|
|
dcmotors[num].MC = this;
|
|
uint8_t pwm, in1, in2;
|
|
if (num == 0) {
|
|
pwm = 8; in2 = 9; in1 = 10;
|
|
} else if (num == 1) {
|
|
pwm = 13; in2 = 12; in1 = 11;
|
|
} else if (num == 2) {
|
|
pwm = 2; in2 = 3; in1 = 4;
|
|
} else if (num == 3) {
|
|
pwm = 7; in2 = 6; in1 = 5;
|
|
}
|
|
dcmotors[num].PWMpin = pwm;
|
|
dcmotors[num].IN1pin = in1;
|
|
dcmotors[num].IN2pin = in2;
|
|
}
|
|
return &dcmotors[num];
|
|
}
|
|
|
|
|
|
Adafruit_StepperMotor *Adafruit_MotorShield::getStepper(uint16_t steps, uint8_t num) {
|
|
if (num > 2) return NULL;
|
|
|
|
num--;
|
|
|
|
if (steppers[num].steppernum == 0) {
|
|
// not init'd yet!
|
|
steppers[num].steppernum = num;
|
|
steppers[num].revsteps = steps;
|
|
steppers[num].MC = this;
|
|
uint8_t pwma, pwmb, ain1, ain2, bin1, bin2;
|
|
if (num == 0) {
|
|
pwma = 8; ain2 = 9; ain1 = 10;
|
|
pwmb = 13; bin2 = 12; bin1 = 11;
|
|
} else if (num == 1) {
|
|
pwma = 2; ain2 = 3; ain1 = 4;
|
|
pwmb = 7; bin2 = 6; bin1 = 5;
|
|
}
|
|
steppers[num].PWMApin = pwma;
|
|
steppers[num].PWMBpin = pwmb;
|
|
steppers[num].AIN1pin = ain1;
|
|
steppers[num].AIN2pin = ain2;
|
|
steppers[num].BIN1pin = bin1;
|
|
steppers[num].BIN2pin = bin2;
|
|
}
|
|
return &steppers[num];
|
|
}
|
|
|
|
|
|
/******************************************
|
|
MOTORS
|
|
******************************************/
|
|
|
|
Adafruit_DCMotor::Adafruit_DCMotor(void) {
|
|
MC = NULL;
|
|
motornum = 0;
|
|
PWMpin = IN1pin = IN2pin = 0;
|
|
}
|
|
|
|
void Adafruit_DCMotor::run(uint8_t cmd) {
|
|
switch (cmd) {
|
|
case FORWARD:
|
|
MC->setPin(IN2pin, LOW); // take low first to avoid 'break'
|
|
MC->setPin(IN1pin, HIGH);
|
|
break;
|
|
case BACKWARD:
|
|
MC->setPin(IN1pin, LOW); // take low first to avoid 'break'
|
|
MC->setPin(IN2pin, HIGH);
|
|
break;
|
|
case RELEASE:
|
|
MC->setPin(IN1pin, LOW);
|
|
MC->setPin(IN2pin, LOW);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Adafruit_DCMotor::setSpeed(uint8_t speed) {
|
|
MC->setPWM(PWMpin, speed*16);
|
|
}
|
|
|
|
/******************************************
|
|
STEPPERS
|
|
******************************************/
|
|
|
|
Adafruit_StepperMotor::Adafruit_StepperMotor(void) {
|
|
revsteps = steppernum = currentstep = 0;
|
|
}
|
|
/*
|
|
|
|
uint16_t steps, Adafruit_MotorShield controller) {
|
|
|
|
revsteps = steps;
|
|
steppernum = 1;
|
|
currentstep = 0;
|
|
|
|
if (steppernum == 1) {
|
|
latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
|
|
~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
|
|
|
|
// enable both H bridges
|
|
pinMode(11, OUTPUT);
|
|
pinMode(3, OUTPUT);
|
|
digitalWrite(11, HIGH);
|
|
digitalWrite(3, HIGH);
|
|
|
|
// use PWM for microstepping support
|
|
MC->setPWM(1, 255);
|
|
MC->setPWM(2, 255);
|
|
|
|
} else if (steppernum == 2) {
|
|
latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
|
|
~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
|
|
|
|
// 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
|
|
MC->setPWM(3, 255);
|
|
MC->setPWM(4, 255);
|
|
}
|
|
}
|
|
*/
|
|
|
|
void Adafruit_StepperMotor::setSpeed(uint16_t rpm) {
|
|
//Serial.println("steps per rev: "); Serial.println(revsteps);
|
|
//Serial.println("RPM: "); Serial.println(rpm);
|
|
|
|
usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
|
|
steppingcounter = 0;
|
|
}
|
|
|
|
void Adafruit_StepperMotor::release(void) {
|
|
MC->setPin(AIN1pin, LOW);
|
|
MC->setPin(AIN2pin, LOW);
|
|
MC->setPin(BIN1pin, LOW);
|
|
MC->setPin(BIN2pin, LOW);
|
|
MC->setPWM(PWMApin, 0);
|
|
MC->setPWM(PWMBpin, 0);
|
|
}
|
|
|
|
void Adafruit_StepperMotor::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--) {
|
|
//Serial.println("step!"); Serial.println(uspers);
|
|
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 Adafruit_StepperMotor::onestep(uint8_t dir, uint8_t style) {
|
|
uint8_t a, b, c, d;
|
|
uint8_t ocrb, ocra;
|
|
|
|
ocra = ocrb = 255;
|
|
|
|
|
|
// 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
|
|
MC->setPWM(PWMApin, ocra*16);
|
|
MC->setPWM(PWMBpin, ocrb*16);
|
|
|
|
|
|
// release all
|
|
uint8_t latch_state = 0; // all motor pins to 0
|
|
|
|
//Serial.println(step, DEC);
|
|
if (style == MICROSTEP) {
|
|
if ((currentstep >= 0) && (currentstep < MICROSTEPS))
|
|
latch_state |= 0x03;
|
|
if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
|
|
latch_state |= 0x06;
|
|
if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
|
|
latch_state |= 0x0C;
|
|
if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
|
|
latch_state |= 0x09;
|
|
} else {
|
|
switch (currentstep/(MICROSTEPS/2)) {
|
|
case 0:
|
|
latch_state |= 0x1; // energize coil 1 only
|
|
break;
|
|
case 1:
|
|
latch_state |= 0x3; // energize coil 1+2
|
|
break;
|
|
case 2:
|
|
latch_state |= 0x2; // energize coil 2 only
|
|
break;
|
|
case 3:
|
|
latch_state |= 0x6; // energize coil 2+3
|
|
break;
|
|
case 4:
|
|
latch_state |= 0x4; // energize coil 3 only
|
|
break;
|
|
case 5:
|
|
latch_state |= 0xC; // energize coil 3+4
|
|
break;
|
|
case 6:
|
|
latch_state |= 0x8; // energize coil 4 only
|
|
break;
|
|
case 7:
|
|
latch_state |= 0x9; // energize coil 1+4
|
|
break;
|
|
}
|
|
}
|
|
#ifdef MOTORDEBUG
|
|
Serial.print("Latch: 0x"); Serial.println(latch_state, HEX);
|
|
#endif
|
|
|
|
if (latch_state & 0x1) {
|
|
// Serial.println(AIN2pin);
|
|
MC->setPin(AIN2pin, HIGH);
|
|
} else {
|
|
MC->setPin(AIN2pin, LOW);
|
|
}
|
|
if (latch_state & 0x2) {
|
|
MC->setPin(BIN1pin, HIGH);
|
|
// Serial.println(BIN1pin);
|
|
} else {
|
|
MC->setPin(BIN1pin, LOW);
|
|
}
|
|
if (latch_state & 0x4) {
|
|
MC->setPin(AIN1pin, HIGH);
|
|
// Serial.println(AIN1pin);
|
|
} else {
|
|
MC->setPin(AIN1pin, LOW);
|
|
}
|
|
if (latch_state & 0x8) {
|
|
MC->setPin(BIN2pin, HIGH);
|
|
// Serial.println(BIN2pin);
|
|
} else {
|
|
MC->setPin(BIN2pin, LOW);
|
|
}
|
|
|
|
return currentstep;
|
|
}
|
|
|