     SPI.begin(); 
     for (int channel = 0; channel < 4; channel++)
         DAC_Write (channel, 0);   // reset did not clear previous states
- 
+
     pinMode(Throttle, OUTPUT);
     pinMode(DiskBrake, OUTPUT);
     pinMode(Steer, OUTPUT);
     pinMode(AccelerateJoystick, INPUT);
     pinMode(SteerJoystick, INPUT);
     pinMode(JoystickCenter, INPUT);
-     
+
     moveBrake(NoBrake);   // release brake
     Serial.println("Initialized");
 }
+
 /*---------------------------------------------------------------------------------------*/
 char * GetWord(char * major, char * str)
 {
-	char * CSp1;
-
-	CSp1 = strstr(str, major);
-	if (CSp1!=NULL)
-	CSp1 += strlen(major);
-	return CSp1;
-}
+    char * CSp1;
+
+    CSp1 = strstr(str, major);
+    if (CSp1!=NULL)
+        CSp1 += strlen(major);
+    return CSp1;
+}
+
 float GetNumber(char *minor, char*Args)
 {
-  float data = NaN;
-  if (Args == NULL) return data;
-  // SENSOR, so grab the new sensor_speed.
-  char * Number = GetWord(minor, Args);
-  if (Number==NULL) return data;
-   // change } to 0
-   char* end = strchr(Number, '}');
-   if (end == NULL) return NaN;
-   *end = '\0';
-   data = atof(Number);
-   // change back to }
-   *end = '}';
-   // convert speed from km/h to mm/s
-//   sensor_speed_mmPs = (float)(data * 1000000.0 / 3600.0);
-   return data;
-}
+    float data = NaN;
+    if (Args == NULL) return data;
+    // SENSOR, so grab the new sensor_speed.
+    char * Number = GetWord(minor, Args);
+    if (Number==NULL) return data;
+    // change } to 0
+    char* end = strchr(Number, '}');
+    if (end == NULL) return NaN;
+    *end = '\0';
+    data = atof(Number);
+    // change back to }
+    *end = '}';
+    // convert speed from km/h to mm/s
+    // sensor_speed_mmPs = (float)(data * 1000000.0 / 3600.0);
+    return data;
+}
+
 /*---------------------------------------------------------------------------------------*/
 #define MSG_NONE 0
 #define MSG_SENSOR 1

    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)
{

   which is something of a black art.
   For more information, search for:
   VanDoren Proportional Integral Derivative Control
 */
void Throttle_PID(float error_speed_mmPs)
{ 
    static float speed_errors[ERROR_HISTORY];
    static int error_index = 0;
    int i;
    static float error_sum = 0;
    float mean_speed_error = 0;
    float extrapolated_error = 0;
    float PID_error;
    const float P_tune = 0.1; //0.4;
    const float I_tune = 0.0; //0.5;
    const float D_tune = 0.0; //0.1;
    const float speed_tolerance_mmPs = 75.0;  // about 0.2 mph
    // setting the max_error affacts control: anything bigger gets maximum response
    const float max_error_mmPs = 2500.0; // about 5.6 mph

    error_sum -= speed_errors[error_index];
    speed_errors[error_index] = error_speed_mmPs;
    error_sum += error_speed_mmPs;
    mean_speed_error = error_sum / ERROR_HISTORY;
    i = (error_index-1) % ERROR_HISTORY;
    extrapolated_error = 2 * error_speed_mmPs - speed_errors[i];

    PID_error = P_tune * error_speed_mmPs 
            + I_tune * mean_speed_error
            + D_tune * extrapolated_error;
    Serial.print("PID_error: ");
    Serial.print(PID_error);
    if (PID_error > speed_tolerance_mmPs)
    {  // too fast
        Serial.print(" too fast");
        float throttle_decrease = (FullThrottle - MinimumThrottle) * PID_error / max_error_mmPs;
        throttle_control -= throttle_decrease;
        if (throttle_control < MinimumThrottle)
            throttle_control = MinimumThrottle;
        moveVehicle(throttle_control);

        float brake_increase = (NoBrake - FullBrake) * PID_error / max_error_mmPs;
        // NoBrake = 207; FullBrake = 167; 
        brake_control -= brake_increase;
        if (brake_control < FullBrake)
            brake_control = FullBrake;
        moveBrake(brake_control);
    }
    else if (PID_error < speed_tolerance_mmPs)
    {  // too slow
        Serial.print(" too slow");
        float throttle_increase = (FullThrottle - MinimumThrottle) * PID_error / max_error_mmPs;
        throttle_control += throttle_increase;
        if (throttle_control > FullThrottle)
            throttle_control = FullThrottle;
        moveVehicle(throttle_control);

        // release brakes
        float brake_decrease = (NoBrake - FullBrake) * PID_error / max_error_mmPs;
        // NoBrake = 207; FullBrake = 167; 
        brake_control += brake_decrease;
        if (brake_control > NoBrake)
            brake_control = NoBrake;
        moveBrake(brake_control);
    }
    // else maintain current speed
    Serial.println();
    //Serial.print("Thottle Brake,");  // csv for spreadsheet
    Serial.print(throttle_control);
    Serial.print(",");
    Serial.print(brake_control);
    Serial.print(",");
    Serial.print(drive_speed_mmPs);
    Serial.print(",");
    Serial.println(sensor_speed_mmPs); 
}

/*---------------------------------------------------------------------------------------*/
