// HexMotor.h // copyright Kevin Karplus 8 August 2011 // Creative Commons license Attribution-NonCommercial // http://creativecommons.org/licenses/by-nc/3.0/ #ifndef _HexMotor_h_ #define _HexMotor_h_ #include #include // Define HEX_MOTOR_DEBUG if you want to get error messages from setup() for erroneous setup. // #define HEX_MOTOR_DEBUG 1 // Declaring a HexMotorBoard gives the jumperable configuration for the board. // The relevant jumpers are which pins drive the motor speed inputs // and whether the H-bridges are configured for lock antiphase or sign-magnitude. // // IN2 of the H-bridge is always connected to SIGN XOR MAG // // IN1 of the H-bridge can be connected to either SIGN or MAG. // If IN1 is connected to SIGN, then the TLE-5206 H-bridge will // be running in a sign magnitude mode, with the Speed pin low meaning BRAKE // and Speed pin high meaning RUN (with the sign bit indicating which way to turn). // If IN1 is connected to MAG, then the TLE-5206 H-bridge will // be in lock antiphase, running if the SIGN bit is high and BRAKING if the SIGN bit is low. // The MAG bit determines which way the motor runs. // If the MAG bit is not a PWM output, then IN1 should be connected to MAG. // Note: on the rev 1.3 boards, the silkscreen for the jumpers is misleading. // The center of the 5 holes for header pins is MAG and the outer one is SIGN. // The PWM frequency for all channels defaults to 1kHz (actually 16Mz/ 2^14 = 976.56Hz) // Changes could be made in the HexMotorBoard::setup() routine if other PWM frequencies are needed. class HexMotorBoard { public: static const uint8_t defaultPins[6]; static const uint8_t defaultPinsWithServo[6]; protected: uint8_t SpeedPins[6]; // which pins are connected to the "speed" (MAG) inputs of each H-bridge? // Numbers 0-54 are for Arduino pins // Numbers 0xA0..0xA7 are for the low byte of the serial output latch // Numbers 0xA8..0xAF are for the high byte of the serial output latch // (on rev2 or later) // Number 0xFF means that the MAG bit is tied to +5V // // Note: all SpeedPins should be connected to something // Note: on Arduinos other than Mega, using the Servo library means that pins 9 and 10 // are not PWM pins. If used, they should be set up as ON/OFF pins. // On Rev 2.3 of the HexMotor Board, // this means changing the jumpers by pins 9 and 10 to B4 and B5, and changing the jumpers for // Motor4 and Motor5 from A to S. // The speed pins should then be 11, 3, 6, 5, 0xB4, 0xB5 // (and the antis string "SSSSFF") // Pin mapping for the serial latches on rev 2.3 of the HexMotor board: // Bits out of the serial-to-parallel latch: // QA A0 is last bit received // name pin # net meaning // QA 15 A0 MOTOR 0 direction/run // QB 1 A1 MOTOR 1 direction/run // QC 2 A2 MOTOR 2 direction/run // QD 3 A3 MOTOR 3 direction/run // QE 4 A4 MOTOR 4 direction/run // QF 5 A5 MOTOR 5 direction/run // QG 6 A6 spare // QH 7 A7 spare // QA 15 B0 MOTOR 0 alternate ON/OFF (instead of Ard 11) // QB 1 B1 MOTOR 1 alternate ON/OFF (instead of Ard 3) // QC 2 B2 MOTOR 2 alternate ON/OFF (instead of Ard 6) // QD 3 B3 MOTOR 3 alternate ON/OFF (instead of Ard 5) // QE 4 B4 MOTOR 4 alternate ON/OFF (instead of Ard 9) // QF 5 B5 MOTOR 5 alternate ON/OFF (instead of Ard 10) // QG 6 B6 spare // QH 7 B7 spare enum{NOT_THERE, SIGN_MAG, ANTIPHASE, FORWARD_BACKWARD_BRAKE, ONE_BIT, ADAFRUIT} MotorMode[6]; // MotorMode[i] is // NOT_THERE if motor_i is not usable on this board // SIGN_MAG if IN1 of motor i is connected to SIGN, and MAG is assumed to be PWM // ANTIPHASE if IN1 of motor i connected to MAG and MAG is a PWM bit // FORWARD_BACKWARD_BRAKE if IN1 of motor i connected to MAG, but MAG is ON/OFF, not PWM. // ONE_BIT if IN1 is connected to MAG, which is tied to +5v, so the // the motor is always either running forward or backward, controlled by the SIGN bit // ADAFRUIT if this is not a HexMotor board, but an Adafruit Motor shield // uint8_t LatchPin, ClockPin, DataPin; // which Arduino pins are used for talking to the Hexmotor shift register? uint16_t ShiftRegister; // the current or future contents of the HexMotor shift register uint8_t Version; // which model of board is used // set (or clear) a bit in the ShiftRegister corresponding to the specified motor inline void set_signbit(uint8_t motor, bool value=1) { digitalWrite(0xA0+motor, value); } void serial_out(void); // transfer the shift register to the HexMotor board. public: HexMotorBoard( const char *antis, const uint8_t *pins=defaultPins, // defaults to {11, 3, 6, 5, 9,10} Arduino with 6 PWM motors // (a NULL pointer will do the same default). // // pins = HexMotorBoard::defaultPinsWithServo sets to alternative {11,3,6,5,0xB4,0xB5} // Arduino with 4 PWM motors and servos // otherwise pointer to array of 6 pin numbers. uint8_t version=2, uint8_t clock=4, uint8_t data=8, uint8_t latch=12); // An array of pins is given to indicate where the MAG output for each motor goes. // The 6 antis characters are // '-' for NOT_THERE // 'S' or 's' for SIGN_MAG (IN1==SIGN) // 'A' or 'a' for ANTIPHASE (IN1==MAG) // 'F' or 'f' for FORWARD_BACKWARD_BRAKE (IN1==MAG, but MAG is not PWM bit) // 'O' or 'o' for ONE_BIT // 'M' or 'm' for ADAFRUIT motor shield // The version is the integer part of the board rev number (rev1.3 is 1, rev 2.3 is 2). // This indicates, for example, whether the board has 8 or 16 bits of shift register. // Use rev 0 to indicate an Adafruit motorshield. // latch, data, and clock are Arduino pins that are used for the serial output to the shift register. void setup(void); // makes sure that PWM frequencies are appropriately set and turns all motors off. // Should be called during the global setup() procedure. // QUERY: should PWM frequency be settable here (or even as separate call?) void digitalWrite(uint8_t pin, bool value); // write a single bit to a pin (using SpeedPins naming convention) friend class HexMotor; }; // Declaring an AdafruitMotorBoard sets up the HexMotorBoard interface for an AdaFruit Industries Motor Shield, // rather than for a HexMotor board. // The declaration has no parameters, as the AdaFruit motor shield is not configurable. // For compatibility with the M1 through M4 labeling on the motor shield, motors 1 through 4 are used, // rather than 0 through 3. class AdafruitMotorBoard : public HexMotorBoard { protected: typedef enum{FORWARD, BACKWARD, BRAKE} MotorDir; void change_motor_bits(uint8_t motor, MotorDir control); public: AdafruitMotorBoard(void); friend class HexMotor; }; class HexMotor { protected: uint8_t Motor; HexMotorBoard* Board; public: HexMotor(HexMotorBoard &board, uint8_t motor_num); void run(int speed); // speed is between -256 (full reverse) and 255 (full forward) // 0 is off (braking on HexMotor, released on Adafruit) void brake(void); void release(void); // Available on Adafruit Motor shield, // but not on HexMotor boards rev1 or rev 2 // since the TLE-5206 chips do not have a Hi-Z output }; #endif