     }
     
   }
+ 
+ void forward(){
+ 
+ }
+ 
+ void left(){
+   
+ }
+ 
+ void right(){
+   
+ }
  
  float formatDistance(int meters){
   return meters / (float) 1000;

  }

  void executeCommand(int cmd){
    switch(cmd){
       case CMD_SHOW_POSITION:
         //Display current position

         lcd.print(compassHeading);
         lcd.setCursor(0,1);
         lcd.print("G:");
         heading = getHeading(mCurrentWayPoint, flat, flon);
         lcd.print(heading);
         lcd.print(" ");
         lcd.print("D:");
         distance = getDistance(mCurrentWayPoint, flat, flon);
         lcd.print(formatDistance(distance));
         //Check if distance is less then 2 meters
         if(distance < 2){
           //Advance to next way point
           mCurrentWayPoint++;
         } else {
           //Navigate to the way point
           if(compassHeading == heading){
              forward(); 
           } else if (compassHeading > heading) {
              left();
           } else {
              right(); 
           }
         }
       break;
       case CMD_ADD_WAY_POINT:
         mLat[mCurrentWayPoint] = flat;
