Diff: main.cpp

Revision:

40:9537722d185e

Parent:

39:ef1a8055d744

Child:

41:d207b407c8bc

--- a/main.cpp	Thu Oct 29 00:42:07 2015 +0000
+++ b/main.cpp	Fri Oct 30 23:43:13 2015 +0000
@@ -65,6 +65,7 @@
 double dis[MAX_TASK_SIZE];
 double ang[MAX_TASK_SIZE];
 
+int autonomous_mode=0;
 double angle_to_path_point,distance_to_path_point,desired_speed;
 double rudder_ctrl_parameters[3];
 double rudder_variables[5];//,,,prev,out
@@ -402,9 +403,9 @@
         //Our sailboat is a slow moving vehicle and GPS cannot provide 
         //very accurate speed reading in our application
     if (distance_to_path_point>30){
-        set_servo_position(wingServo,45,-45,0,45,1);
+        if (autonomous_mode){set_servo_position(wingServo,45,-45,0,45,1);}
     }else{
-        set_servo_position(wingServo,0,-45,0,45,1);
+        if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);}
     }
     
     //actuator saturation
@@ -412,7 +413,7 @@
     if (rudder_variables[4]<-60){rudder_variables[4]=-60;}
     
     //Drive servos
-    set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);
+    if (autonomous_mode){set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);}
 }
 
 
@@ -442,13 +443,23 @@
     initialize();
     //float angle=20;    
     while (1) {
+        
+        //Enmao, please put your function below:
+        
+        angle_to_path_point=0;
+        distance_to_path_point=0;
+        
+        autonomous_mode=0; //set 1 if in autonomous mode
+                           
+        
+        
     //    if (angle>160){angle=20;}               
     //    set_servo_position(rudderServo, angle, 0, 0, 180, 1);
     //    set_servo_position(wingServo, angle, 0, 0, 180, 1);        
     //    angle=angle+10;
      //   pc.printf("Hello World!\n");
 
-        wait(0.4);
+        wait(0.2);
         //printStateIMU();
         //printStateGPS();
         //printPath();