Diff: main.cpp

Revision:

30:faf6e0f81a0c

Parent:

29:95b0320bf779

Child:

31:e3339036c98b

--- a/main.cpp	Sat Sep 19 18:40:43 2015 +0000
+++ b/main.cpp	Sun Sep 27 00:52:09 2015 +0000
@@ -66,6 +66,7 @@
 double angle_to_path_point,distance_to_path_point,desired_speed;
 double rudder_ctrl_parameters[3];
 double rudder_variables[5];//,,,prev,out
+double distance_to_route;
 double T=0.2; //controller update period=0.2sec, 5Hz
 
 
@@ -333,16 +334,19 @@
 
 void update_controller_tmr_ISR() {
     /*Input:  angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角   
-              distance(meter) to the next path point
-      Global Variables: angle_to_path_point,distance_to_path_point;
+              angle to path point
+              distance_to_route(meter)
+              
+      Global Variables: angle_to_path_point,distance_to_path_point,distance_to_route;
       
       Function: drive two servos to navigate the sailboat to the desired path point
     */
-
-    rudder_variables[0]=rudder_ctrl_parameters[0]*angle_to_path_point;
-    rudder_variables[1]=rudder_variables[1]+rudder_ctrl_parameters[1]*angle_to_path_point*T;
-    rudder_variables[2]=(rudder_variables[3]-angle_to_path_point)/T;
-    rudder_variables[3]=angle_to_path_point;
+    if (angle_to_path_point<0){distance_to_route=-1*distance_to_route;}
+    
+    rudder_variables[0]=rudder_ctrl_parameters[0]*distance_to_route;
+    rudder_variables[1]=rudder_variables[1]+rudder_ctrl_parameters[1]*distance_to_route*T;
+    rudder_variables[2]=(rudder_variables[3]-distance_to_route)/T;
+    rudder_variables[3]=distance_to_route;
     rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2];