   moistureSensor.enable();
   rudderServo.enable();
   temperatureSensor.enable();
-  proximitySensorArray.enable();
+  //proximitySensorArray.enable();
   
   PIDController.enableCruisingMode();
-  
-  currentGpsData = gps.getData();
+
   while(currentGpsData.latitude < 4500 && currentGpsData.longitude < 7000){
     currentGpsData = gps.getData();
   }
+  Serial.print("GPS data lat & lon: ");
+  Serial.print(currentGpsData.latitude);
+  Serial.print(" & ");
+  Serial.println(currentGpsData.longitude);
+  /*
   startPoint[0][0] = ((currentGpsData.latitude-4500)/60)+45;
   startPoint[0][1] = ((currentGpsData.longitude-7100)/60)+71;
+  */
+  startPoint[0][0] = 45.39554;
+  startPoint[0][1] = 71.9274;
   trajectory.getData(startPoint,destinationPoint,listPointsX,listPointsY);
+  for (int i = 1; i < 10; i++){
+    Serial.println(listPointsX[i], 4); 
+    Serial.println(listPointsY[i], 4);
+  }
   
   nextGpsData.longitude = listPointsY[2];
   nextGpsData.latitude = listPointsX[2];
   
-  delay(TEST_DELAY);
+  delay(START_DELAY);
+
   timer1 = millis();
   timer2 = millis();
-
+ 
   moisture = moistureSensor.getData();
   temperature = temperatureSensor.getData();
   rudderServo.setAngle(0);
-
+ 
   //currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
   
 #endif /* __BOAT__ */
- /********************************************/
 }
 
 void loop() {
   
   currentGpsData = gps.getData();
- 
-
-#if __BOAT__>0
-
-  xbee.receiveData(); //Update les valeurs du servo, esc et GPS de la sation fixe
-  mode = xbee.getMode();  //Retourne le mode de communication de la station fixe (Manuel,GPS)
-  Serial.print("xbee: ");
-  Serial.print(mode);
-  if(mode == Manuel){
-    servoValue = xbee.getServoValue();
-    escValue = xbee.getEscValue();
-    rudderServo.setAngle(servoValue);
+
+#if __BOAT__>0
+
+  xbee.receiveData();
+  mode = xbee.getMode();
+  if(mode == Manuel)
+  {
+    servoValue = (int)xbee.getServoValue();
+    escValue = (int)xbee.getEscValue();
+    Serial.println(servoValue);
+    Serial.println(escValue);
+    rudderServo.setPosition(servoValue);
     esc.setSpeed(escValue, 0, 100);  
   }
-  else if(mode == GPS){
-    gpsValue = xbee.getGPSValue();
+  else if(mode == GPS)
+  {
+    gpsValueLat = xbee.getGpsLatitude();
+    gpsValueLon = xbee.getGpsLongitude();
     esc.setSpeed(MOTOR_SPEED, 0, 100);
   }
-  /*currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
+  /*
+  currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
   while (currentEmergencyState != 0) {
     emergencyHandler.handleEmergency(currentEmergencyState);
     Serial.println("In emergency state loop.");
     currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
   }
-  */
-  if (getDistanceBetweenGPSPoints(currentGpsData, nextGpsData) < PIDController.InertialDerivationDistance && gpsListCounter < listPointsX[0]+1){ 
-    gpsListCounter = gpsListCounter + 1;
-    nextGpsData.longitude = listPointsY[gpsListCounter];
-    nextGpsData.latitude = listPointsX[gpsListCounter];
-    esc.setSpeed(0, 0, 100);
-    delay(STATIONNARY_DELAY);
-  }
-  else if (getDistanceBetweenGPSPoints(currentGpsData, nextGpsData) < PIDController.InertialDerivationDistance && gpsListCounter == listPointsX[0]+1){ 
-    esc.setSpeed(0, 0, 100);
-    delay(STATIONNARY_DELAY);
-  }
-  else{
-    esc.setSpeed(MOTOR_SPEED, 0, 100);
-  }
-  
-  if (timer1 > millis())  timer1 = millis();
-  if (timer2 > millis())  timer2 = millis();
-  
-  if (millis() - timer1 > TS*SECONDS) {
+ */
+ if (timer1 > millis())  timer1 = millis();
+ if (timer2 > millis())  timer2 = millis();
     
-    timer1 = millis();
-    heading = gyroscope.getData(LSM_303);
-    theta = PIDController.compute_theta(currentGpsData, nextGpsData, heading);
-    rudder = PIDController.control_rudder(theta);
-    rudderServo.setAngle(rudder);
-    Serial.println(getDistanceBetweenGPSPoints(currentGpsData, nextGpsData));
-  }
-  
-  if (millis() - timer2 > 5000) {
-   
-      timer2 = millis();
-      moisture = moistureSensor.getData();
-      temperature = temperatureSensor.getData();
+ if (currentGpsData.latitude > 4000 && currentGpsData.latitude < 5000 && currentGpsData.longitude > 6000 && currentGpsData.longitude < 8000 && mode != Manuel) 
+ {
+    if (getDistanceBetweenGPSPoints(currentGpsData, nextGpsData) < PIDController.InertialDerivationDistance && gpsListCounter < listPointsX[0]+1)
+    {
+      gpsListCounter = gpsListCounter + 1;
+      nextGpsData.longitude = listPointsY[gpsListCounter];
+      nextGpsData.latitude = listPointsX[gpsListCounter];
+      esc.setSpeed(0, 0, 100);
+      delay(STATIONNARY_DELAY);
+    }
+    else if (getDistanceBetweenGPSPoints(currentGpsData, nextGpsData) < PIDController.InertialDerivationDistance && gpsListCounter == listPointsX[0]+1)
+    { 
+      esc.setSpeed(0, 0, 100);
+    }
+    else
+    {
+      esc.setSpeed(MOTOR_SPEED, 0, 100);
+    }
+    
+    if (millis() - timer1 > TS*SECONDS)
+    {
+      timer1 = millis();
       
-      moistureSensor.printData(moisture);
-      temperatureSensor.printData(temperature);
-  }
+      heading = gyroscope.getData(LSM_303);
+      theta = PIDController.computeTheta(currentGpsData, nextGpsData, heading);
+      gyroscope.printData(heading);
+      Serial.print("Theta: ");
+      Serial.println(theta);    
+      rudder = PIDController.controlRudder(theta);
+      rudderServo.setAngle(rudder);
+    }
+ }
+  
+ if (millis() - timer2 > RUN_TIME) 
+ {
+    esc.setSpeed(0,0,100);
+    while(true)
+    {
+      servoValue = (int)xbee.getServoValue();
+      escValue = (int)xbee.getEscValue();
+      servoValue = servoValue > MOTOR_SPEED ? MOTOR_SPEED : servoValue;
+      rudderServo.setPosition(servoValue);
+      esc.setSpeed(escValue, 0, 100);
+    }
+ }
 #endif /* __BOAT__ */
 }

   */
  gps.enable();
  xbee.begin();

  delay(2*SECONDS);
  
#if __BOAT__>0


  double rudder = 0.0;
  int servoValue = 0;
  int escValue = 0;
  double gpsValueLat = 0.0;
  double gpsValueLon = 0;
  unsigned long timer1;
  unsigned long timer2;
  double distanceToTarget = 0.00;
  //double destinationPoint[1][2] = { 45.379184, 71.9239 }; //test campus
  double destinationPoint[1][2] = {45.39545,71.92775};
  double startPoint[1][2];
  int gpsListCounter = 2;
  double listPointsX[10] = {0};
  double listPointsY[10] = {0};
#endif /* __BOAT__ */

  const char GPS = 'G';
  const char Manuel = 'M';
  char mode = 'E';
  NMEAData currentGpsData;
/********************************************/


