Diff: main.cpp

Revision:

43:a430d5e4f33d

Parent:

42:e1a2a6daf70b

Child:

44:4d90645d15ec

--- a/main.cpp	Sun Nov 01 22:20:08 2015 +0000
+++ b/main.cpp	Tue Nov 03 15:54:29 2015 +0000
@@ -67,6 +67,7 @@
 double ang[MAX_TASK_SIZE];
 
 int autonomous_mode=0;
+int arrived_destination=0;
 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
@@ -449,8 +450,10 @@
         //very accurate speed reading in our application
     if (distance_to_path_point>30){
         if (autonomous_mode){set_servo_position(wingServo,45,-45,0,45,1);}
+        arrived_destination=0;
     }else{
         if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);}
+        arrived_destination=1;
     }
     
     //actuator saturation
@@ -496,11 +499,12 @@
         
         //Enmao, please put your function below:
         
-        angle_to_path_point=0;  //positive if pathpoint is on the right of the boat
-        distance_to_path_point=0; //positive if trace is on the right of the boat
-        distance_to_route=0;  //no sign
+        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
         
-        autonomous_mode=0; //set 1 if in autonomous mode
+        arrived_destination=0; //will be 1 if arrived destination
+        autonomous_mode=0; //set 1 if in autonomous mode, set 0 bypass the controller
                            
         
         
@@ -509,7 +513,7 @@
     //    set_servo_position(wingServo, angle, 0, 0, 180, 1);        
     //    angle=angle+10;
 
-        wait(0.2);
+        wait(2);
         //printStateIMU();
         //printStateGPS();
         //printPath();