Diff: main.cpp

Revision:

26:353a80179346

Parent:

24:8e38cc14150c

Child:

27:1be1f25be449

--- a/main.cpp	Thu Sep 10 17:40:48 2015 +0000
+++ b/main.cpp	Thu Sep 10 18:15:16 2015 +0000
@@ -11,6 +11,7 @@
 Servo rudderServo(p25);
 Servo wingServo(p26);
 SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs
+Ticker ctrl_updt_timer; //timer to refresh controller
 
 char IMU_message[256];
 int  IMU_message_counter=0;
@@ -62,6 +63,12 @@
 double Longtitude_Path[MAX_TASK_SIZE];
 double Latitude_Path[MAX_TASK_SIZE];
 
+double angle_to_path_point,distance_to_path_point,desired_speed;
+double rudder_ctrl_parameters[3];
+double rudder_variables[5];//,,,prev,out
+double T=0.2; //controller update period=0.2sec, 5Hz
+
+
 vector<string> split(const string &s, char delim) {
     stringstream ss(s);
     string item;
@@ -276,16 +283,6 @@
 }
 
 
-
-void update_controller() {
-    /*Input:  angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角   
-              distance(meter) to the next path point
-      
-      Function: drive two servos to navigate the sailboat to the desired path point
-    */
-    
-}
-
 void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
     /*angleDeg:  desired angle
       minDeg:    minimum angle in degrees
@@ -314,6 +311,39 @@
     targetServo=pos;
 }
 
+
+void initialize_controller(){
+    rudder_variables[0]=0;
+    rudder_variables[1]=0;
+    rudder_variables[2]=0;
+    rudder_variables[3]=0;
+    rudder_variables[4]=0;
+    rudder_ctrl_parameters[0]=1;
+    rudder_ctrl_parameters[1]=0;
+    rudder_ctrl_parameters[2]=0;
+}
+
+void update_controller_tmr_ISR() {
+    /*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;
+      
+      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];
+    
+    
+    //Drive servos
+    set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);
+}
+
+
+
 int log_data_SD(){   
     FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
     if(fp == NULL) {
@@ -334,6 +364,9 @@
     GPS.attach(&GPS_serial_ISR);
     pc.baud(115200);
     pc.attach(&PC_serial_ISR);
+    ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz
+   
+    initialize_controller();
     
     //float angle=20;    
     while (1) {
@@ -341,6 +374,8 @@
     //    set_servo_position(rudderServo, angle, 0, 0, 180, 1);
     //    set_servo_position(wingServo, angle, 0, 0, 180, 1);        
     //    angle=angle+10;
+    
+    
         wait(0.4);
         printStateIMU();
         printStateGPS();