   uint8_t count = 0;
   double theta = 0.0;
   double rudder = 0.0;
+  int servoValue = 0;
+  int escValue = 0;
+  int gpsValue = 0;
   uint32_t timer1;
   uint32_t timer2;
   double distanceToTarget = 0.00;
+  double destinationPoint[1][2] = { 45.379184, 71.9239 }; //test campus
+  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;
 /********************************************/
 
 
 
 void setup() {
-
+  Serial.begin(115200);
   /*
    * Sensor initialization
    */
   gps.enable();
-  
-#if __BOAT__>0
-  Serial.begin(115200);
+  xbee.begin();
+
+  delay(2*SECONDS);
+  
+#if __BOAT__>0
+
   gyroscope.enable();
   moistureSensor.enable();
   rudderServo.enable();
   temperatureSensor.enable();
   proximitySensorArray.enable();
-  /*  Uncomment when xbee is integrated
-  basecampGPSData = xbee.getGPSDataFromBasecamp();
-  gpsError = getGPSError(basecamp GPSData);
-  */
-  lcd.begin(16, 2);
-  lcd.clear();
-  lcd.setCursor(0, 0);
-  lcd.print("Oktopus");
-  lcd.setCursor(0, 1);
-  lcd.print("autopilot");
+  
+  PIDController.enableCruisingMode();
+  
+  currentGpsData = gps.getData();
+  while(currentGpsData.latitude < 4500 && currentGpsData.longitude < 7000){
+    currentGpsData = gps.getData();
+  }
+  startPoint[0][0] = ((currentGpsData.latitude-4500)/60)+45;
+  startPoint[0][1] = ((currentGpsData.longitude-7100)/60)+71;
+  trajectory.getData(startPoint,destinationPoint,listPointsX,listPointsY);
+  
+  nextGpsData.longitude = listPointsY[2];
+  nextGpsData.latitude = listPointsX[2];
+  
   delay(TEST_DELAY);
   timer1 = millis();
   timer2 = millis();
 
   moisture = moistureSensor.getData();
   temperature = temperatureSensor.getData();
-
-  currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
+  rudderServo.setAngle(0);
+
+  //currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
   
 #endif /* __BOAT__ */
  /********************************************/
 }
 
 void loop() {
+  
   currentGpsData = gps.getData();
-
-#if __BOAT__>0
-  
-  currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
+ 
+
+#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);
+    esc.setSpeed(escValue, 0, 100);  
+  }
+  else if(mode == GPS){
+    gpsValue = xbee.getGPSValue();
+    esc.setSpeed(MOTOR_SPEED, 0, 100);
+  }
+  /*currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
   while (currentEmergencyState != 0) {
     emergencyHandler.handleEmergency(currentEmergencyState);
     Serial.println("In emergency state loop.");
-    lcd.clear();
-    lcd.setCursor(0, 0);
-    lcd.print("In emergency state loop.");
     currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
   }
-  
-  esc.setSpeed(MOTOR_SPEED, 0, 100);
-
-  nextGpsData.longitude = currentGpsData.longitude + 5;
-  nextGpsData.latitude = currentGpsData.latitude + 5;
-  /*  Uncomment when xbee is integrated
-  basecampGPSData = xbee.getGPSDataFromBasecamp();
-  gpsError = getGPSError(basecamp GPSData);
   */
+  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();
   

#include "ProximitySensor.h"
#include "RudderServo.h"
#include "Temperature.h"
#include <LiquidCrystal.h>
#include "EmergencyHandler.h"

/*
 * Defines

 */
#if __BOAT__>0
  PIDController PIDController;
  ESC esc(1500, 2000, 12);
  Gyroscope gyroscope;
  Moisture moistureSensor;
  ProximitySensor proximitySensorArray;
  RudderServo rudderServo;
  Temperature temperatureSensor;
  Trajectory trajectory;
  EmergencyHandler emergencyHandler("Dry", TEMPERATURE_THRESHOLD, SONAR_THRESHOLD, PROXIMITY_THRESHOLD);
#endif

GPS gps;
XbeeReceiver xbee;
/********************************************/ 


