// HexMotor library // Kevin Karplus //copyright 8 August 2011 // http://creativecommons.org/licenses/by-nc/3.0/ #include #include #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" #else #include "WProgram.h" #endif #include "HexMotor.h" // The PWM frequency for all channels defaults to 1kHz. // Changes could be made in the HexMotorBoard::setup() routine if other PWM frequencies are needed. // The frequencies below are approximate. // The actual frequency in fast PWM mode is f_clk/(256*prescale) // For the 16.000MHz crystal of the Arduino, the frequencies are // 62.5KHz, 7.125KHz, 1.9531 kHz, 976.56Hz, 488.28Hz, 244.14 Hz, and 61.035Hz // Don't mess with timer 0, since it is used for "delay()" and "millis()" #define OCR0_64KHz (1) // no prescale #define OCR0_8KHz (2) // divide by 8 #define OCR0_1KHz (3) // divide by 16 // Timer 1 (and Timers 3, 4, 5 on Arduino Mega) have limited // prescale choices, because they allow external clock as well. #define OCR1_62KHz (1) // no prescale #define OCR1_7KHz (2) // divide by 8 #define OCR1_1KHz (3) // divide by 64 #define OCR1_240Hz (4) // divide by 256 #define OCR1_61Hz (5) // divide by 1024 #define OCR2_62KHz (1) // no prescale #define OCR2_7KHz (2) // divide by 8 #define OCR2_2KHz (3) // divide by 32 #define OCR2_1KHz (4) // divide by 64 #define OCR2_490Hz (5) // divide by 128 #define OCR2_240Hz (6) // divide by 256 #define OCR2_61Hz (7) // divide by 1024 // The Adafruit Motor Shield has an extra output bit for its serial interface: #define AdafruitEnablePin (7) ///////////////// // HexMotorBoard ///////////////// const uint8_t HexMotorBoard::defaultPins[6]= {11, 3, 6, 5, 9, 10}; const uint8_t HexMotorBoard::defaultPinsWithServo[6] = {11, 3, 6, 5, 0xB4, 0xB5}; HexMotorBoard::HexMotorBoard( const char *antis, const uint8_t *pins, uint8_t version, uint8_t clock, uint8_t data, uint8_t latch) { if (pins==0) {pins= defaultPins;} // Save all the jumper information. for(int8_t i=5; i>=0; i--) { SpeedPins[i] = pins[i]; switch (antis[i]) { case 'A': case 'a': MotorMode[i] = ANTIPHASE; // should check that pins[i] is legal PWM pin break; case 'S': case 's': MotorMode[i] = SIGN_MAG; // should check that pins[i] is legal PWM pin break; case 'F': case 'f': MotorMode[i] = FORWARD_BACKWARD_BRAKE; // should check that pins[i] is consistent with board version break; case 'O': case 'o': MotorMode[i] = ONE_BIT; // should check that pins[i] is 0xFF (indicating MAG tied to +5v) break; case 'M': case 'm': MotorMode[i] = ADAFRUIT; // should check that 1<=i<=4 and pins[i]=AdafruitDefaultPins[i] and version==0 break; default: MotorMode[i] = NOT_THERE; break; } } Version=version; ClockPin=clock; DataPin=data; LatchPin=latch; } void HexMotorBoard::setup(void) { // setup the serial output pins and clear the shift register pinMode(LatchPin, OUTPUT); pinMode(DataPin, OUTPUT); pinMode(ClockPin, OUTPUT); if (Version==0) { pinMode(AdafruitEnablePin, OUTPUT); } ShiftRegister=0; serial_out(); for (int8_t m=5;m>=0;m--) { switch (MotorMode[m]) { case NOT_THERE: case ONE_BIT: #ifdef HEX_MOTOR_DEBUG if (SpeedPins[m]!=0xFF) { Serial.print(m,DEC); Serial.println(" motor shouldn't have speed pin"); } #endif continue; case FORWARD_BACKWARD_BRAKE: if (SpeedPins[m] >= 0xA0) { #ifdef HEX_MOTOR_DEBUG if (Version<2 && SpeedPins[m]>0xA7) { Serial.print(m,DEC); Serial.println(" motor has SpeedPin>0xA7"); } #endif continue; // no need to set pinMode for HexMotorPins } pinMode(SpeedPins[m], OUTPUT); continue; // no PWM to check case ADAFRUIT: #ifdef HEX_MOTOR_DEBUG if (Version!=0) { Serial.print(m,DEC); Serial.print(" mode M, but board version"); Serial.println(Version,DEC); } #endif break; default: break; } if (SpeedPins[m] >= 0xA0) { // A HexMotor shift register output or tied to +5v, can't do PWM. #ifdef HEX_MOTOR_DEBUG Serial.print(m,DEC); Serial.print(" motor has one-bit speed in mode "); Serial.println(MotorMode[m]); #endif continue; } //real Arduino pin (not HexMotor shift register) pinMode(SpeedPins[m], OUTPUT); // Set up PWM frequency switch (digitalPinToTimer(SpeedPins[m])) { case NOT_ON_TIMER: #ifdef HEX_MOTOR_DEBUG Serial.print(m,DEC); Serial.print(" pin "); Serial.print(SpeedPins[m]); Serial.println(" not a PWM pin"); #endif break; // timer 0 is used for delay() and millis(), // so don't mess with its frequency case TIMER0A: TCCR0A = (TCCR0A & 0x30) | _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A OCR0A = 0; break; case TIMER0B: TCCR0A = (TCCR0A & 0xC0) | _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0B OCR0B = 0; break; // Timer 2 is used for pins 11 and 3 of Arduino (not Arduino Mega) // default pins for motors 0 and 1, (M1 and M2 on Adafruit Motor shield) case TIMER2A: TCCR2A = (TCCR2A & 0x30) | _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a TCCR2B = OCR2_1KHz & 0x7; OCR2A = 0; break; case TIMER2B: TCCR2A = (TCCR2A & 0xC0) | _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b TCCR2B = OCR2_1KHz & 0x7; OCR2B = 0; break; // Timer 1 case TIMER1A: TCCR1A = (TCCR1A & 0x3C) | _BV(COM1A1) | _BV(WGM10); // fast PWM 8-bit, turn on oc1a TCCR1B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR1C = 0; OCR1A = 0; break; case TIMER1B: TCCR1A = (TCCR1A & 0xCC) | _BV(COM1B1) | _BV(WGM10); // fast PWM 8-bit, turn on oc1b TCCR1B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR1C = 0; OCR1B = 0; break; #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // Timer 3 case TIMER3A: TCCR3A = (TCCR3A & 0x3C) | _BV(COM3A1) | _BV(WGM10); // fast PWM 8-bit, turn on oc3a TCCR3B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR3C = 0; OCR3A = 0; break; case TIMER3B: TCCR3A = (TCCR3A & 0xCC) | _BV(COM3B1) | _BV(WGM10); // fast PWM 8-bit, turn on oc3b TCCR3B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR3C = 0; OCR3B = 0; break; case TIMER3C: TCCR3A = (TCCR3A & 0xF0) | _BV(COM3C1) | _BV(WGM10); // fast PWM 8-bit, turn on oc3c TCCR3B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR3C = 0; OCR3C = 0; break; // Timer 4 case TIMER4A: TCCR4A = (TCCR4A & 0x3C) | _BV(COM4A1) | _BV(WGM10); // fast PWM 8-bit, turn on oc4a TCCR4B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR4C = 0; OCR4A = 0; break; case TIMER4B: TCCR4A = (TCCR4A & 0xCC) | _BV(COM4B1) | _BV(WGM10); // fast PWM 8-bit, turn on oc4b TCCR4B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR4C = 0; OCR4B = 0; break; case TIMER4C: TCCR4A = (TCCR4A & 0xF0) | _BV(COM4C1) | _BV(WGM10); // fast PWM 8-bit, turn on oc4c TCCR4B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR4C = 0; OCR4C = 0; break; // Timer 5 case TIMER5A: TCCR5A = (TCCR5A & 0x3C) | _BV(COM5A1) | _BV(WGM10); // fast PWM 8-bit, turn on oc5a TCCR5B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR5C = 0; OCR5A = 0; break; case TIMER5B: TCCR5A = (TCCR5A & 0xCC) | _BV(COM5B1) | _BV(WGM10); // fast PWM 8-bit, turn on oc5b TCCR5B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR5C = 0; OCR5B = 0; break; case TIMER5C: TCCR5A = (TCCR5A & 0xF0) | _BV(COM5C1) | _BV(WGM10); // fast PWM 8-bit, turn on oc5c TCCR5B = (OCR1_1KHz & 0x7) | _BV(WGM12); TCCR5C = 0; OCR5C = 0; break; #endif default: break; } } } void HexMotorBoard::serial_out(void) { #ifdef HEX_MOTOR_DEBUG Serial.print("shift 0x"); Serial.print(ShiftRegister, HEX); #endif digitalWrite(LatchPin,LOW); // Output high order bits first if (Version>=2) { shiftOut(DataPin,ClockPin, MSBFIRST, highByte(ShiftRegister)); #ifdef HEX_MOTOR_DEBUG Serial.print(" hi 0x"); Serial.print(highByte(ShiftRegister),HEX); #endif } shiftOut(DataPin,ClockPin, MSBFIRST, lowByte(ShiftRegister)); #ifdef HEX_MOTOR_DEBUG Serial.print(" lo 0x"); Serial.println(lowByte(ShiftRegister),HEX); #endif // rising edge on latch pin transfers shift register to output register digitalWrite(LatchPin, HIGH); if (Version==0) { digitalWrite(AdafruitEnablePin, LOW); // enable output } } void HexMotorBoard::digitalWrite(uint8_t pin, bool value) { if (pin<0xA0) { ::digitalWrite(pin, value); return; } if (pin==0xFF) return; // pin is +5v and can't be changed // pin can be 0xA0..0xAF OR 0xA0..0xA7,0xB0..0xB7 uint16_t pos = pin<0xB0? (1 << (pin-0xA0)): (1<< (pin-0xA8)); bool old_bit = (ShiftRegister & pos) !=0; if ((value && ! old_bit) || (!value && old_bit)) { ShiftRegister ^= pos; serial_out(); } } ////////////////////// // AdafruitMotorBoard ////////////////////// const uint8_t AdafruitDefaultPins[6]={0xFF, 11, 3, 6, 5, 0xFF}; AdafruitMotorBoard::AdafruitMotorBoard(void): HexMotorBoard("-MMMM-",AdafruitDefaultPins,0) { } // Map the motor number to the two control pins for the motors, based on the // mapping from the AFMotor.h file: // MOTOR1_A 2 // MOTOR1_B 3 // MOTOR2_A 1 // MOTOR2_B 4 // MOTOR3_A 5 // MOTOR3_B 7 // MOTOR4_A 0 // MOTOR4_B 6 const uint8_t AdafruitMotorAPin[5]={0, 1<<2, 1<<1, 1<<5, 1<<0}; const uint8_t AdafruitMotorBPin[5]={0, 1<<3, 1<<4, 1<<7, 1<<6}; void AdafruitMotorBoard::change_motor_bits(uint8_t motor, MotorDir control) { if (MotorMode[motor] != ADAFRUIT) return; // error switch(control) { case FORWARD: ShiftRegister |= AdafruitMotorAPin[motor]; ShiftRegister &= ~AdafruitMotorBPin[motor]; break; case BACKWARD: ShiftRegister &= ~AdafruitMotorAPin[motor]; ShiftRegister |= AdafruitMotorBPin[motor]; break; case BRAKE: // set both outputs low ShiftRegister &= ~AdafruitMotorAPin[motor]; ShiftRegister &= ~AdafruitMotorBPin[motor]; break; } serial_out(); } ///////////////////// // HexMotor ///////////////////// HexMotor::HexMotor(HexMotorBoard &board, uint8_t motor_num) { Board=&board; Motor=motor_num; } // speed is between -255 (full reverse) and 255 (full forward) // 0 is off (braking) void HexMotor::run(int speed) { // clip to legal range if (speed>255) speed=255; else if (speed<-255) speed= 0-255; #ifdef HEX_MOTOR_DEBUG Serial.print(Motor, DEC); Serial.print(" motor running at "); Serial.println(speed); #endif switch (Board->MotorMode[Motor]) { case HexMotorBoard::NOT_THERE: #ifdef HEX_MOTOR_DEBUG Serial.print(Motor, DEC); Serial.println("motor NOT_THERE. Can't run"); #endif return; case HexMotorBoard::SIGN_MAG: // (Sign-magnitude) IN1 is connected to SIGN, and MAG is assumed to be PWM Board->set_signbit(Motor, speed<0); analogWrite(Board->SpeedPins[Motor], speed>=0? speed: 0-speed); return; case HexMotorBoard::ANTIPHASE: // (MotorMode) IN1 is connected to MAG, and MAG is PWM if (speed==0) { // Brake Board->set_signbit(Motor,0); analogWrite(Board->SpeedPins[Motor], 0); return; } Board->set_signbit(Motor,1); analogWrite(Board->SpeedPins[Motor], (256-speed)>>1); return; case HexMotorBoard::FORWARD_BACKWARD_BRAKE: // (Forward/Backward/Brake) IN1 is connected to MAG, and MAG is ON/OFF only if (speed==0) { // Brake Board->set_signbit(Motor,0); Board->digitalWrite(Board->SpeedPins[Motor], 0); return; } Board->digitalWrite(Board->SpeedPins[Motor], speed<0); Board->set_signbit(Motor,1); return; case HexMotorBoard::ONE_BIT: // IN1 connected to MAG=+5V Board->digitalWrite(Board->SpeedPins[Motor], speed<0); return; case HexMotorBoard::ADAFRUIT: // shift_register pins are interpreted differently by Adafruit board if (speed>=0) { static_cast(Board)->change_motor_bits(Motor,AdafruitMotorBoard::FORWARD); analogWrite(Board->SpeedPins[Motor],speed); } else { static_cast(Board)->change_motor_bits(Motor,AdafruitMotorBoard::BACKWARD); analogWrite(Board->SpeedPins[Motor],0-speed); } return; } } void HexMotor::brake(void) { switch (Board->MotorMode[Motor]) { case HexMotorBoard::ADAFRUIT: static_cast(Board)->change_motor_bits(Motor,AdafruitMotorBoard::BRAKE); analogWrite(Board->SpeedPins[Motor],255); return; case HexMotorBoard::NOT_THERE: case HexMotorBoard::ONE_BIT: #ifdef HEX_MOTOR_DEBUG Serial.print(Motor,DEC); Serial.println("motor NOT_THERE or ONE_BIT. Can't brake."); #endif return; // no such operation exists default: run(0); return; } } void HexMotor::release(void) { switch (Board->MotorMode[Motor]) { case HexMotorBoard::ADAFRUIT: static_cast(Board)->change_motor_bits(Motor,AdafruitMotorBoard::BRAKE); analogWrite(Board->SpeedPins[Motor],0); return; default: #ifdef HEX_MOTOR_DEBUG Serial.println("Not Adafruit motor shield, can't release."); #endif run(0); // not a release, but a brake on HexMotor boards. // The closest the TLE-5206 chips can get. return; } }