Revision 46:305112d73c69, committed 2015-11-04

Comitter:

dem123456789

Date:

Wed Nov 04 21:45:34 2015 +0000

Parent:

45:dfa689f159ce

Child:

47:4b490931e75f

Commit message:

path complete and update finish

Changed in this revision

--- a/Config.h	Tue Nov 03 16:31:44 2015 +0000
+++ b/Config.h	Wed Nov 04 21:45:34 2015 +0000
@@ -53,6 +53,7 @@
 extern double Longitude_Path[MAX_TASK_SIZE];
 extern double Latitude_Path[MAX_TASK_SIZE];
 extern int IF_Path_Complete[MAX_TASK_SIZE];
+extern int current_task;
 extern Serial pc;
 extern Servo rudderServo;
 extern Servo wingServo;
@@ -61,6 +62,7 @@
 string decodeCommandGET(string cmd);
 string decodeCommandSET(string cmd);
 vector<string> split(const string &s, char delim);
+int get_current_task();
 double getDistance(int task_id);
 double getAngle(int task_id);
 double getCTE(int task_id);

--- a/Get.cpp	Tue Nov 03 16:31:44 2015 +0000
+++ b/Get.cpp	Wed Nov 04 21:45:34 2015 +0000
@@ -49,7 +49,14 @@
     }
 }
 
-
+int get_current_task() {
+    for(int i=0;i<MAX_TASK_SIZE;i++){
+        if(IF_Path_Complete[i]==0) {
+            return i+1;
+        }
+    }
+    return 1;
+}
 
 double Deg2Rad(double degree) {
     return degree*DEG2RAD_RATIO;

--- 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