     This turns control over to a high level computer and begins motion to the next waypoint.
     Waypoints are loaded into the higher level computer and are not directly settable for the C2 module.
     Once under computer control, manual control will be unresponsive.
-   
+
    Future enhancements can make transfer between manual and automatic control similar to automotive 
-   cruise control. 
- 
+   cruise control.
+
  Outputs are
  1) Analog 0-4 V signal for traction motor speed
  2) Pulse wave Modulated (PWM) signal for brakes.
  3) PWM signal for steering.
- 
- 
  */
 
-// Input/Output (IO) pin names for the MegaShieldDB printed circuit board (PCB)
-#include "IOPCB.h"
-
-// When setting up the project, select
-//   Sketch  |  Import Library ... |  SPI
-// include the Serial Periferal Interface (SPI) library:
-#include <SPI.h>
 // The MegaShieldDB has a four channel Digital to Analog Converter (DAC).
 // Basic Arduino cannot write a true analog signal, but only PWM.
 // Many servos take PWM.
 // An electric bicycle (E-bike) throttle expects an analog signal.
 // We have found that feeding a pwm signal to an e-bike controller makes the motor chug at low speed.
+
+// Input/Output (IO) pin names for the MegaShieldDB printed circuit board (PCB)
+#include "IOPCB.h"
+
+// When setting up the project, select
+//   Sketch  |  Import Library ... |  SPI
+// include the Serial Periferal Interface (SPI) library:
+#include <SPI.h>
 
 #ifndef TRUE
 #define TRUE 1

    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;
}
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;
}
/*---------------------------------------------------------------------------------------*/
#define MSG_NONE 0
#define MSG_SENSOR 1

   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); 
}

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