/*

CoasterBot Motor Controller

Black Dog Robotics
I2C slave, motor control
receive messages from master, set motor speed
*/
#include <Wire.h>
// easily convert 2 bytes to/from an integer
union BYTE2INT {
byte asBytes[2];
int asInt;
} byteToInt;
// easily convert 2 bytes to/from an integer
union 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 control
int 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 motor
int changed = 0;
// I2C Modes
byte 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 requ
ested
//
void setup() {
// join i2c bus with address 0x04
// register receive event handler
Wire.begin(4);
// receive message handler
Wire.onReceive(receiveEvent);
// request info handler
Wire.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 ready
digitalWrite(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 master
if(changed == 1)
{
// flash ready
digitalWrite(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 brakes
digitalWrite(motor_pins[motorIdx][1], HIGH);
digitalWrite(motor_pins[motorIdx][2], HIGH);
motor_speed[motorIdx] = 0;
return;
}
// check speed ranges
if(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 side
int pwmValue = abs(motorSpd);
// disable first
analogWrite(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 - done
analogWrite(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 fol
lows
void receiveEvent(int byteCnt) {
I2C_mode = Wire.receive();
// convert bytes to ints easily using defined union
BYTE2INT received;
// int storage buffer
int intReceived[4] = {0, 0, 0, 0};
switch(I2C_mode)
{
// set both motors to same speed
// 2 bytes representing integer speed follow
case(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) follow
case(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 speeds
case(SPEED_REQUESTED):
changed = 1;
break;
 // unknown message, just clear receive buffer
default:
for(int i = 0; i < byteCnt - 1; i++)
{
Wire.receive();
}
break;
}
}
//
// I2C slave request event handler
// registered in setup()
//
void requestEvent() {
byte msg4[4];
BYTE2INT toSend;
// info sent out depends on request mode set by
// last message received
switch(I2C_mode)
{
case SPEED_REQUESTED:
toSend.asInt = motor_speed[R_MOTOR];
msg4[0] = toSend.asBytes[0];
msg4[1] = toSend.asBytes[1];
toSend.asInt = motor_speed[L_MOTOR];
msg4[2] = toSend.asBytes[0];
msg4[3] = toSend.asBytes[1];
// begin and end transmission not required
// for slave response.
Wire.send(msg4, 4);
break;
default:
toSend.asInt = 32767;
msg4[0] = toSend.asBytes[0];
msg4[1] = toSend.asBytes[1];
msg4[2] = toSend.asBytes[0];
msg4[3] = toSend.asBytes[1];
// begin and end transmission not required
// for slave response.
Wire.send(msg4, 4);
break;
}
}