Diff: main.cpp

Revision:

31:e3339036c98b

Parent:

30:faf6e0f81a0c

Child:

32:263d39ea6d3e

--- a/main.cpp	Sun Sep 27 00:52:09 2015 +0000
+++ b/main.cpp	Sun Sep 27 00:58:18 2015 +0000
@@ -341,6 +341,8 @@
       
       Function: drive two servos to navigate the sailboat to the desired path point
     */
+    
+    //CTE based controller for rudder
     if (angle_to_path_point<0){distance_to_route=-1*distance_to_route;}
     
     rudder_variables[0]=rudder_ctrl_parameters[0]*distance_to_route;
@@ -349,6 +351,18 @@
     rudder_variables[3]=distance_to_route;
     rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2];
     
+    //bang-bang controller for vehicle velocity
+        //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);
+    }else{
+        set_servo_position(wingServo,0,-45,0,45,1);
+    }
+    
+    //actuator saturation
+    if (rudder_variables[4]> 60){rudder_variables[4]= 60;}
+    if (rudder_variables[4]<-60){rudder_variables[4]=-60;}
     
     //Drive servos
     set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);