Diff: main.cpp

Revision:

41:d207b407c8bc

Parent:

40:9537722d185e

Child:

42:e1a2a6daf70b

--- a/main.cpp	Fri Oct 30 23:43:13 2015 +0000
+++ b/main.cpp	Sun Nov 01 01:43:23 2015 +0000
@@ -5,7 +5,8 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-Serial pc(p9, p10);
+//Serial pc(p9, p10);
+Serial pc(USBTX, USBRX);
 Serial IMU(p28, p27);  // tx, rx
 Serial GPS(p13, p14);  // tx, rx
 Servo rudderServo(p25);
@@ -66,11 +67,10 @@
 double ang[MAX_TASK_SIZE];
 
 int autonomous_mode=0;
-double angle_to_path_point,distance_to_path_point,desired_speed;
+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
-double distance_to_route;
-double T=0.2; //controller update period=0.2sec, 5Hz
+double T=0.5; //controller update period=0.5sec, 2Hz
 
 
 vector<string> split(const string &s, char delim) {
@@ -390,6 +390,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;}
     
@@ -409,11 +411,13 @@
     }
     
     //actuator saturation
-    if (rudder_variables[4]> 60){rudder_variables[4]= 60;}
-    if (rudder_variables[4]<-60){rudder_variables[4]=-60;}
+    if (rudder_variables[4]> 45){rudder_variables[4]= 45;}
+    if (rudder_variables[4]<-45){rudder_variables[4]=-45;}
     
     //Drive servos
     if (autonomous_mode){set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);}
+    
+    pc.printf("%f\n",rudder_variables[4]);
 }
 
 
@@ -448,6 +452,7 @@
         
         angle_to_path_point=0;
         distance_to_path_point=0;
+        distance_to_route=0;
         
         autonomous_mode=0; //set 1 if in autonomous mode
                            
@@ -457,7 +462,6 @@
     //    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.2);
         //printStateIMU();