Diff: main.cpp

Revision:

20:a820531c78bc

Parent:

19:eef0e3ec32a0

Child:

21:99be83550601

--- a/main.cpp	Fri Aug 28 08:04:57 2015 +0000
+++ b/main.cpp	Sat Aug 29 01:09:40 2015 +0000
@@ -1,27 +1,13 @@
-#include "mbed.h"
-#include <string>
-#include <sstream>
-#include <vector>
-#include "Get.h"
-#include "Servo.h"
-#include "SDFileSystem.h"
-using namespace std;
+#include "Config.h"
 
-#define MAX_IMU_SIZE 29
-
-//Indicator LEDs
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-
-//Hardware connections 
+ 
 Serial pc(USBTX, USBRX);
 Serial IMU(p28, p27);  // tx, rx
 Serial GPS(p13, p14);  // tx, rx
-Servo rudderServo(p25);
-Servo wingServo(p26);
-SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs
 
 char IMU_message[256];
 int  IMU_message_counter=0;
@@ -48,13 +34,16 @@
 
 int asterisk_idx;
 
+double Longtitude_Path[MAX_TASK_SIZE];
+double Latitude_Path[MAX_TASK_SIZE];
+
 vector<string> split(const string &s, char delim) {
     stringstream ss(s);
     string item;
     vector<string> tokens;
     while (getline(ss, item, delim)) {
         if (item.empty()) {
-            item == "N/A";
+            item = "N/A";
         }
         tokens.push_back(item);
     }
@@ -111,16 +100,26 @@
     Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longtitude, GPS_Altitude,
     GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph
     Set path to SailBoat
-    @SET=Latitude, Longtitude, Task index
-    @SET=33.776318, -84.407590, 3
+    @SET=PATH, Latitude, Longtitude, Task id
+    @SET=PATH, 33.776318, -84.407590, 3 
 */
 void decodePC(string PC_data) {
     string PC_data_string(PC_data);
     if (PC_data_string.substr(0,4) == "@GET") {
         pc.printf("%s", PC_data_string.c_str());
         PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
-        pc.printf("%s\n", decodeCommand(PC_data_string).c_str());
+        pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str());
     } else if (PC_data_string.substr(0,4) == "@SET") {
+        pc.printf("%s", PC_data_string.c_str());
+        PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
+        string claim = decodeCommandSET(PC_data_string);
+        if (claim == "DONE") {
+            pc.printf("Current Path: Longtitude, Latitude\n");
+            for (int i=0;i<MAX_TASK_SIZE;++i) {
+                pc.printf("              %f, %f\n", Longtitude_Path[i], Latitude_Path[i]);
+            }
+        }
+        pc.printf("%s\n", claim.c_str());
     }
 }
 
@@ -204,46 +203,6 @@
     
     led4= !led4;
 }
-
-void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
-    /*angleDeg:  desired angle
-      minDeg:    minimum angle in degrees
-      minNorVal: normalized value[0,1] when servo is at its minimum angle
-      maxDeg:    maximum angle in degrees
-      maxNorVal: normalized value[0,1] when servo is at its maximum angle
-    */
-    
-    float pos; //normalized angle, [0,1]
-    float scale; //scale factor for servo calibration
-    
-    //extreme conditions
-    if (angleDeg<minDeg){
-        pos=minNorVal;
-    }   
-    if (angleDeg>maxDeg){
-        pos=maxNorVal;
-    }
-    
-    //Calculate scale factor for servo calibration
-    scale = (angleDeg-minDeg)/(maxDeg-minDeg);
-    //Calculate servo normalized value
-    pos = minNorVal + scale*(maxNorVal-minNorVal);
-    
-    //send signal to servo
-    targetServo=pos;
-}
-
-int log_data_SD(){   
-    FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
-    if(fp == NULL) {
-        return 0;
-    }else{
-        //Write all the useful data to the SD card        
-        fprintf(fp, "Nya Pass~");
-        fclose(fp); 
-        return 1;
-    }
-}
  
 int main() {
     IMU.baud(57600);
@@ -253,6 +212,7 @@
     pc.baud(115200);
     pc.attach(&PC_serial_ISR);
     
+    
     while (1) {
         led1 = !led1;
         wait(0.4);