system_controller: C:/Documents and Settings/student/My Documents/robo_magellan/visual_studio_(system_controller)/system_controller/SystemController/RaceStrategy.cs Source File

00001 using System;
00002 using System.Collections.Generic;
00003 using System.Linq;
00004 using System.Text;
00005 using System.Threading;
00006 using UtmConvert;
00007 
00008 namespace system_controller {
00009     class RaceStrategy {
00010 
00011                 public delegate void updateUiEventHandler();
00012                 public event updateUiEventHandler updateUiEvent;
00013 
00014                 SystemData _data;
00015                 int _pointCount = 0;
00016                 public int PointCount {
00017                         get { return _pointCount; }
00018                 }
00019 
00020                 UtmConvert.UtmPoint _currentLocationVector = new UtmPoint();
00021                 public UtmConvert.UtmPoint CurrentVectorLocation {
00022                         get { return _currentLocationVector; }
00023                 }
00024 
00025                 UtmConvert.UtmPoint _currentLocationGps = new UtmPoint();
00026                 public UtmConvert.UtmPoint CurrentGpsLocation {
00027                         get { return _currentLocationGps; }
00028                 }
00029 
00030                 UtmConvert.UtmPoint _currentLocation = new UtmPoint();
00031                 public UtmConvert.UtmPoint CurrentLocation {
00032                         get { return _currentLocation; }
00033                 }
00034 
00035                 public readonly double steeringKp;
00036                 //the distance the robot must be from the cone for it to start using
00037                 //the vision system to look for it
00038                 public readonly double coneDistanceThreshold; //meters
00039                 //the distance the robot must be from a dummy waypoint 
00040                 //in order to increment the waypoint counter
00041                 public readonly double dummyDistanceThreshold; //meters
00042                 double _waypointDistance = 0;
00043                 public double WaypointDistance {
00044                         get { return _waypointDistance; }
00045                 }
00046 
00047                 //the angular difference between the robots
00048                 //vector heading and position and theta
00049                 double _crossTrackError = 0;
00050                 public double CrossTrackError {
00051                         get { return _crossTrackError; }
00052                 }
00053 
00054                 string _turnDirection = "null"; //left, right
00055                 public string TurnDirection {
00056                         get { return _turnDirection; }
00057                 }
00058                 //theta according to vector navigation
00059                 double _theta = 0;
00060                 public double Theta {
00061                         get { return _theta; }
00062                 }
00063 
00064                 //modified compass heading 
00065                 double _offsetCompassHeading;
00066                 public double OffsetCompassHeading {
00067                         get { return _offsetCompassHeading; }
00068                 }
00069 
00070                 bool _arrivedAtWaypoint = true;
00071                 public bool ArrivedAtWaypoint {
00072                         get { return _arrivedAtWaypoint; }
00073                 }
00074 
00075                 //the x axis delta between the waypoint and the robot
00076                 double _xDelta = 0;
00077                 //the y axis delta between the waypoint and the robot
00078                 double _yDelta = 0;
00079                 int _gpsPositionConf = 0;
00080 
00081                 int _viewAttempts = 0;
00082                 int _coneViewCount = 0; //confidence level that it is a cone
00083                 bool _touchedCone = false;
00084                 //tells whether the robot is looking for the cone
00085                 bool _lookingForCone = false;
00086                 public bool LookingForCone {
00087                         get { return _lookingForCone; }
00088                 }
00089 
00090                 VectorCalculator _vCalc;
00091                 double _previousHeading = 0;
00092                 double _previousDistance = 0;
00093 
00094                 readonly int minimumSpeed;
00095                 readonly int maximumSpeed;
00096 
00097                 double steering = 0;
00098                 DataLogger _dataLog;
00099 
00100                 EventLogger _eventLog;
00101                 public EventLogger EventLog {
00102                         get { return _eventLog; }
00103                         set { _eventLog = value; }
00104                 }
00105 
00106                 double _searchDistance = 10;
00107                 bool _finishedRace = false;
00108 
00109                 public RaceStrategy(ref SystemData data, double steerKp, double searchDistance
00110                         , double coneDistance, double dummyDistance, int minSpeed, int maxSpeed) {
00111                         steeringKp = steerKp;
00112                         _searchDistance = searchDistance;
00113                         coneDistanceThreshold = coneDistance;
00114                         dummyDistanceThreshold = dummyDistance;
00115                         minimumSpeed = minSpeed;
00116                         maximumSpeed = maxSpeed;
00117                         _dataLog = new DataLogger();
00118                         _eventLog = new EventLogger();
00119                         _dataLog.openFile();
00120                         _eventLog.openFile();
00121                         _eventLog.write(DateTime.Now.ToString() + ": race started\r\n");
00122                         //print column headings for the data logger
00123                         printDataLogHeadder();
00124                         _data = data;
00125                         _data.initPlatform();
00126                         _data.initNavigation();
00127                         _data.initGps();
00128                         _data.initVision();
00129                         calculateOffsetCompassHeading();
00130                         _previousHeading = _offsetCompassHeading;
00131                         //clear encoder counter
00132                         _data.Navigation.clearEncoderCounts();
00133                         _previousDistance = 0.0;
00134                         _data.Navigation.updateDataEvent += navigateToWaypoint;
00135                         _data.Platform.redLedOff();
00136                         _data.Platform.greenLedOn();
00137                         _data.Navigation.setRunMode(2); //set run mode to two
00138                         _data.Navigation.setThrottleSetpoint(maximumSpeed); //set the speed to fast
00139                 }
00140 
00141                 public void shutdownRace() {
00142                         _data.Navigation.updateDataEvent -= navigateToWaypoint;
00143                         _dataLog.closeLogFile();
00144                         _eventLog.closeLogFile();
00145                         _data.Platform.greenLedOff();
00146                         _data.Platform.resetPlatformController();
00147                         _data.Navigation.setThrottleSetpoint(0);
00148                         _data.Navigation.setRunMode(0);
00149                         _data.Navigation.resetNavigationController();
00150                 }
00151 
00152                 double distFromPrevWaypoint = 0, distFromPrevWptMark = 0;
00153                 int vectorPositionConf = 12;
00154                 const int GPS_CONF_LIMIT = 6;
00155 
00156                 //update the robots current location
00157                 private void updateCurrentLocation() {
00158                         if (_arrivedAtWaypoint) {
00159                                 _arrivedAtWaypoint = false;
00160                                 if ((_data.Parser.UtmPointSetList[0].Points[_pointCount]
00161                                         .Note == "cone" && _touchedCone) || _pointCount == 0) {
00162                                         _currentLocationVector = new UtmPoint(new Point(
00163                                                 _data.Parser.UtmPointSetList[0].Points[_pointCount].Point.X
00164                                                 , _data.Parser.UtmPointSetList[0].Points[_pointCount].Point.Y)
00165                                                 , _data.Parser.UtmPointSetList[0].Points[_pointCount].Zone
00166                                                 , _data.Parser.UtmPointSetList[0].Points[_pointCount].Note
00167                                                 , _data.Parser.UtmPointSetList[0].Points[_pointCount].Handle);
00168                                         _touchedCone = false;
00169                                 } else //this compensates for vector navigation drift, it pulls the vecor position 
00170                                         _currentLocationVector = new UtmPoint(new Point(_currentLocation.Point.X //towards
00171                                                 , _currentLocation.Point.Y), _data.ConvertUtm.Zone, "", ""); //the GPS position
00172                                 _vCalc = new VectorCalculator(_currentLocationVector);
00173                                 //only increment this in one place
00174                                 if (_pointCount + 1 < _data.Parser.UtmPointSetList[0].Points.Count) {
00175                                         _eventLog.write(DateTime.Now.ToString()
00176                                                 + ": arrived at waypoint " + _pointCount.ToString() + ", " +
00177                                                 _data.Parser.UtmPointSetList[0].Points[_pointCount].Handle + ", "
00178                                                 + _data.Parser.UtmPointSetList[0].Points[_pointCount].Note + "\r\n");
00179                                         distFromPrevWptMark = _data.Navigation.Data.Meters;
00180                                         ++_pointCount;
00181                                 } else {
00182                                         _finishedRace = true; //the end of the race, this is a big deal!
00183                                         _eventLog.write(DateTime.Now.ToString() + ": finished race\r\n");
00184                                 }
00185                         }
00186                         if (_data.Gps.Gga.Data.QualityIndicator > 0) {
00187                                 _gpsPositionConf = _data.Gps.Gga.Data.SatelitesInView;
00188                                 _data.ConvertUtm.convertLatLonToUtm(ConvertDegRad.getRadians(
00189                                         _data.Gps.Gga.Data.LatDeg, _data.Gps.Gga.Data.LatMin
00190                                         , _data.Gps.Gga.Data.NorthOrSouth)
00191                                         , ConvertDegRad.getRadians(_data.Gps.Gga.Data.LonDeg
00192                                         , _data.Gps.Gga.Data.LonMin, _data.Gps.Gga.Data.EastOrWest));
00193                                 _currentLocationGps = new UtmPoint(new Point(_data.ConvertUtm.Easting
00194                                         , _data.ConvertUtm.Northing), _data.ConvertUtm.Zone, "", "");
00195                         }
00196                         calculateOffsetCompassHeading(); //use vector addition to update location
00197                         if (_previousDistance != _data.Navigation.Data.Meters) {
00198                                 _currentLocationVector = _vCalc.getPointFromVector(_currentLocationVector
00199                                         , _data.Navigation.Data.Meters - _previousDistance
00200                                         , (_previousHeading + _offsetCompassHeading) / 2);
00201                                 _previousHeading = _offsetCompassHeading;
00202                                 _previousDistance = _data.Navigation.Data.Meters;
00203                                 distFromPrevWaypoint = _data.Navigation.Data.Meters - distFromPrevWptMark;
00204                         }
00205                         _currentLocation = new UtmPoint(new Point(_currentLocationVector.Point.X
00206                                 , _currentLocationVector.Point.Y), _data.ConvertUtm.Zone, "", "");
00207                         if (_gpsPositionConf > GPS_CONF_LIMIT) {
00208                                 vectorPositionConf = (1 / (1 + ((int)distFromPrevWaypoint / 60))) * 12; //if we are to factor the GPS position
00209                                 _currentLocation.Point.X += (_currentLocationGps.Point.X                //into the actual location of the robot
00210                                         * (double)(_gpsPositionConf - GPS_CONF_LIMIT))                      //then we also want to weigh the vector
00211                                         + (_currentLocationVector.Point.X * (double)vectorPositionConf);    //the robot position relative to how far 
00212                                 _currentLocation.Point.Y += (_currentLocationGps.Point.Y                //has travelled from the last waypoint
00213                                         * (double)(_gpsPositionConf - GPS_CONF_LIMIT))
00214                                         + (_currentLocationVector.Point.Y * (double)vectorPositionConf);
00215                                 _currentLocation.Point.X /= (double)((_gpsPositionConf
00216                                         - GPS_CONF_LIMIT) + (vectorPositionConf + 1));
00217                                 _currentLocation.Point.Y /= (double)((_gpsPositionConf
00218                                         - GPS_CONF_LIMIT) + (vectorPositionConf + 1));
00219                         }
00220                 }
00221 
00222                 private void checkArrivedAtWaypointWaypoint() {
00223                         //calculate x and y deltas
00224                         _xDelta = _data.Parser.UtmPointSetList[0]
00225                                 .Points[_pointCount].Point.X - _currentLocation.Point.X;
00226                         _yDelta = _data.Parser.UtmPointSetList[0]
00227                                 .Points[_pointCount].Point.Y - _currentLocation.Point.Y;
00228                         //update position
00229                         _waypointDistance = Math.Sqrt(Math.Pow(_xDelta, 2) + Math.Pow(_yDelta, 2));
00230                         //if(cone waypoint then must be touching cone)
00231                         //warning: must be able to navigate to cone 
00232                         //even if it is immediately close by
00233                         if (_data.Parser.UtmPointSetList[0].Points[_pointCount].Note == "cone"
00234                                 && _waypointDistance <= coneDistanceThreshold) {
00235                                 //this sets arrivedAtWaypoint to true if cone is touched
00236                                 prepareToLookForCone();
00237                         } else if (_waypointDistance <= dummyDistanceThreshold) {
00238                                 //if dummy then must be right on top of dummy 
00239                                 //to increment
00240                                 _arrivedAtWaypoint = true;
00241                         }
00242                 }
00243 
00244                 //main "loop" called by navigation controller events
00245                 private void navigateToWaypoint() {
00246                         if (_finishedRace) {
00247                                 if (_data.Navigation.Data.RunMode != 0)
00248                                         _data.Navigation.setRunMode(0); //set run mode to zero
00249                                 if (_data.Navigation.Data.ThrottleSetpoint != 0)
00250                                         _data.Navigation.setThrottleSetpoint(0); //set the speed to stop
00251                         } else {
00252                                 updateCurrentLocation();
00253                                 if (!_lookingForCone) checkArrivedAtWaypointWaypoint();
00254                                 calculateTheta();
00255                                 calculateCrossTrackError();
00256                                 determineTurnDirection();
00257                                 if (!_lookingForCone && _crossTrackError > 0.5)
00258                                         steerToCone();
00259                                 //post update to the UI
00260                                 if (updateUiEvent != null) updateUiEvent.Invoke();
00261                                 if (_pointCount > 0) LogData();
00262                         }
00263                 }
00264 
00265                 private void steerToCone() {
00266                         if (_turnDirection == "left") {
00267                                 steering = _crossTrackError * steeringKp;
00268                                 _data.Navigation.setSteeringSetpoint((int)Math.Round(steering, 0));
00269                         } else if (_turnDirection == "right") {
00270                                 steering = -_crossTrackError * steeringKp;
00271                                 //if (steering > 50.0) steering = 50.0;
00272                                 //else if (steering < -50.0) steering = -50.0;
00273                                 _data.Navigation.setSteeringSetpoint((int)Math.Round(steering, 0));
00274                         }
00275                 }
00276 
00277                 //calculate theta
00278                 //what is the robots desired heading?
00279                 //use vector navigation and gps.
00280                 private void calculateTheta() {
00281                         if (_xDelta == 0 && _yDelta > 0) _theta = 90;
00282                         else if (_xDelta == 0 && _yDelta < 0) _theta = 270;
00283                         else {
00284                                 _theta = ConvertDegRad.getDegrees
00285                                         (Math.Atan(_yDelta / _xDelta));
00286                                 if (_xDelta < 0 && _yDelta > 0)
00287                                         _theta = 180.0 + _theta;
00288                                 else if (_xDelta < 0 && _yDelta < 0)
00289                                         _theta = 180.0 + _theta;
00290                                 else if (_xDelta > 0 && _yDelta < 0)
00291                                         _theta = 360.0 + _theta; 
00292                         }
00293                 }
00294 
00295                 //the angular difference between the robots heading and its desired heading
00296                 private void calculateCrossTrackError() {
00297                         _crossTrackError = Math.Abs(_theta - _offsetCompassHeading);
00298                         if (_crossTrackError > 180)
00299                                 _crossTrackError = 360.0 - _crossTrackError;
00300                 }
00301 
00302                 //based on the robots heading and theta, decide which direction to turn
00303                 private void determineTurnDirection() {
00304                         calculateOffsetCompassHeading();
00305                         if (_theta - _offsetCompassHeading < 0
00306                                 && Math.Abs(_theta - _offsetCompassHeading) >= 180) _turnDirection = "left";
00307                         else if (_theta - _offsetCompassHeading > 0
00308                                 && Math.Abs(_theta - _offsetCompassHeading) >= 180) _turnDirection = "right";
00309                         else if (_theta - _offsetCompassHeading < 0
00310                                 && Math.Abs(_theta - _offsetCompassHeading) < 180) _turnDirection = "right";
00311                         else if (_theta - _offsetCompassHeading > 0
00312                                 && Math.Abs(_theta - _offsetCompassHeading) < 180) _turnDirection = "left";
00313                         else _turnDirection = "null";
00314                 }
00315 
00316                 //compensate for the fact that compasses increase in degrees clockwise
00317                 //wheras, trigonometric functions in a coordinate plane go counterclockwise
00318                 private void calculateOffsetCompassHeading() {
00319                         if (_data.Platform.Data.CompassHeadingDouble + 90 <= 180)
00320                                 _offsetCompassHeading = 180 
00321                                         - (_data.Platform.Data.CompassHeadingDouble + 90.0);
00322                         else _offsetCompassHeading =  360 
00323                                 - (_data.Platform.Data.CompassHeadingDouble - 90.0);
00324                 }
00325 
00326                 //is the robot near a cone?
00327                 public void prepareToLookForCone() {
00328                         _lookingForCone = true;
00329                         _data.Navigation.setRunMode(0); //set run mode to zero
00330                         _data.Navigation.setThrottleSetpoint(0); //stop the robot
00331                         _data.Vision.initializeCamera();
00332                         _data.Vision.startCamera();
00333                         //also post to the debug console
00334                         _eventLog.write(DateTime.Now.ToString() + ": prepairing to look for cone, "
00335                                 + "distance to cone is " + _waypointDistance.ToString() + " meters\r\n");
00336                         _data.Vision.updateDataEvent += lookForCone;
00337                 }
00338 
00339                 private void lookForCone() {
00340                         ++_viewAttempts;
00341                         if (_data.Vision.Data.ConeAquired) ++_coneViewCount;
00342                         else _coneViewCount = 0;
00343                         if (_coneViewCount > 8) {
00344                                 _data.Vision.updateDataEvent -= lookForCone;
00345                                 _viewAttempts = 0;
00346                                 _coneViewCount = 0;
00347                                 _data.Platform.redLedOn();
00348                                 _eventLog.write(DateTime.Now.ToString() + ": see cone, saving image\r\n");
00349                                 _data.Vision.saveRawImage();
00350                                 _data.Navigation.setRunMode(0); //set run mode to zero
00351                                 _data.Navigation.setThrottleSetpoint(minimumSpeed); //set the speed to slow
00352                                 //this triggers the delegate assigned to touchCone
00353                                 _data.Navigation.updateDataEvent += touchCone;
00354                         }
00355                         if (_viewAttempts > 32) {
00356                                 //if dont see cone drive around looking for it
00357                                 //execute a search pattern i.e. drive around in a circle
00358                                 _data.Vision.updateDataEvent -= lookForCone;
00359                                 elapsedMetersMark = _data.Navigation.Data.Meters;
00360                                 _data.Platform.redLedOff();
00361                                 _eventLog.write(DateTime.Now.ToString() 
00362                                         + ": can't see cone, executing search pattern\r\n");
00363                                 //let the platform controller set the speed
00364                                 _data.Navigation.setRunMode(2); //set run mode to two 
00365                                 _data.Navigation.setThrottleSetpoint(minimumSpeed); //set the speed to slow
00366                                 _data.Navigation.updateDataEvent += executeConeSearchPattern;
00367                         }
00368                 }
00369 
00370                 private void touchCone() {
00371                         //assumes the cone has been aquired visually
00372                         if (_data.Platform.Data.BumperLeft || _data.Platform.Data.BumperRight) {
00373                                 _data.Navigation.updateDataEvent -= touchCone;
00374                                 _data.Navigation.setThrottleSetpoint(0);
00375                                 _data.Vision.saveRawImage();
00376                                 _data.Platform.redLedOff();
00377                                 _eventLog.write(DateTime.Now.ToString() + ": touched cone\r\n");
00378                                 _eventLog.write(DateTime.Now.ToString() + ": saving image of touched cone\r\n");
00379                                 _data.Vision.stopCamera();
00380                                 _data.Navigation.setRunMode(2);
00381                                 _data.Navigation.setThrottleSetpoint(maximumSpeed); //set the speed to fast
00382                                 _touchedCone = true; //the robot touched the cone
00383                                 _arrivedAtWaypoint = true;
00384                                 _lookingForCone = false;
00385                                 return;
00386                         }
00387                         if (_data.Vision.Data.ConeAquired) {
00388                                 //turn in porportion to the error that the cone is from the center of the camera 
00389                                 _data.Navigation.setSteeringSetpoint
00390                                         ((int)Math.Round((double)_data.Vision.Data.TurnMagnitude, 0));
00391                                 _viewAttempts = 0;
00392                         } else {
00393                                 ++_viewAttempts;
00394                                 //reject noise, vision system occasionally looses the cone's signature
00395                                 //withought actually loosing sight of it.
00396                                 if (_viewAttempts > 4) _data.Navigation.setThrottleSetpoint(0);
00397                                 if (_viewAttempts > 32) { //lost the cone
00398                                         _data.Navigation.updateDataEvent -= touchCone;
00399                                         _data.Platform.redLedOff();
00400                                         _eventLog.write(DateTime.Now.ToString()
00401                                                 + ": lost sight of cone, resuming looking\r\n");
00402                                         _data.Vision.updateDataEvent += lookForCone;
00403                                 }
00404                         }
00405                 }
00406 
00407 
00408                 double elapsedMetersMark = 0;
00409                 const int MAX_LEFT = 127;
00410 
00411                 private void executeConeSearchPattern() {
00412                         if (_data.Vision.Data.ConeAquired) {
00413                                 _data.Navigation.setThrottleSetpoint(0); //stop the robot
00414                                 _data.Navigation.updateDataEvent -= executeConeSearchPattern;
00415                                 _data.Platform.redLedOn();
00416                                 _eventLog.write(DateTime.Now.ToString() + ": spotted cone in search pattern "
00417                                 + "resuming approach to cone\r\n");
00418                                 _viewAttempts = 0;
00419                                 _data.Vision.updateDataEvent += lookForCone;
00420                         } else {
00421                                 if (_data.Navigation.Data.Meters - elapsedMetersMark > _searchDistance) {
00422                                         _data.Navigation.updateDataEvent -= executeConeSearchPattern;
00423                                         _data.Vision.stopCamera();
00424                                         _eventLog.write(DateTime.Now.ToString() + ": can't see cone, giving up\r\n");
00425                                         //this is a lie, essentially the robot gives up on the cone here.
00426                                         _lookingForCone = false;
00427                                         _arrivedAtWaypoint = true;
00428                                         _data.Navigation.setRunMode(2); //set run mode to two 
00429                                         _data.Navigation.setThrottleSetpoint(maximumSpeed); //set the speed to fast
00430                                 }
00431                                 if (_data.Navigation.Data.Meters - elapsedMetersMark < _searchDistance / 3)
00432                                         steerToCone();
00433                                 else _data.Navigation.setSteeringSetpoint(MAX_LEFT / 2);
00434                         }
00435                 }
00436 
00437                 private void LogData() {
00438                         //write the data out to file
00439                         _dataLog.write(
00440                                 DateTime.Now.ToString()
00441                                 //race strategy data
00442                                 + "\t" + _pointCount.ToString()
00443                                 + "\t" + _currentLocationVector.Point.X.ToString()
00444                                 + "\t" + _currentLocationVector.Point.Y.ToString()
00445                                 + "\t" + _currentLocationGps.Point.X.ToString()
00446                                 + "\t" + _currentLocationGps.Point.Y.ToString()
00447                                 + "\t" + _currentLocation.Point.X.ToString()
00448                                 + "\t" + _currentLocation.Point.Y.ToString()
00449                                 + "\t" + _waypointDistance.ToString()
00450                                 + "\t" + _crossTrackError.ToString()
00451                                 + "\t" + _turnDirection
00452                                 + "\t" + _theta.ToString()
00453                                 + "\t" + _data.Navigation.Data.ThrottleSetpoint.ToString()
00454 
00455                                 //platform controller data
00456                                 + "\t" + _data.Platform.Data.SystemBatteryVolts.ToString()
00457                                 + "\t" + _data.Platform.Data.MicroBatteryVolts.ToString()
00458                                 + "\t" + _data.Platform.Data.InfraredLeftCm.ToString()
00459                                 + "\t" + _data.Platform.Data.InfraredRightCm.ToString()
00460                                 + "\t" + _data.Platform.Data.SonarRangefinderLeftCm.ToString()
00461                                 + "\t" + _data.Platform.Data.SonarRangefinderCenterCm.ToString()
00462                                 + "\t" + _data.Platform.Data.SonarRangefinderRightCm.ToString()
00463                                 + "\t" + _data.Platform.Data.BumperLeft.ToString()
00464                                 + "\t" + _data.Platform.Data.BumperRight.ToString()
00465                                 + "\t" + _data.Platform.Data.CompassHeadingDouble.ToString()
00466                                 + "\t" + _data.Platform.Data.SteeringCalculatedValue.ToString()
00467                                 + "\t" + _data.Platform.Data.ThrottleCalculatedValue.ToString()
00468 
00469                                 //navigation controller data
00470                                 + "\t" + _data.Navigation.Data.XAccGs.ToString()
00471                                 + "\t" + _data.Navigation.Data.YAccGs.ToString()
00472                                 + "\t" + _data.Navigation.Data.ZAccGs.ToString()
00473                                 + "\t" + _data.Navigation.Data.Meters.ToString()
00474                                 + "\t" + _data.Navigation.Data.SteeringSetpoint.ToString()
00475                                 + "\t" + _data.Navigation.Data.ThrottleSetpoint.ToString()
00476                                 + "\t" + _data.Navigation.Data.Steering.ToString()
00477                                 + "\t" + _data.Navigation.Data.Throttle.ToString()
00478 
00479                                 //gps data
00480                                 + "\t" + _data.Gps.Gga.Data.QualityIndicator.ToString()
00481                                 + "\t" + _data.Gps.Gga.Data.SatelitesInView.ToString()
00482                                 + "\t" + _data.Gps.Gga.Data.Latitude.ToString()
00483                                 + "\t" + _data.Gps.Gga.Data.NorthOrSouth.ToString()
00484                                 + "\t" + _data.Gps.Gga.Data.Longitude.ToString()
00485                                 + "\t" + _data.Gps.Gga.Data.EastOrWest.ToString()
00486                                 + "\t" + _data.Gps.Gga.Data.AntennaAltitude.ToString()
00487                                 + "\t" + _data.Gps.Gga.Data.UnitsOfAntennaAltitude.ToString()
00488                                 + "\t" + _data.Gps.Gga.Data.GeoidalSeparation.ToString()
00489                                 + "\t" + _data.Gps.Gga.Data.UnitsOfGeoidalSeperation.ToString()
00490                                 + "\t" + _data.Gps.Gga.Data.HorizontalDilutionOfPrecision.ToString()
00491                                 + "\t" + _data.Gps.Gga.Data.Time.ToString()
00492                                 + "\t" + _data.Gps.Rmc.Data.SpeedOverGround.ToString()
00493                                 + "\t" + _data.Gps.Rmc.Data.TrackMadeGood.ToString()
00494                                 + "\t" + _data.Gps.Rmc.Data.MagneticVariation.ToString()
00495                                 + "\t" + _data.Gps.Rmc.Data.EastOrWestM.ToString() + "\r\n");
00496                 }
00497 
00498                 private void printDataLogHeadder() {
00499                         _dataLog.write(
00500                                 //Race Strategy Data
00501                                 "Date Time"
00502                                 + "\tPoint Count"
00503                                 + "\tCurrent Location Vector X"
00504                                 + "\tCurrent Location Vector Y"
00505                                 + "\tCurrent Location GPS X"
00506                                 + "\tCurrent Location GPS Y"
00507                                 + "\tCurrent Location X"
00508                                 + "\tCurrent Location Y"
00509                                 + "\tWaypoint Distance"
00510                                 + "\tCross Track Error"
00511                                 + "\tTurn Direction"
00512                                 + "\tTheta"
00513                                 + "\tThrottle Setpoint"
00514 
00515                                 //Platform Data
00516                                 + "\tSystem Battery (V)"
00517                                 + "\tPlatform Battery (V)"
00518                                 + "\tInfrared Left(cm)"
00519                                 + "\tInfrared Right (cm)"
00520                                 + "\tSonar Left (cm)"
00521                                 + "\tSonar Center (cm)"
00522                                 + "\tSonar Right (cm)"
00523                                 + "\tBumper Left"
00524                                 + "\tBumper Right"
00525                                 + "\tCompass Heading (Deg)"
00526                                 + "\tSteering Calculated Value"
00527                                 + "\tThrottle Calculated Value"
00528 
00529                                 //Navigation Data
00530                                 + "\tX Axis Acc (G)"
00531                                 + "\tY Axis Acc (G)"
00532                                 + "\tZ Axis Acc (G)"
00533                                 + "\tMeters"
00534                                 + "\tSteering Setpoint"
00535                                 + "\tThrottle Setpoint"
00536                                 + "\tSteering Actual Value"
00537                                 + "\tThrottle Actual Value"
00538 
00539                                 //GPS Data
00540                                 + "\tGPS Quality Indicator"
00541                                 + "\tSatellites in View"
00542                                 + "\tLatitude"
00543                                 + "\tNorth or South"
00544                                 + "\tLongitude"
00545                                 + "\tEast or West"
00546                                 + "\tAntenna Altitude"
00547                                 + "\tUnits of Antenna Altitude"
00548                                 + "\tGeoidal Separation"
00549                                 + "\tUnits of Geoidal Separation"
00550                                 + "\tHorizontal Dilution of Precision"
00551                                 + "\tGPS Time"
00552                                 + "\tSpeed over Ground (knots)"
00553                                 + "\tTrack Made Good"
00554                                 + "\tMagnetic Variation"
00555                                 + "\tEast or West Meters\r\n");
00556                 }
00557     }
00558 }