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); // A and B both low
latch_state &= ~_BV(b);
MC.latch_tx();
break;
}
}
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::onestep(uint8_t dir) {
if (dir == FORWARD) {
currentstep++;
} else {
// BACKWARDS
currentstep--;
}
currentstep += MICROSTEPS*4;
currentstep %= MICROSTEPS*4;
#ifdef MOTORDEBUG
Serial.print("current step: "); Serial.println(currentstep, DEC);
#endif
// set all of this motor's pins to 0 (don't smash other motor)
latch_state &= ~a & ~b & ~c & ~d;
// No wait! Keep some energized.
switch (currentstep/(MICROSTEPS/2)) {
case 0: latch_state |= a; break; // energize coil 1 only
case 1: latch_state |= a | b; break; // energize coil 1+2
case 2: latch_state |= b; break; // energize coil 2 only
case 3: latch_state |= b | c; break; // energize coil 2+3
case 4: latch_state |= c; break; // energize coil 3 only
case 5: latch_state |= c | d; break; // energize coil 3+4
case 6: latch_state |= d; break; // energize coil 4 only
case 7: latch_state |= d | a; break; // energize coil 1+4
}
// change the energized state now
MC.latch_tx();
}
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;
}
if(a != MOTOR2_A) {
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;
}
}
else {
switch (cmd) {
case FORWARD:
//digitalWrite(12, 0x01);
PORTB |= _BV(4);
break;
case BACKWARD:
//digitalWrite(12, 0x00);
PORTB &= ~_BV(4);
break;
}
}
}
AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
MC.enable();
revsteps = steps;
steppernum = num;
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);
#ifdef MICROSTEPPING
// use PWM for microstepping support
initPWM1(MOTOR12_64KHZ);
initPWM2(MOTOR12_64KHZ);
setPWM1(255);
setPWM2(255);
#endif
} 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);
#ifdef MICROSTEPPING
// use PWM for microstepping support
// use PWM for microstepping support
initPWM3(1);
initPWM4(1);
setPWM3(255);
setPWM4(255);
#endif
}
}
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(STEPPER1_PWM_RATE);
initPWM2(STEPPER1_PWM_RATE);
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(STEPPER2_PWM_RATE);
initPWM4(STEPPER2_PWM_RATE);
setPWM3(255);
setPWM4(255);
}
}
/*-- MOTOR SETTINGS --*/
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);
a = _BV(MOTOR1_A);
b = _BV(MOTOR2_A);
c = _BV(MOTOR1_B);
d = _BV(MOTOR2_B);
} 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);
a = _BV(MOTOR3_A);
b = _BV(MOTOR4_A);
c = _BV(MOTOR3_B);
d = _BV(MOTOR4_B);
}
}
/*-- MOTOR ONE-STEP CONFIG --*/
uint8_t AF_Stepper::onestep(uint8_t dir)
{
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;
}
currentstep += MICROSTEPS*4;
currentstep %= MICROSTEPS*4;
#ifdef MOTORDEBUG
Serial.print("current step: "); Serial.println(currentstep, DEC);
#endif
// preprare to release all coils
latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0
// No wait! Keep some energized.
switch (currentstep/(MICROSTEPS/2))
{
case 0: latch_state |= a; break; // energize coil 1 only
case 1: latch_state |= a | b; break; // energize coil 1+2
case 2: latch_state |= b; break; // energize coil 2 only
case 3: latch_state |= b | c; break; // energize coil 2+3
case 4: latch_state |= c; break; // energize coil 3 only
case 5: latch_state |= c | d; break; // energize coil 3+4
case 6: latch_state |= d; break; // energize coil 4 only
case 7: latch_state |= d | a; break; // energize coil 1+4
}
// change the energized state now
MC.latch_tx();
return currentstep;
}
void AF_Stepper::release() {
// release all
latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0
MC.latch_tx();
}
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;
}
uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) {
uint8_t a, b, c, d;
uint8_t step;
uint8_t mstep = 0;
#ifdef MICROSTEPPING
uint8_t ocrb, ocra;
#endif
if (steppernum == 1) {
a = _BV(MOTOR1_A);
b = _BV(MOTOR2_A);
c = _BV(MOTOR1_B);
d = _BV(MOTOR2_B);
#ifdef MICROSTEPPING
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
ocra = OCR2A;
ocrb = OCR2B;
#elif defined(__AVR_ATmega1280__)
ocra = OCR1A;
ocrb = OCR3C;
#endif
if (style == MICROSTEP) {
//TCCR2B = _BV(CS21);
}
#endif
} else if (steppernum == 2) {
a = _BV(MOTOR3_A);
b = _BV(MOTOR4_A);
c = _BV(MOTOR3_B);
d = _BV(MOTOR4_B);
#ifdef MICROSTEPPING
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
ocra = OCR0A;
ocrb = OCR0B;
#elif defined(__AVR_ATmega1280__)
ocra = OCR4A;
ocrb = OCR3A;
#endif
if (style == MICROSTEP) {
//TCCR0B = _BV(CS00);
}
#endif
} else {
return 0;
}
#ifdef MOTORDEBUG
Serial.print("a = "); Serial.print(ocra, DEC);
Serial.print(" b = "); Serial.print(ocrb, DEC);
Serial.print("\t");
#endif
// OK next determine what step we are at
if ((latch_state & (a | b)) == (a | b))
step = 1 * MICROSTEPS;
else if ((latch_state & (b | c)) == (b | c))
step = 3 * MICROSTEPS;
else if ((latch_state & (c | d)) == (c | d))
step = 5 * MICROSTEPS;
else if ((latch_state & (d | a)) == (d | a))
step = 7 * MICROSTEPS;
else if (latch_state & a)
step = 0;
else if (latch_state & b)
step = 2 * MICROSTEPS;
else if (latch_state & c)
step = 4 * MICROSTEPS;
else
step = 6 * MICROSTEPS;
//Serial.print("step "); Serial.print(step, DEC); Serial.print("\t");
// next determine what sort of stepping procedure we're up to
if (style == SINGLE) {
if ((step/MICROSTEPS) % 2) { // we're at an odd step, weird
if (dir == FORWARD)
step = (step + MICROSTEPS) % (8*MICROSTEPS);
else
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
} else { // go to the next even step
if (dir == FORWARD)
step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
else
step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
}
#ifdef MICROSTEPPING
ocra = 255;
ocrb = 255;
#endif
} else if (style == DOUBLE) {
if (! (step/MICROSTEPS % 2)) { // we're at an even step, weird
if (dir == FORWARD)
step = (step + MICROSTEPS) % (8*MICROSTEPS);
else
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
} else { // go to the next odd step
if (dir == FORWARD)
step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
else
step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
}
#ifdef MICROSTEPPING
ocra = 255;
ocrb = 255;
#endif
} else if (style == INTERLEAVE) {
if (dir == FORWARD)
step = (step + 1*MICROSTEPS) % (8*MICROSTEPS);
else
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
#ifdef MICROSTEPPING
ocra = 255;
ocrb = 255;
#endif
}
#ifdef MICROSTEPPING
else if (style == MICROSTEP) {
//Serial.print("Step #"); Serial.print(step/MICROSTEPS, DEC); Serial.print("\t");
if (dir == FORWARD) {
// if at even step, make sure its 'odd'
if (! (step/MICROSTEPS) % 2) {
step = (step + MICROSTEPS) % (8*MICROSTEPS);
}
// fix hiccups from changing direction
if (((ocra == 255) && ((step/MICROSTEPS)%4 == 3)) ||
((ocrb == 255) && ((step/MICROSTEPS)%4 == 1))) {
step += 2*MICROSTEPS;
step %= 8*MICROSTEPS;
}
if ((step == MICROSTEPS) || (step == 5*MICROSTEPS)) {
// get the current microstep
if (ocrb == 255)
mstep = MICROSTEPS;
else
mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print("uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep++;
mstep %= (MICROSTEPS+1);
if (mstep == MICROSTEPS)
ocrb = 255;
else
ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" -> "); Serial.println(mstep, DEC);
#endif
if (mstep == MICROSTEPS)
step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
} else {
// get the current microstep
if (ocrb == 255)
mstep = MICROSTEPS;
else
mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print("uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep += MICROSTEPS;
mstep %= (MICROSTEPS+1);
if (mstep == MICROSTEPS)
ocrb = 255;
else
ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" +> "); Serial.println(mstep, DEC);
#endif
if (mstep == 0)
step = (step + 2*MICROSTEPS) % (8*MICROSTEPS);
}
} else {
// fix hiccups from changing direction
if (((ocra == 255) && ((step/MICROSTEPS)%4 == 1)) ||
((ocrb == 255) && ((step/MICROSTEPS)%4 == 3))) {
step = (step + 6*MICROSTEPS);
step %= (8*MICROSTEPS);
}
// if at even step, make sure its 'odd'
if (! (step/MICROSTEPS % 2)) {
step = (step + 7*MICROSTEPS) % (8*MICROSTEPS);
}
if ((step == MICROSTEPS) || (step == 5*MICROSTEPS)) {
// get the current microstep
if (ocrb == 255)
mstep = MICROSTEPS;
else
mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print(" uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep += MICROSTEPS;
mstep %= (MICROSTEPS+1);
if (mstep == MICROSTEPS)
ocrb = 255;
else
ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" !> "); Serial.println(mstep, DEC);
#endif
if (mstep == 0)
step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
} else {
// get the current microstep
if (ocrb == 255)
mstep = MICROSTEPS;
else
mstep = ocrb / (256UL / MICROSTEPS);
#ifdef MOTORDEBUG
Serial.print("uStep = "); Serial.print(mstep, DEC);
#endif
// ok now go to next step
mstep++;
mstep %= (MICROSTEPS + 1);
if (mstep == MICROSTEPS)
ocrb = 255;
else
ocrb = mstep * (256UL / MICROSTEPS);
ocra = 255 - ocrb;
#ifdef MOTORDEBUG
Serial.print(" *> "); Serial.println(mstep, DEC);
#endif
if (mstep == MICROSTEPS)
step = (step + 6*MICROSTEPS) % (8*MICROSTEPS);
}
}
}
//Serial.print(" -> step = "); Serial.print(step/MICROSTEPS, DEC); Serial.print("\t");
if (steppernum == 1) {
setPWM1(ocra);
setPWM2(ocrb);
} else if (steppernum == 2) {
setPWM3(ocra);
setPWM4(ocrb);
}
#endif // microstepping
// release all
latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0
//Serial.println(step, DEC);
switch (step/MICROSTEPS) {
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 mstep;
}
