We now begin coding the firmware for the stepper controller. Our conceptual leap was to realize that the code for driving the three stepper motors is the same as that driving a three holomonic robotic base. Basically the intended motion is resolved into three direction 120 degrees apart. The resolved motion is then translated into the motion of the three stepper motors. There are codes for handling mixing of linear and rotational motion. Lastly, there are some codes for implementing the balancing and head-locking, but these are commented out for initial tests.

//***************************************************************************************************************
/*

 Ballbot Firmware V1
 
 Copyright (c) 2014 Space Trek Systems.
 http://www.spacetreksystems.com/
 
 */

//***************************************************************************************************************

// header files

#include "EasyTransferI2C.h"
#include <PID_v1.h>
//#include <TimerOne.h>
#include <TimerThree.h>
#include <TimerFour.h>
#include <TimerFive.h>
#include <Wire.h>

//***************************************************************************************************************
//communication with reciever module
//create object
EasyTransferI2C ET; 

struct RECEIVE_DATA_STRUCTURE{
  //put your variable definitions here for the data you want to send
  //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
  double Xspeedtrans;
  double Yspeedtrans;
  double Rotationtrans;
};

//give a name to the group of data
RECEIVE_DATA_STRUCTURE senddata;

//define slave i2c address
#define I2C_SLAVE_ADDRESS 9

//***************************************************************************************************************

//imu definition

/*

 MinIMU-9-Arduino-AHRS
 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
 
 Copyright (c) 2011 Pololu Corporation.
 http://www.pololu.com/
 
 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
 http://code.google.com/p/sf9domahrs/
 
 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
 Julio and Doug Weibel:
 http://code.google.com/p/ardu-imu/
 
 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
 under the terms of the GNU Lesser General Public License as published by the
 Free Software Foundation, either version 3 of the License, or (at your option)
 any later version.
 
 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
 more details.
 
 You should have received a copy of the GNU Lesser General Public License along
 with MinIMU-9-Arduino-AHRS. If not, see .
 
 */

// Uncomment the below line to use this axis definition: 
// X axis pointing forward
// Y axis pointing to the right 
// and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise
int SENSOR_SIGN[9] = {
  1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
// Uncomment the below line to use this axis definition: 
// X axis pointing forward
// Y axis pointing to the left 
// and Z axis pointing up.
// Positive pitch : nose down
// Positive roll : right wing down
// Positive yaw : counterclockwise
//int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer

// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168

// LSM303 accelerometer: 8 g sensitivity
// 3.9 mg/digit; 1 g = 256
#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer 
 
 #define ToRad(x) ((x)*0.01745329252)  // *pi/180
 #define ToDeg(x) ((x)*57.2957795131)  // *180/pi
 
 // L3G4200D gyro: 2000 dps full scale
 // 70 mdps/digit; 1 dps = 0.07
 #define Gyro_Gain_X 0.007 //X axis Gyro gain
 #define Gyro_Gain_Y 0.007 //Y axis Gyro gain
 #define Gyro_Gain_Z 0.007 //Z axis Gyro gain
 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
 
 // LSM303 magnetometer calibration constants; use the Calibrate example from
 // the Pololu LSM303 library to find the right values for your board
 #define M_X_MIN -421
 #define M_Y_MIN -639
 #define M_Z_MIN -238
 #define M_X_MAX 424
 #define M_Y_MAX 295
 #define M_Z_MAX 472
 
 #define Kp_ROLLPITCH 0.02
 #define Ki_ROLLPITCH 0.00002
 #define Kp_YAW 1.2
 #define Ki_YAW 0.00002
 
/*For debugging purposes*/
//OUTPUTMODE=1 will print the corrected data, 
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1

//#define PRINT_DCM 0     //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw

#define STATUS_LED 13 

float G_Dt = 0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible

long timer = 0;   //general purpuse timer
long timer_old;
long timer24 = 0; //Second timer used to print values 
int AN[6]; //array that stores the gyro and accelerometer data
int AN_OFFSET[6]={
  0,0,0,0,0,0}; //Array that stores the Offset of the sensors

int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_magnetom_z;
float MAG_Heading;

float Accel_Vector[3]= {
  0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {
  0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector[3]= {
  0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {
  0,0,0};//Omega Proportional correction
float Omega_I[3]= {
  0,0,0};//Omega Integrator
float Omega[3]= {
  0,0,0};

// Euler angles
float roll;
float pitch;
float yaw;

float errorRollPitch[3]= {
  0,0,0}; 
float errorYaw[3]= {
  0,0,0};

unsigned int counter=0;
byte gyro_sat=0;

float DCM_Matrix[3][3]= {
  {
    1,0,0                                                  }
  ,{
    0,1,0                                                  }
  ,{
    0,0,1                                                  }
}; 
float Update_Matrix[3][3]={
  {
    0,1,2                                                }
  ,{
    3,4,5                                                }
  ,{
    6,7,8                                                }
}; //Gyros here


float Temporary_Matrix[3][3]={
  {
    0,0,0                                                  }
  ,{
    0,0,0                                                  }
  ,{
    0,0,0                                                  }
};

//***************************************************************************************************************
// PID loop

double Xspeed, Yspeed, Rotation;
//double pitchSet, rollSet, yawSet;
//double pitchAngle, rollAngle, yawAngle;

//initialise PID loops
//PID pitchPID(&pitchAngle, &Xspeed, &pitchSet, 700, 0, 0, DIRECT);
//PID rollPID(&rollAngle, &Yspeed, &rollSet, 700, 0, 0, DIRECT);
//PID yawPID(&yawAngle, &Rotation, &yawSet, 100, 0, 0, DIRECT); 

//***************************************************************************************************************
//stepper driver

//defining 
const double STEP = 0.0982;//100*pi/200/16 (mm, for wheel)
//const double STEP = 0.00014394;//(100*pi/2*108.55*pi)/200/16 (mm, for ball roller)

//defining pins assignment (control pins for quadstepper)
//channel mapping 1->X, 2->Y, and 3->Z
const int XPUL = 11, XDIR = 36, XENABLE = A1;
const int YPUL = 5,  YDIR = 9,  YENABLE = 10;
const int ZPUL = 6,  ZDIR = 23, ZENABLE= 22;

//defining pins assignment (mircrostep selection for quadstepper)
const int XMS1 = A8, XMS2 = A9, XMS3 = A10;
const int YMS1 = 8,  YMS2 = 7,  YMS3 = 4;
const int ZMS1 = 24, ZMS2 = 25, ZMS3 = 26;

//static double mag, angle, rotation;
//static double angle_ref = 0;

//const double RADIUS = 83.154;
//const double DEG2RAD = 1000/57296;

double xspeed, yspeed, zspeed;
long x_t, y_t, z_t;
long prev_time = micros();

//const int NUMBER_OF_FIELDS = 3; // how many comma separated fields we expect
//int fieldIndex = 0;            // the current field being received
//double values[NUMBER_OF_FIELDS];   // array holding values for all the fields
//boolean negative = false;

//***************************************************************************************************************

//reciever input
//const int XRECPIN = 27, YRECPIN = 28, YAWRECPIN = 29;
//const double MAXSPEED = 200, MINSPEED = -200; //(mms^-1)
//const double MAXROTSPEED = 200, MINROTSPEED = -200; //(degs^-1)
//const long DEADZONEUPPERLIMIT = 1520, DEADZONELOWERLIMIT = 1480; 
//const long TIMEOUT = 2500;
//const int MAXVALUE = 2000, MINVALUE = 1000;

//***************************************************************************************************************

// initialisation subroutines

void init_stepper()
{

  pinMode(XPUL,OUTPUT);
  pinMode(XDIR,OUTPUT);
  pinMode(XENABLE, OUTPUT);
  pinMode(XMS1, OUTPUT);
  pinMode(XMS2, OUTPUT);
  pinMode(XMS3, OUTPUT);

  pinMode(YPUL,OUTPUT);
  pinMode(YDIR,OUTPUT);
  pinMode(YENABLE, OUTPUT);
  pinMode(YMS1, OUTPUT);
  pinMode(YMS2, OUTPUT);
  pinMode(YMS3, OUTPUT);

  pinMode(ZPUL,OUTPUT);
  pinMode(ZDIR,OUTPUT);
  pinMode(ZENABLE, OUTPUT);
  pinMode(ZMS1, OUTPUT);
  pinMode(ZMS2, OUTPUT);
  pinMode(ZMS3, OUTPUT);

  digitalWrite(XMS1, HIGH);
  digitalWrite(XMS2, HIGH);
  digitalWrite(XMS3, HIGH);

  digitalWrite(YMS1, HIGH);
  digitalWrite(YMS2, HIGH);
  digitalWrite(YMS3, HIGH);

  digitalWrite(ZMS1, HIGH);
  digitalWrite(ZMS2, HIGH);
  digitalWrite(ZMS3, HIGH);

  digitalWrite(XENABLE, LOW);
  digitalWrite(YENABLE, LOW);
  digitalWrite(ZENABLE, LOW);

  //Timer1.initialize(1000000);
  Timer3.initialize(1000000);
  Timer4.initialize(1000000);
  Timer5.initialize(1000000);
}

/*
void init_imu()
 {
 pinMode (STATUS_LED, OUTPUT);  // Status LED
 
 I2C_Init();
 
 digitalWrite(STATUS_LED,LOW);
 delay(1500);
 
 Accel_Init();
 Compass_Init();
 Gyro_Init();
 
 delay(20);
 
 for(int i = 0; i < 32; i++)    // We take some readings...
 {
 Read_Gyro();
 Read_Accel();
 for(int y=0; y<6; y++)   // Cumulate values
 AN_OFFSET[y] += AN[y];
 delay(20);
 }
 
 for(int y = 0; y < 6; y++)
 AN_OFFSET[y] = AN_OFFSET[y]/32;
 
 AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
 
 //Serial.println("Offset:");
 for(int y = 0; y < 6; y++)
 Serial.println(AN_OFFSET[y]);
 
 delay(2000);
 digitalWrite(STATUS_LED,HIGH);
 
 timer=millis();
 delay(20);
 counter=0;
 
 }
 
 void init_pid()
 {
 //intialising PID set values
 pitchSet = 0;
 rollSet = 0;
 yawSet = 0;
 
 Xspeed = 0;
 Yspeed = 0;
 Rotation = 0;
 
 //turn the PID on
 pitchPID.SetMode(AUTOMATIC);
 rollPID.SetMode(AUTOMATIC);
 yawPID.SetMode(MANUAL);
 
 pitchPID.SetOutputLimits(-100, 100);
 rollPID.SetOutputLimits(-100, 100);
 yawPID.SetOutputLimits(-180, 180);
 
 pitchPID.SetSampleTime(200);
 rollPID.SetSampleTime(200);
 yawPID.SetSampleTime(200);
 
 }
 
 void init_control()
 {
 pinMode(XRECPIN, INPUT);
 pinMode(YRECPIN, INPUT);
 pinMode(YAWRECPIN, INPUT);
 }*/

void init_control()
{
  Wire.begin(I2C_SLAVE_ADDRESS);
  //start the library, pass in the data details and the name of the serial port. Can be Serial, Serial1, Serial2, etc. 
  ET.begin(details(senddata), &Wire);
  //define handler function on receiving data
  Wire.onReceive(receive);
}

//***************************************************************************************************************
// actuation subroutines
/*
void imu_act()
 {
 if((millis() - timer)>=20)  // Main loop runs at 50Hz
 {
 Serial.println("imu act");
 counter++;
 timer_old = timer;
 timer = millis();
 if (timer > timer_old)
 G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
 else
 G_Dt = 0;
 
 
 // *** DCM algorithm
 // Data adquisition
 Read_Gyro();   // This read gyro data
 Read_Accel();     // Read I2C accelerometer
 
 if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
 {
 counter=0;
 Read_Compass();    // Read I2C magnetometer
 Compass_Heading(); // Calculate magnetic heading  
 }
 
 // Calculations...
 Matrix_update(); 
 Normalize();
 Drift_correction();
 Euler_angles();
 //printdata();
 }
 }
 
 void pid_act()
 {
 
 Serial.println("pid act");
 pitchAngle = double(pitch);
 rollAngle = double(roll);
 yawAngle = double(yaw);
 
 pitchPID.Compute();
 rollPID.Compute();
 yawPID.Compute();
 }
 */

/*void control()
 {
 
 unsigned long xrecpindur = pulseIn(XRECPIN, HIGH);
 //unsigned long yrecpindur = pulseIn(YRECPIN, HIGH);
 //unsigned long yawrecpindur = pulseIn(YAWRECPIN, HIGH);
 
 unsigned long yrecpindur = 1500;
 unsigned long yawrecpindur = 1500;
 
 if(xrecpindur != 0)
 {
 if(xrecpindur < DEADZONELOWERLIMIT || xrecpindur > DEADZONEUPPERLIMIT)
 { 
 Xspeed = map(xrecpindur, MINVALUE, MAXVALUE, MINSPEED, MAXSPEED);
 }
 else
 {
 Xspeed = 0;
 }
 }
 
 if(yrecpindur != 0)
 {
 if(yrecpindur < DEADZONELOWERLIMIT || yrecpindur > DEADZONEUPPERLIMIT)
 { 
 Yspeed = map(yrecpindur, MINVALUE, MAXVALUE, MINSPEED, MAXSPEED);
 }
 else
 {
 Yspeed = 0;
 }
 }
 
 if(yawrecpindur != 0)
 {
 if(yawrecpindur < DEADZONELOWERLIMIT || yawrecpindur > DEADZONEUPPERLIMIT)
 { 
 Rotation = map(yawrecpindur, MINVALUE, MAXVALUE, MINROTSPEED, MAXROTSPEED);
 }
 else
 {
 Rotation = 0;
 }
 }
 
 }*/

void control()
{
  if(ET.receiveData())
  {
    //this is how you access the variables. [name of the group].[variable name]
    Xspeed = senddata.Xspeedtrans;
    Yspeed = senddata.Yspeedtrans;
    Rotation = senddata.Rotationtrans;
  }
  
}


//***************************************************************************************************************
// stepper driver subroutines

void stepper_act()
{
  //Serial.println("stepper act");
  speed(Xspeed, Yspeed, Rotation);

  if(xspeed != 0)
    x_t = int(STEP/abs(xspeed)*1000000); //time (us) for 1 step at xspeed

  if(yspeed != 0)
    y_t = int(STEP/abs(yspeed)*1000000); //time (us) for 1 step at yspeed

  if(zspeed != 0)
    z_t = int(STEP/abs(zspeed)*1000000); //time (us) for 1 step at zspeed

  if(xspeed > 0)
    digitalWrite(XDIR, HIGH);
  else
    digitalWrite(XDIR, LOW);

  if(yspeed > 0)
    digitalWrite(YDIR, HIGH);
  else
    digitalWrite(YDIR, LOW);

  if(zspeed >0)
    digitalWrite(ZDIR, HIGH);
  else
    digitalWrite(ZDIR, LOW);

  xMove(xspeed);
  yMove(yspeed);
  zMove(zspeed);

}

void speed(double XSpeed, double YSpeed, double Rotation)
{
  long time_step = micros() - prev_time;
  prev_time = micros();

  xspeed = XSpeed + 83.154*ToRad(Rotation);
  yspeed = YSpeed*0.866 - XSpeed*0.5 + 83.154*ToRad(Rotation);
  zspeed = -YSpeed*0.866 - XSpeed*0.5 + 83.154*ToRad(Rotation);
}

void xMove(double xs)
{
  if(xs != 0)
  { 
    Timer5.setPeriod(x_t/2);
    Timer5.attachInterrupt(xPulse);
  } 
  else
  {
    Timer5.detachInterrupt();
  }
}


void yMove(double ys)
{
  if(ys != 0)
  { 
    Timer3.setPeriod(y_t/2);
    Timer3.attachInterrupt(yPulse);
  } 
  else
  {
    Timer3.detachInterrupt();
  }
}


void zMove(double zs)
{
  if(zs != 0)
  { 
    Timer4.setPeriod(z_t/2);
    Timer4.attachInterrupt(zPulse);
  } 
  else
  {
    Timer4.detachInterrupt();
  }
}


void xPulse()
{
  digitalWrite(XPUL, digitalRead(XPUL) ^ 1);
}


void yPulse()
{
  digitalWrite(YPUL, digitalRead(YPUL) ^ 1);
}


void zPulse()
{
  digitalWrite(ZPUL, digitalRead(ZPUL) ^ 1);
}

//***************************************************************************************************************

//setup
void setup()
{   
  Serial.begin(115200);
  //Serial1.begin(115200);

  Serial.println("ballbot firmware v1");

  init_control();
  Serial.println("control initialized.");

  //init_imu();
  //Serial.println("imu initialized.");

  //init_pid();
  //Serial.println("pid loops initialized.");

  init_stepper();
  Serial.println("stepper driver initialized");

}


//main Loop
void loop() 
{

  control();
  //imu_act();
  //pid_act();
  stepper_act();

  /*Serial.print(pitch);
   Serial.print(",");
   Serial.print(roll);
   Serial.print(",");
   Serial.print(yaw);
   Serial.print(",");*/

  Serial.print(Xspeed);
  Serial.print(",");
  Serial.print(Yspeed);
  Serial.print(",");
  Serial.print(Rotation);
  Serial.print(",");
  Serial.print(xspeed);
  Serial.print(",");
  Serial.print(yspeed);
  Serial.print(",");
  Serial.print(zspeed);
  Serial.println(";");


}

void receive(int numBytes) {}

//***************************************************************************************************************