Diff: main.cpp

Revision:

42:e1a2a6daf70b

Parent:

41:d207b407c8bc

Child:

43:a430d5e4f33d

--- a/main.cpp	Sun Nov 01 01:43:23 2015 +0000
+++ b/main.cpp	Sun Nov 01 22:20:08 2015 +0000
@@ -5,8 +5,8 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-//Serial pc(p9, p10);
-Serial pc(USBTX, USBRX);
+Serial pc(p9, p10);
+//Serial pc(USBTX, USBRX);
 Serial IMU(p28, p27);  // tx, rx
 Serial GPS(p13, p14);  // tx, rx
 Servo rudderServo(p25);
@@ -422,6 +422,51 @@
 
 
 
+
+void update_controller_tmr_ISR2() {
+    /*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,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;
+    rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2];
+    
+    /*if (distance_to_route>0){
+        rudder_variables[4]=rudder_variables[4]+10;
+    }else{
+        rudder_variables[4]=rudder_variables[4]-10;
+    }*/
+    
+    
+    //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){
+        if (autonomous_mode){set_servo_position(wingServo,45,-45,0,45,1);}
+    }else{
+        if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);}
+    }
+    
+    //actuator saturation
+    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]);
+}
+
+
+
+
+
 int log_data_SD(){   
     FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
     if(fp == NULL) {
@@ -442,7 +487,8 @@
     GPS.attach(&GPS_serial_ISR);
     pc.baud(9600);
     pc.attach(&PC_serial_ISR);
-    ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz
+    //ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz
+    ctrl_updt_timer.attach(&update_controller_tmr_ISR2, T); // Update controller at 1/T Hz
     initialize_controller();
     initialize();
     //float angle=20;    
@@ -450,9 +496,9 @@
         
         //Enmao, please put your function below:
         
-        angle_to_path_point=0;
-        distance_to_path_point=0;
-        distance_to_route=0;
+        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
         
         autonomous_mode=0; //set 1 if in autonomous mode