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;
}
}
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);
}
}
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;
}
