Diff: main.cpp

Revision:

46:305112d73c69

Parent:

44:4d90645d15ec

Child:

47:4b490931e75f

--- a/main.cpp	Tue Nov 03 16:31:44 2015 +0000
+++ b/main.cpp	Wed Nov 04 21:45:34 2015 +0000
@@ -60,14 +60,17 @@
 double D_GPS_VelocityKph=0;
 double D_temp=0;
 int asterisk_idx;
+int current_task=0;
 
 double Longitude_Path[MAX_TASK_SIZE];
 double Latitude_Path[MAX_TASK_SIZE];
+int IF_Path_Complete[MAX_TASK_SIZE];
 double dis[MAX_TASK_SIZE];
 double ang[MAX_TASK_SIZE];
 
 int autonomous_mode=0;
-int arrived_destination=0;
+int arrived_destination=0; // fot test purpose
+
 double angle_to_path_point,distance_to_path_point,desired_speed,distance_to_route;
 double rudder_ctrl_parameters[3];
 double rudder_variables[5];//,,,prev,out
@@ -99,8 +102,10 @@
 void initialize() {
     fill(Longitude_Path, Longitude_Path+MAX_TASK_SIZE, 181);
     fill(Latitude_Path, Latitude_Path+MAX_TASK_SIZE, 181);
+    fill(IF_Path_Complete, IF_Path_Complete+MAX_TASK_SIZE, 0);
     fill(dis, dis+MAX_TASK_SIZE, -1);
     fill(ang, ang+MAX_TASK_SIZE, -361);
+    current_task = 1;
 }
 
 void updateIMU(string IMU_data) {
@@ -381,6 +386,7 @@
     rudder_ctrl_parameters[2]=0;
 }
 
+
 void update_controller_tmr_ISR() {
     /*Input:  angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角   
               angle to path point
@@ -454,6 +460,7 @@
     }else{
         if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);}
         arrived_destination=1;
+        IF_Path_Complete[current_task]=1;
     }
     
     //actuator saturation
@@ -496,10 +503,10 @@
     initialize();
     //float angle=20;    
     while (1) {
-        
-        angle_to_path_point=getAngle(1);  //positive if pathpoint is on the right of the boat
-        distance_to_path_point=getCTE(1); //positive if trace is on the right of the boat
-        distance_to_route=getDistance(1);  //no sign
+        current_task = get_current_task();
+        angle_to_path_point=getAngle(current_task);  //positive if pathpoint is on the right of the boat
+        distance_to_path_point=getCTE(current_task); //positive if trace is on the right of the boat
+        distance_to_route=getDistance(current_task);  //no sign
         
         arrived_destination=0; //will be 1 if arrived destination
         autonomous_mode=0; //set 1 if in autonomous mode, set 0 bypass the controller