Diff: main.cpp

Revision:

32:263d39ea6d3e

Parent:

31:e3339036c98b

Child:

33:37345814fad0

--- a/main.cpp	Sun Sep 27 00:58:18 2015 +0000
+++ b/main.cpp	Sun Sep 27 09:26:00 2015 +0000
@@ -222,6 +222,29 @@
           
 }
 
+void printPath() {
+    pc.printf("Current Path: Longitude, Latitude\n");
+    for (int i=0;i<MAX_TASK_SIZE;++i) {
+        pc.printf("              %f, %f\n", Longitude_Path[i], Latitude_Path[i]);
+    }
+}
+
+void printDistance() {
+    double[MAX_TASK_SIZE] dis = fill_n(array, MAX_TASK_SIZE, -1);;
+    for(int i=0;i<MAX_TASK_SIZE;++i) {       
+        dis[i] = getDistance(i);
+        pc.printf("Distance Task %d: %f\n", i+1, dis[i]);
+    }   
+}
+
+void printAngle() {
+    double[MAX_TASK_SIZE] ang = fill_n(array, MAX_TASK_SIZE, -361);;
+    for(int i=0;i<MAX_TASK_SIZE;++i) {       
+        ang[i] = getAngle(i);
+        pc.printf("Angle Task %d: %f\n", i+1, ang[i]);
+    }   
+}
+
 //#YPR=-183.74,-134.27,-114.39
 void IMU_serial_ISR() {
     char buf;
@@ -403,8 +426,11 @@
     
     
         wait(0.4);
-        printStateIMU();
-        printStateGPS();
+        //printStateIMU();
+        //printStateGPS();
+        //printPath();
+        printDistance();
+        printAngle();
         led1 = !led1;
     }
 }
\ No newline at end of file