/*

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}};
#define L_MOTOR 0// index of left motor
#define R_MOTOR 1// index of right motor

int changed = 0;

void setup() {
// join i2c bus with address 0x04
// register receive event handler
 Wire.begin(4);
Wire.onReceive(receiveEvent);
// 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 resp
//
// 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], 0);
// change as required for motor brakes
digitalWrite(motor_pins[motorIdx][1], LOW);
digitalWrite(motor_pins[motorIdx][2], LOW);
return;
}
// check speed ranges
if(motorSpd < -255)
motorSpd = -255;
if(motorSpd > 255)
motorSpd = 255;

// replace with lookup table here to allow for non-linear motor re

// 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 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
// mode 1 - set both motors to same speed. 2 bytes (1 int) for spee
// mode 2 - set motors to individual speeds. 4 bytes (2 int) for r,
void receiveEvent(int byteCnt) {
byte mode =Wire.receive();
// convert bytes to ints easily using defined union
BYTE2INT received;
// int storage buffer
int intReceived[4] = {0, 0, 0, 0};
switch(mode)
{
// set both motors to same speed
// 2 bytes representing integer speed follow
case(1):
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(2):
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;
// just clear receive buffer
default:
for(int i = 0; i < byteCnt - 1; i++)
{
Wire.receive();
}
break;
}
}