     ReadINU.
     Set attitude.
     Read Hall Odometer;  */
-//   IMU.Read(GPS_reading);
-
+//   IMU.Read(GPS_reading);   
+ 
 /*  Read Optical Odometer;
-    Read lane deviation;
+    Read lane deviation;  
     If (message from C4)
     { */
 //      readline(2);  // C4 Path planner on serial 2 get new route and speed


Yes: Distance and bearing to landmarks / obstacles.

Yes: S7: GPS.  GPS should not the primary navigation sensor and the robot should be
able to operate indoors, in tunnels or other areas where GPS is not available
or accurate.

Sensed navigation information is passed to C3/C4, which use prexisting or SLAM
digital maps, intended path to refine the estimated position.
C3/C4 may use a particle filter.
C3/C4 may also receive lateral lane deviation from a camera (C7).

The odometry sensor is one dimensional and gives the position along the
intended path. Lane deviation is also one dimensional and gives position
normal to the intended path. Odometry, lane following and a digital map
should be sufficient for localization.  An odometer will drift. This drift
can be nulled out by landmark recognition or GPS.

An alternative to the Kalman Filter (KF) is fuzzy numbers.

          dataFile.print(Header);
          dataFile.println(ObstHeader);
          dataFile.close();
      }  
    }
    pinMode(CYCLOMETER, INPUT);
    attachInterrupt (5, WheelRev, RISING);

}
/*---------------------------------------------------------------------------------------*/ 
void waypoint::SetTime(char *pTime, char * pDate)
{
//    GPSfile = "mmddhhmm.CSV";
     strncpy(GPSfile,   pDate+2, 2);  // month   
     strncpy(GPSfile+2, pDate, 2);    // day    
     strncpy(GPSfile+4, pTime,2);     // GMT hour
     strncpy(GPSfile+6, pTime+2,2);   // minute
     Serial.println(GPSfile); 
    
     strncpy(StartTime,     pDate+4, 2);  // year   
     strncpy(StartTime+3,   pDate+2, 2);  // month   
     strncpy(StartTime+6,   pDate, 2);    // day    
     strncpy(StartTime+9,   pTime,2);     // GMT hour
     strncpy(StartTime+12,  pTime+2,2);   // minute
     strncpy(StartTime+15,  pTime+4,2);   // second
     strncpy(StartTime+18,  pTime+7,3);   // millisecond
     
}

/*---------------------------------------------------------------------------------------*/ 
void loop() 
{
    unsigned long deltaT_ms;
    unsigned long time = millis();
