 /*---------------------------------------------------------------------------------------*/
 void Print7headers (bool processed)
 {
-    processed? Serial.print("processed data \t") : Serial.print("received data \t");
- #ifdef RC_SPEKTRUM
-    Serial.print("Time\t");
-    Serial.print("TURN\t");
-    Serial.print("AUTO\t");
-    Serial.print("GO\t");
-    Serial.print("Rudder\t");
-    Serial.print("E-Stop\t");
-    Serial.println("Reverse\t"); 
+  processed ? Serial.print("processed data \t") : Serial.print("received data \t");
+#ifdef RC_SPEKTRUM
+  Serial.print("Time\t");
+  Serial.print("TURN\t");
+  Serial.print("AUTO\t");
+  Serial.print("GO\t");
+  Serial.print("Rudder\t");
+  Serial.print("E-Stop\t");
+  Serial.println("Reverse\t");
 #endif
 
 #ifdef RC_HITEC
-    Serial.print("Time\t");
-    Serial.print("TURN\t");
-    Serial.print("AUTO\t");
-    Serial.print("GO\t");
-    Serial.print("E-Stop\t");
-    Serial.print("Rudder\t");
-    Serial.println("Reverse\t"); 
+  Serial.print("Time\t");
+  Serial.print("TURN\t");
+  Serial.print("AUTO\t");
+  Serial.print("GO\t");
+  Serial.print("E-Stop\t");
+  Serial.print("Rudder\t");
+  Serial.println("Reverse\t");
 #endif
 }
 /*---------------------------------------------------------------------------------------*/
 void Print7 (bool processed, unsigned long results[7])
 {
-  
-    processed? Serial.print("processed data \t") : Serial.print("received data \t");
-    Serial.print(results[0]); Serial.print("\t");
-    Serial.print(results[1]); Serial.print("\t");
-    Serial.print(results[2]); Serial.print("\t");
-    Serial.print(results[3]); Serial.print("\t");
-    Serial.print(results[4]); Serial.print("\t");
-    Serial.print(results[5]); Serial.print("\t");
-    Serial.println(results[6]);
+
+  processed ? Serial.print("processed data \t") : Serial.print("received data \t");
+  Serial.print(results[0]); Serial.print("\t");
+  Serial.print(results[1]); Serial.print("\t");
+  Serial.print(results[2]); Serial.print("\t");
+  Serial.print(results[3]); Serial.print("\t");
+  Serial.print(results[4]); Serial.print("\t");
+  Serial.print(results[5]); Serial.print("\t");
+  Serial.println(results[6]);
 }
 /*---------------------------------------------------------------------------------------*/
 void LogData(unsigned long commands[7], SerialData *sensors)  // data for spreadsheet
 {
-     show7seg(HubSpeed_kmPh);
-     /*Serial.print("(ms) Time\t");
-     Serial.print("(cm/s) Speed\t");
-     Serial.print("(km/h) Speed\t");
-     Serial.print("(km/h) Hub Speed\t");
-     Serial.print("(deg) Angle\t");
-     Serial.print("Right\t");
-     Serial.print("Left\t");
-     Serial.print("Throttle\t");
-     Serial.print("Brake\t");
-     Serial.print("Steer\t");
-     Serial.println("(m) Distance");*/
-     Serial.print(millis()); Serial.print("\t");                        //(ms) Time
-     Serial.print(sensors->speed_cmPs); Serial.print("\t");             //(cm/s) Speed
-     Serial.print(sensors->speed_cmPs*36.0/1000.); Serial.print("\t");  //(km/h) Speed
-     Serial.print(HubSpeed_kmPh); Serial.print("\t");                   //(km/h) Hub Speed
-     Serial.print(sensors->angle_deg); Serial.print("\t");              //(deg) Angle
-     int right = analogRead(A3);
-     int left = analogRead(A2);
-     Serial.print(right); Serial.print("\t");                           //Right turn sensor
-     Serial.print(left); Serial.print("\t");                            //Left turn sensor
-     Serial.print(throttle_control); Serial.print("\t");                //Throttle
-     Serial.print(brake_control); Serial.print("\t");                   //Brake
-     Serial.print(steer_control); Serial.print("\t");                   //Steer
-     Serial.println(Odometer_m);                                        //(m) Distance
-//     Print7 (true, commands);
-}
-/*---------------------------------------------------------------------------------------*/
-void PrintHeaders (void)
-{
-    Serial.print("(ms) Time\t");
+  show7seg(HubSpeed_kmPh);
+  /*Serial.print("(ms) Time\t");
     Serial.print("(cm/s) Speed\t");
     Serial.print("(km/h) Speed\t");
     Serial.print("(km/h) Hub Speed\t");

          - Wheels are pointed straight ahead, and have been for a while.
          - Trike is not moving, and is stable.
    Calibration will block any response to controls; there will be no turning or movemnet during calibration.
    Angle sensors can drift, and should periodically be zeroed.    
*/
void CalibrateTurnAngle(int count, int pause)
{
    int totalRight = 0;
    int totalLeft = 0;
    int i, left, right;
    for (i = 0; i < count; i++)
    {
          totalRight += analogRead(A2);
          totalLeft += analogRead(A3);
          delay(pause);
    }
    right = totalRight / count;
    left  = totalLeft  / count;
    // Only recalibrate when instruments are reasonable
    // Do not make garbage readings the new normal.
    if(RIGHT_MIN_COUNT <= right && right <= RIGHT_MAX_COUNT)
    {
         RightStraight_A2 = right; 
         Right_Min_Count = RightStraight_A2 - 60;   // 60 counts is 20 degrees
         Right_Max_Count = RightStraight_A2 + 60;
     }
    else
    {
         RightStraight_A2 = RIGHT_MIN_COUNT + (RIGHT_MAX_COUNT - RIGHT_MIN_COUNT)/2 ;
         Right_Min_Count = RIGHT_MIN_COUNT;
         Right_Max_Count = RIGHT_MAX_COUNT;
    } 
    if  (LEFT_MIN_COUNT  <= left  && left  <= LEFT_MAX_COUNT)
    {
        LeftStraight_A3  = left;
        Left_Min_Count  = LeftStraight_A3  - 60; 
        Left_Max_Count  = LeftStraight_A3  + 60;
    }
    else
    {
        LeftStraight_A3  = LEFT_MIN_COUNT + (LEFT_MAX_COUNT - LEFT_MIN_COUNT)/2 ;
        Left_Min_Count = LEFT_MIN_COUNT;
        Left_Max_Count = LEFT_MAX_COUNT;
    }
//    Serial.print("\tCALIBRATE: Left Straight\t"); Serial.print(LeftStraight_A3);
//    Serial.print("\tRight Straight\t"); Serial.println(RightStraight_A2);
    old_turn_degx1000 = 0; // straight
}
/*---------------------------------------------------------------------------------------*/
/*======================ReadTurnAngle======================*/
int TurnAngle_degx10()
{
    long new_turn_degx1000;
    long expected_turn_degx1000;
    int new_turn_degx10;
    long min_ang, max_ang;
    bool OK_right = false;
    bool OK_left = false;
    int right = analogRead(A2);
    int left = analogRead(A3);
//    Serial.print("Left"); Serial.print("\t"); Serial.print(left);
//    Serial.print("\tRight"); Serial.print("\t"); Serial.print(right);
//    Serial.print("LeftStraight_A3"); Serial.print("\t"); Serial.print(LeftStraight_A3);
//    Serial.print("\tRightStraight_A2"); Serial.print("\t"); Serial.print(RightStraight_A2);
    if ((Right_Min_Count <= right) && (right <= Right_Max_Count))
       OK_right = true;    
    if ((Left_Min_Count <= left) && (left <= Left_Max_Count))
       OK_left = true;    
    long right_degx1000 = (right - RightStraight_A2) * RIGHT_DEGx1000pCOUNT;  
    long left_degx1000  = (left -  LeftStraight_A3) *  LEFT_DEGx1000pCOUNT;  
//    Serial.print("Left"); Serial.print("\t"); Serial.print(left_degx1000);
//    Serial.print("\tRight"); Serial.print("\t"); Serial.print(right_degx1000);
 
    expected_turn_degx1000 = old_turn_degx1000; 
    if (OK_left && OK_right)
    {  // use the median
        if (right_degx1000 < left_degx1000)
        {
            min_ang = right_degx1000;
            max_ang = left_degx1000;
        }
        else
        {
            min_ang = left_degx1000;
            max_ang = right_degx1000;
        }
        if (expected_turn_degx1000 < min_ang)
            new_turn_degx1000 = min_ang;
        else if (expected_turn_degx1000 > max_ang)
            new_turn_degx1000 = max_ang;
        else  
            new_turn_degx1000 = expected_turn_degx1000;
    }
    else if (OK_left)
    {
        new_turn_degx1000 = (left_degx1000 + expected_turn_degx1000)/2;
    }
    else if (OK_right)
    {
        new_turn_degx1000 = (right_degx1000 + expected_turn_degx1000)/2;
    }
    else
    {   // No sensors; use last valid measurement
        new_turn_degx1000 = old_turn_degx1000;
    }
    new_turn_degx10 = (int) (new_turn_degx1000 / 100);
    old_turn_degx1000 = new_turn_degx1000;
//  Serial.print("\tnew_turn_degx10"); Serial.print("\t"); Serial.println(new_turn_degx10);
    return new_turn_degx10;
}
/*---------------------------------------------------------------------------------------*/
void Throttle_PID(long error_speed_mmPs)

  noInterrupts();
  ProcessFallOfINT(RC_GO);
  RC_Done[RC_GO] = 1;
  Serial.println("GO");
  attachInterrupt(digitalPinToInterrupt(IRPT_GO), ISR_GO_rise, RISING);
  interrupts();
}
/*---------------------------------------------------------------------------------------*/
void ISR_ESTOP_fall() {
  noInterrupts();
  ProcessFallOfINT(RC_ESTP);
  RC_Done[RC_ESTP] = 1;
  //  if (RC_elapsed[RC_ESTP] > MIDDLE)
  //     E_Stop();
  Serial.println("ESTOP");
  attachInterrupt(digitalPinToInterrupt(IRPT_ESTOP), ISR_ESTOP_rise, RISING);
  interrupts();
}
/*---------------------------------------------------------------------------------------*/
