The control system has been switched to a 3 core MCU running at 200 MHz, the 'ShieldBuddy'. This enabled the whole steering and drive system to be hosted on one device. The first core runs 330 lines of code to control the stepper motors with full differential on both the steering and drive, using the 'micros' function. The next core has serial debugging and a 4 second delay so that it can be read easily. The third core oversamples some potentiometers to control the machine on a 5 core cable.

/*** Don't worry, the normal Arduino setup() and loop() are below this block! ***/

/* LMU uninitialised data */
StartOfUninitialised_LMURam_Variables
/* Put your LMU RAM fast access variables that have no initial values here e.g. uint32 LMU_var; */
EndOfUninitialised_LMURam_Variables

/* LMU uninitialised data */
StartOfInitialised_LMURam_Variables
/* Put your LMU RAM fast access variables that have an initial value here e.g. uint32 LMU_var_init = 1; */
const int ledPin =  13;
int ledStateOne = LOW;   
int ledStateTwo = LOW;   
int ledStateThree = LOW;
int ledStateFour = LOW;

unsigned long previousMicrosOne = 0;
unsigned long previousMicrosTwo = 0;
unsigned long previousMicrosThree = 0;
unsigned long previousMicrosFour = 0;

long intervalOne = 1000;
long intervalTwo = 1000;
long intervalThree = 1000;
long intervalFour = 1000;

int difference=0;
int previousFinalSteeringValue=15300;

long rightWheel=0;
long leftWheel=0;
long wheelsPosition=0;

int finalDriveValue=0;
long finalSteeringValue =0;

EndOfInitialised_LMURam_Variables


/*** Core 0 ***/
void setup()
{  
  delay(5000);
  pinMode(ledPin, OUTPUT);
  pinMode(5,OUTPUT); //STEP
  pinMode(6,OUTPUT); //DIRECTION HIGH is clockwise
  pinMode(7,OUTPUT); //STEP
  pinMode(8,OUTPUT); //DIRECTION HIGH is clockwise
  pinMode(9,OUTPUT); //STEP
  pinMode(10,OUTPUT); //DIRECTION HIGH is clockwise
  pinMode(11,OUTPUT); //STEP
  pinMode(12,OUTPUT); //DIRECTION HIGH is clockwise
}

void loop()
{  
  unsigned long currentMicrosOne = micros();
  unsigned long currentMicrosTwo = micros(); 
  unsigned long currentMicrosThree = micros();
  unsigned long currentMicrosFour = micros();

speedDifferential();

if (finalDriveValue>=600) //Forwards.
{
  intervalThree = (600000/finalDriveValue)-300; // 250 is max speed.
  intervalFour =  (600000/finalDriveValue)-300; // 250 is max speed.
  speedDifferential();
  digitalWrite(10,HIGH);
  digitalWrite(12,HIGH);
  if (currentMicrosThree - previousMicrosThree >= intervalThree)
  {
    changeStateThree();
    digitalWrite(9,ledStateThree); // Drive motor step
  }
  if (currentMicrosFour - previousMicrosFour >= intervalFour)
  {
    changeStateFour();
    digitalWrite(11,ledStateFour); // Drive motor step
  }
}
if (finalDriveValue<400) //Backwards.
{
  intervalThree = (finalDriveValue*2)+250; // 250 is max speed.
  intervalFour = (finalDriveValue*2)+250; // 250 is max speed.
  speedDifferential();
  digitalWrite(10,LOW);
  digitalWrite(12,LOW);
  if (currentMicrosThree - previousMicrosThree >= intervalThree)
  {
    changeStateThree();
    digitalWrite(9,ledStateThree); // Drive motor step
  }
  if (currentMicrosFour - previousMicrosFour >= intervalFour)
  {
    changeStateFour();
    digitalWrite(11,ledStateFour); // Drive motor step
  }
}
  
//////////////////////////////////////////////////////////////////////////////////////////////////////
  difference = finalSteeringValue - previousFinalSteeringValue;
//////////////////////////////////////////////////////////////////////////////////////////////////////  
  if((difference>500)&&(wheelsPosition>=0)) // Clockwise from the centre
  {
    clockWise();
    if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Right wheel (SLOW)
    {
      changeStateTwo();
      digitalWrite(7,ledStateTwo); //STEP
    }
    if (currentMicrosOne - previousMicrosOne >= intervalOne) // Left wheel (FAST)
    {
      changeStateOne();
      digitalWrite(5,ledStateOne); //STEP
      wheelsPosition++;
      previousFinalSteeringValue++; // This must be within backets containing 'changeStateOne'.
    }
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////  
  if((difference>500)&&(wheelsPosition<0)) // Clockwise from the full lock position 
  {
    clockWise();
    if (currentMicrosOne - previousMicrosOne >= intervalOne) // Right wheel (FAST)
    {
      changeStateOne();
      digitalWrite(7,ledStateOne); //STEP
      wheelsPosition++;
      previousFinalSteeringValue++; // This must be within backets containing 'changeStateOne'.
    }
    if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Left wheel (SLOW)
    {
      changeStateTwo();
      digitalWrite(5,ledStateTwo); //STEP
    }
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////
  if((difference<-500)&&(wheelsPosition>=0)) // Anti-clockwise from the full lock position 
  {
    antiClockWise();
    if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Right wheel (SLOW)
    {
      changeStateTwo();
      digitalWrite(7,ledStateTwo); //STEP
    }
    if (currentMicrosOne - previousMicrosOne >= intervalOne) // Left wheel (FAST)
    {
      changeStateOne();
      digitalWrite(5,ledStateOne); //STEP
      wheelsPosition--;
      previousFinalSteeringValue--; // This must be within backets containing 'changeStateOne'.
    }
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////
  if((difference<-500)&&(wheelsPosition<0)) // Anti-clockwise from the centre 
  {
    antiClockWise();
    if (currentMicrosOne - previousMicrosOne >= intervalOne)  // Right wheel (FAST)
    {
      changeStateOne();
      digitalWrite(7,ledStateOne); //STEP
      wheelsPosition--;
      previousFinalSteeringValue--; // This must be within backets containing 'changeStateOne'.
    }
    if (currentMicrosTwo - previousMicrosTwo >= intervalTwo) // Left wheel (SLOW)
    {
      changeStateTwo();
      digitalWrite(5,ledStateTwo); //STEP
    }
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////
  difference = finalSteeringValue - previousFinalSteeringValue;
}


/*** Core 1 ***/

/* CPU1 Uninitialised Data */
StartOfUninitialised_CPU1_Variables
/* Put your CPU1 fast access variables that have no initial values here e.g. uint32 CPU1_var; */
EndOfUninitialised_CPU1_Variables

/* CPU1 Initialised Data */
StartOfInitialised_CPU1_Variables
/* Put your CPU1 fast access variables that have an initial value here e.g. uint32 CPU1_var_init = 1; */
EndOfInitialised_CPU1_Variables
  
void setup1() 
{
  SerialASC.begin(9600);
}
void loop1() 
{
  SerialASC.print("Steering Value= ");SerialASC.println(finalSteeringValue);
  SerialASC.print("Previous steering Value= ");SerialASC.println(previousFinalSteeringValue);
  SerialASC.print("Difference= ");SerialASC.println(difference);
  SerialASC.print("wheelsPosition= ");SerialASC.println(wheelsPosition);
  SerialASC.print("Drive= ");SerialASC.println(finalDriveValue);
  SerialASC.print("IntervalThree= ");SerialASC.println(intervalThree);
  SerialASC.print("IntervalFour= ");SerialASC.println(intervalFour);
  SerialASC.println("");
  delay(4000);
}


/*** Core 2 ***/

/* CPU2 Uninitialised Data */
StartOfUninitialised_CPU2_Variables
/* Put your CPU2 fast access variables that have no initial values here e.g. uint32 CPU2_var; */
EndOfUninitialised_CPU2_Variables

/* CPU2 Initialised Data */
StartOfInitialised_CPU2_Variables
/* Put your CPU2 fast access variables that have an initial value here e.g. uint32 CPU2_var_init = 1; */
long driveValue=0;
long steeringValue =0;
int i=0;

EndOfInitialised_CPU2_Variables


void setup2() {
  // put your setup code for core 2 here, to run once:


}


void loop2() 
{
  steeringValue=0;
  driveValue=0;
  i=0;
  while(i<200)
  { 
    steeringValue = steeringValue + analogRead(A0);
    driveValue = driveValue + analogRead(A1);
    i++;
  }
  finalDriveValue = driveValue/i;
  finalSteeringValue = 30*steeringValue/i;
  delay(10);
}
void changeStateOne()
{
  unsigned long currentMicrosOne = micros();
  previousMicrosOne = currentMicrosOne;
  if (ledStateOne == LOW) 
    {
    ledStateOne = HIGH;
    } 
  else 
    {
    ledStateOne = LOW;
    }
  digitalWrite(ledPin, ledStateOne);
}
void changeStateTwo()
{
  unsigned long currentMicrosTwo = micros();  
  previousMicrosTwo = currentMicrosTwo;
  if (ledStateTwo == LOW) 
    {
    ledStateTwo = HIGH;
    } 
  else 
    {
    ledStateTwo = LOW;
    }
  digitalWrite(ledPin, ledStateTwo);
}
void changeStateThree()
{
  unsigned long currentMicrosThree = micros();  
  previousMicrosThree = currentMicrosThree;
  if (ledStateThree == LOW) 
    {
    ledStateThree = HIGH;
    } 
  else 
    {
    ledStateThree = LOW;
    }
  digitalWrite(ledPin, ledStateThree);
}
void changeStateFour()
{
  unsigned long currentMicrosFour = micros();
  previousMicrosFour = currentMicrosFour;
  if (ledStateFour == LOW)
    {
    ledStateFour = HIGH;
    } 
  else
    {
    ledStateFour = LOW;
    }
  digitalWrite(ledPin, ledStateFour);
}
void clockWise()
{
  intervalOne = 1000; // Fast
  intervalTwo = 1250; // Slow
  digitalWrite(6,HIGH); //DIRECTION HIGH is clockwise
  digitalWrite(8,HIGH); //DIRECTION HIGH is clockwise
  unsigned long currentMicrosOne = micros();
  unsigned long currentMicrosTwo = micros();
}
void antiClockWise()
{
  intervalOne = 1000; // Fast
  intervalTwo = 1250; // Slow
  digitalWrite(6,LOW); //DIRECTION HIGH is clockwise
  digitalWrite(8,LOW); //DIRECTION HIGH is clockwise
  unsigned long currentMicrosOne = micros();
  unsigned long currentMicrosTwo = micros();
}
void speedDifferential()
{
  if(wheelsPosition>200)
  {
     intervalThree =  intervalThree+(wheelsPosition*intervalThree)/20000;
  }
  if(wheelsPosition<-200)
  {
     intervalFour =  intervalFour+(wheelsPosition*intervalFour)/-20000;
  }
}