/*CoasterBot Motor ControllerBlack Dog RoboticsI2C slave, motor controlreceive messages from master, set motor speed */#include <Wire.h>// easily convert 2 bytes to/from an integerunion BYTE2INT {byte asBytes[2];int asInt;} byteToInt;// easily convert 2 bytes to/from an integerunion BYTE2FLOAT {byte asBytes[2];float asFloat;} byteToFloat;// set pin numbers:#define LED_PIN 13 // the number of the LED pin#define MOTOR_CNT 2 // number of motors#define MOTOR_PINS 3 // Arduino pins/motor// array of motor control pins// [n][0] - enable for motor n// [n][1] - odd pin (1A for n==0, 3A for n==1) for H-Bridge// [n][2] - even pin (2A for n==0, 4A for n==1) for H-Bridge// enable pins must be PWM enabled for speed controlint motor_pins[2][3] = {{10, 11, 12}, {9, 8, 7}};int motor_speed[2] = {0, 0};#define L_MOTOR 0 // index of left motor#define R_MOTOR 1 // index of right motorint changed = 0;// I2C Modesbyte I2C_mode = 0;#define SAME_SPEED 1 // get both speed integer from master#define UNIQUE_SPEED 2 // get r/l speed values from master#define SPEED_REQUESTED 3 // master will request speed, return 4 bytes when requested//void setup() {// join i2c bus with address 0x04// register receive event handlerWire.begin(4);// receive message handlerWire.onReceive(receiveEvent);// request info handlerWire.onRequest(requestEvent);// initialize the LED pin as an output:pinMode(LED_PIN, OUTPUT);// initialize motor pins as output:for(int i = 0; i < MOTOR_CNT; i++){for(int j = 0; j < MOTOR_PINS; j++){pinMode(motor_pins[i][j], OUTPUT);}}

 

// flash readydigitalWrite(LED_PIN, HIGH);delay(1000);digitalWrite(LED_PIN, LOW);}//// just flash if change from I2C master// work is done in registered recieve event//void loop(){// indicate change from masterif(changed == 1){// flash readydigitalWrite(LED_PIN, HIGH);delay(50);digitalWrite(LED_PIN, LOW);changed = 0;}}//// set motor speed to requested value// motorSpd in range -255 to 255// assume linear for now, add lookup tables for non-linear pwn response later?//// pwm range 0 to 255, direction controlled by direction pins//void setMotorSpeed(int motorIdx, int motorSpd){// stopped?if(motorSpd == 0){//analogWrite(motor_pins[motorIdx][0], 255);digitalWrite(motor_pins[motorIdx][0], HIGH);// change as required for motor brakesdigitalWrite(motor_pins[motorIdx][1], HIGH);digitalWrite(motor_pins[motorIdx][2], HIGH);motor_speed[motorIdx] = 0;return;}// check speed rangesif(motorSpd < -255)motorSpd = -255;if(motorSpd > 255)motorSpd = 255;motor_speed[motorIdx] = motorSpd;// replace with lookup table here to allow for non-linear motor response?// for now, assume linear and equal side to sideint pwmValue = abs(motorSpd);// disable firstanalogWrite(motor_pins[motorIdx][0], 0);if (motorSpd < 0){digitalWrite(motor_pins[motorIdx][1], LOW);digitalWrite(motor_pins[motorIdx][2], HIGH);}else{

 

digitalWrite(motor_pins[motorIdx][1], HIGH);digitalWrite(motor_pins[motorIdx][2], LOW);}// set analog output to start motor - doneanalogWrite(motor_pins[motorIdx][0], pwmValue);}//// set both motors to same speed in 1 call for convinience//void setBothMotors(int motorSpd){for(int i = 0; i < MOTOR_CNT; i++){setMotorSpeed(i, motorSpd);}}//// I2C slave receive event handler// registered in setup()//// byteCnt is the number of bytes received from master// first byte is mode, determines how many to follow and what they mean// mode 1 - set both motors to same speed. 2 bytes (1 int) for speed follows// mode 2 - set motors to individual speeds. 4 bytes (2 int) for r, l speeds followsvoid receiveEvent(int byteCnt) {I2C_mode = Wire.receive();// convert bytes to ints easily using defined unionBYTE2INT received;// int storage bufferint intReceived[4] = {0, 0, 0, 0};switch(I2C_mode){// set both motors to same speed// 2 bytes representing integer speed followcase(SAME_SPEED):received.asBytes[0] = Wire.receive();received.asBytes[1] = Wire.receive();setBothMotors(received.asInt);changed = 1;break;//// set motors to different speeds// 4 bytes representing 2 integer speeds (right, left) followcase(UNIQUE_SPEED):received.asBytes[0] = Wire.receive();received.asBytes[1] = Wire.receive();intReceived[0] = received.asInt;received.asBytes[0] = Wire.receive();received.asBytes[1] = Wire.receive();intReceived[1] = received.asInt;setBothMotors(received.asInt);setMotorSpeed(R_MOTOR, intReceived[0]);setMotorSpeed(L_MOTOR, intReceived[1]);changed = 1;break;// next master request is for motor speedscase(SPEED_REQUESTED):changed = 1;break;