carcode

#include <Stepper.h>
#include <SoftwareSerial.h>
#include <RoboClaw.h>


//See limitations of Arduino SoftwareSerial
SoftwareSerial serial(10,11);
RoboClaw roboclaw(&serial,10000);

#define address 0x80

float goalSpeed = -1;
float currentSpeed = -1;
float goalDegree = 0;
double currentDegree = 0;
int currentStepSteer = 0;
int currentStepAccel = 0;
int goalStepSteer = 0;
const int stp1 = 13; // sets step and direction pins for drivers stp1 = x and stp2 = y
const int dir1 = 12;
const int steerStp = 6;
const int steerDir = 7;
const float tol = 1;

void setup() {
  //Open roboclaw serial ports
  roboclaw.begin(38400);
  Serial.begin( 9600 );
  // Sets the  pins as Outputs
  pinMode(stp1,OUTPUT);
  pinMode(dir1,OUTPUT);
  pinMode( steerStp, OUTPUT );
  pinMode( steerDir, OUTPUT );
  //resetActuator();
}

void loop() {

  readIncoming();
  if ( goalSpeed == 0 ) {
    goHome();
    brake();
  } else if ( currentSpeed < goalSpeed && currentStepAccel <= 28 ) {
    acellerateOneStep();
    currentStepAccel++;
  } else if ( currentSpeed > goalSpeed && currentStepAccel >= 0 ) {
    decelerateOneStep();
    currentStepAccel--;
  }
  if ( goalDegree > currentDegree && currentStepSteer <= 169200 && currentStepSteer >= -169200 ) {
    steerRight();
  } else if ( goalDegree < currentDegree && currentStepSteer <= 169200 && currentStepSteer >= -169200 ) {
    steerLeft();
  }
  currentDegree = convertToDegree( currentStepSteer );

}

/**
 * First number is goal, second number is actual.
 */
void readIncoming() {
  if ( Serial.available() > 0 ) {
    if ( Serial.find( ":" ) ) {
      goalSpeed = Serial.parseFloat();
      currentSpeed = Serial.parseFloat();
      goalDegree = Serial.parseFloat();

      Serial.write( Serial.print(goalSpeed) );
      Serial.write( " " );
      Serial.write( Serial.print(currentSpeed) );
      Serial.write( " " );
      Serial.write( Serial.print( goalDegree ) );
      Serial.write( " " );
      Serial.write( Serial.print( currentDegree, 6 ) );
      Serial.write( "\n");
  }

  }
}

void resetActuator() {
  roboclaw.BackwardM2( address, 120 );
  delay(5000);
}

void steerRight() {

  digitalWrite( steerDir, HIGH );
  delayMicroseconds( 500 );
  for ( int n = 0; n < 175 && goalDegree > currentDegree; n++ ) {
    digitalWrite( steerStp, HIGH );
    delayMicroseconds( 100 );
    digitalWrite( steerStp, LOW );
    delayMicroseconds( 100 );
    currentStepSteer++;
    currentDegree = convertToDegree( currentStepSteer );
  }

}

void steerLeft() {
  digitalWrite( steerDir, LOW );
  delayMicroseconds( 500 );
  for ( int n = 0; n < 175 && goalDegree < currentDegree; n++ ) {
    digitalWrite( steerStp, HIGH );
    delayMicroseconds( 100 );
    digitalWrite( steerStp, LOW );
    delayMicroseconds( 100 );
    currentStepSteer--;
    currentDegree = convertToDegree( currentStepSteer );
  }
}

void acellerateOneStep() {

  digitalWrite( dir1, HIGH );
  delayMicroseconds( 50 );
  digitalWrite( stp1, HIGH );
  delayMicroseconds( 50 );
  digitalWrite( stp1, LOW );
  delay( 2 );

}

void decelerateOneStep() {

  digitalWrite( dir1, LOW );
  delayMicroseconds( 50 );
  digitalWrite( stp1, HIGH );
  delayMicroseconds( 50 );
  digitalWrite( stp1, LOW );
  delay( 2 );

}


void brake() {
  roboclaw.ForwardM2( address, 127 );
  delay(1500);
  roboclaw.BackwardM2( address, 127 );
  delay(1000);
  while ( goalSpeed == 0 ) {
    roboclaw.ForwardM2( address, 0 );
    readIncoming();
  }
  roboclaw.BackwardM2( address, 127 );
  delay(500);
  roboclaw.ForwardM2( address, 0 );
}

void goHome() {
  digitalWrite( dir1, LOW);
  for ( int i = currentStepAccel; i >= 0; i-- ) {
      digitalWrite( stp1, HIGH);
      delayMicroseconds(50);
      digitalWrite( stp1, LOW );
      delay( 2 );
      currentStepAccel--;
      //Serial.write( "\n" );
  }
}

double convertToDegree( int step1 ) {
  return (1.0/209.0) * step1;
}

int convertToStep( float degree ){
  return (int) degree / 4.4;
}