     int kind = MSG_NONE;
     float data;
     Serial.println(IncomingMessage);
-	// Determine if message is "SENSOR {Speed xxx.xx}"	
-	char * Args = GetWord ("SENSOR", IncomingMessage);
-	if (Args != NULL)
-	{	
-            data = GetNumber("Speed", Args);
-    	    // convert speed from km/h to mm/s
-    	    if (data != NaN) 
-            {
-                sensor_speed_mmPs = (float)(data * 1000000.0 / 3600.0);
-                kind = MSG_SENSOR;
-            }
-	}
-	// Determine if message is "DRIVE {Speed xxx.xx}"	
-        Args = GetWord ("DRIVE", IncomingMessage);
-	if (Args != NULL)
-	{	
-            data = GetNumber("Speed", Args);
-    	    // convert speed from rev/s to mm/s
-    	    if (data != NaN) 
-            {
-                drive_speed_mmPs = (float)(data * PI * WHEEL_DIAMETER_MM);
-                kind = MSG_DRIVE;
-            }
-	}
+    // Determine if message is "SENSOR {Speed xxx.xx}"	
+    char * Args = GetWord ("SENSOR", IncomingMessage);
+    if (Args != NULL)
+    {	
+        data = GetNumber("Speed", Args);
+        // convert speed from km/h to mm/s
+        if (data != NaN) 
+        {
+            sensor_speed_mmPs = (float)(data * 1000000.0 / 3600.0);
+            kind = MSG_SENSOR;
+        }
+    }
+    // Determine if message is "DRIVE {Speed xxx.xx}"	
+    Args = GetWord ("DRIVE", IncomingMessage);
+    if (Args != NULL)
+    {	
+        data = GetNumber("Speed", Args);
+        // convert speed from rev/s to mm/s
+        if (data != NaN) 
+        {
+            drive_speed_mmPs = (float)(data * PI * WHEEL_DIAMETER_MM);
+            kind = MSG_DRIVE;
+        }
+    }
     return kind;
 }
-/*---------------------------------------------------------------------------------------*/
-void Throttle_PID(float error_speed_mmPs)
-
+
+/*---------------------------------------------------------------------------------------*/
 /* Use throttle and brakes to keep vehicle at a desired speed.
    A PID controller uses the error in the set point to increase or decrease the juice.
    P = proportional; change based on present error

    endTime = time + LOOP_TIME_MS ;
    while (time < endTime)
    {
	if ( Serial3.available() > 0) 
	{
	    // read the incoming byte from C4:
	    incomingByte =  Serial3.read();
	    {
		IncomingMessage[InIndex] = (char)(incomingByte);
		if (IncomingMessage[InIndex] == '\0'
		 || InIndex >= BUFFER_SIZE-1)
		{
		    int kind = ProcessMessage();  // see what we got
		    InIndex = 0;
                    if (kind != MSG_SENSOR)  // Sensor messages originate from C6
		        Serial3.print(IncomingMessage); // pass msg on to C6
                    //Serial.println(IncomingMessage);  // for monitor
		}
		else
		{
		    ++InIndex;    	
		}
	    }
	}
	time = millis();
    }
 
  float error_speed_mmPs = sensor_speed_mmPs - drive_speed_mmPs;
  Throttle_PID(error_speed_mmPs);
  
 // apply steering
  steer_control =  Straight;
  moveSteer(steer_control); 
 
}
/*---------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------*/ 
void moveBrake(int i)
{
     analogWrite(DiskBrake, i);
}
/*---------------------------------------------------------------------------------------*/
void moveSteer(int i)
{
     analogWrite(Steer, i);
}
/*---------------------------------------------------------------------------------------*/
void moveVehicle(int counts)
{

/*
 Elcano Contol Module C2 Basic: Bare bones low level control of vehicle.
 This code is the first step in moving the vehicle under high level control from the C3 Pilot. 

 Throttle, brake and steering inputs can come from three sources: 
 1) On board joystick
     e.g. 
 2) Remotely controlled (RC) joystick and buttons
     e.g: HiTec Optic 5 RCS: http://hitecrcd.com/images/products/pdf/22_OPTIC_5_MANUAL_P.pdf
 3) An external high level computer, such as Arduino C3 Pilot.

 Normally the vehicle will be configured to use either (1) or (2) for manual control. 
 Automatic control (3) is always an option.

 The inputs consist of analog or Pulse wave Modulated (PWM) values.  Inputs are:
 1) Throttle
 2) Brake
