自学教程：C++ AFMotorController类代码示例

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);//.........这里部分代码省略.........

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;//.........这里部分代码省略.........