Revision 4:b2eaf0cf5063, committed 2016-11-19

Comitter:

Soyoon

Date:

Sat Nov 19 15:03:35 2016 +0000

Parent:

3:e3e965924dde

Commit message:

one directional yaw control

Changed in this revision

diff -r e3e965924dde -r b2eaf0cf5063 main.cpp
--- a/main.cpp	Tue Aug 02 14:38:15 2016 +0000
+++ b/main.cpp	Sat Nov 19 15:03:35 2016 +0000
@@ -3,73 +3,52 @@
 #include "LocalFileSystem.h"
 #include "math.h"
 #include "Servo.h"
+#define dt 0.1
 
+Serial pc(USBTX, USBRX);
 Ticker blue_trig;
 Timer end;
-int stat=1;
 
-////////////////////////////////////
-//         GPS         5V p27(RX) //  
-//      Bluetooth    3.3V p28(TX) //
-////////////////////////////////////
-Serial Blue_GPS(p28, p27);
-char GPS_msg[150], ns, ew;
-int i = 0, j=0, k=0, gps_ok=0, GPS_flag;
-volatile unsigned char GPS_buffer[2];
-float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
-int blue_ok=0;
-
-void GPS_isr(){
-    i++;
-    GPS_buffer[1] = GPS_buffer[0];
-    GPS_buffer[0] = Blue_GPS.getc();
-    if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){   
-        i=0;
-        if (GPS_flag == 1){GPS_flag = 0;gps_ok = 1;j=0;}
-    }
-    if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
-    if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
-}
+////////////////////////////////////////
+//   Bluetooth    3.3V p13 p14(TX,RX) //
+////////////////////////////////////////
+Serial Blue(p13, p14);
+int send_ok=0;
+char Blue_msg[150];
+int k=0, Blue_ok=0, Blue_flag=0;
+volatile unsigned char Blue_buffer[2];
+float input_roll, input_pitch, input_yaw, input_thr;
 
-float trunc(float v) {
-    if(v < 0.0) {
-        v*= -1.0;
-        v = floor(v);
-        v*=-1.0;
+void Blue_isr(){                      //inturupt 
+    while(Blue.readable()){
+        Blue_buffer[1] = Blue_buffer[0];
+        Blue_buffer[0] = Blue.getc();
+        if (Blue_buffer[0] == '\n' && Blue_flag == 1){Blue_flag = 0; Blue_ok = 1; k=0;}
+        if (Blue_buffer[0] == '*'){Blue_flag=1;}
+        if (Blue_flag==1){Blue_msg[k] = Blue_buffer[0]; k++;}
     }
-    else {v = floor(v);}
-    return v;
-}
+} 
 
-void get_GPS(float *Kor_time, float *latitude, char *ns, float *longitude, char *ew, float *fix, float *sat, float *x, float *alt_GPS, float *lock)
+void get_Blue(float*input_roll, float*input_pitch, float*input_yaw, float*input_thr)
 {
-    if (gps_ok == 1){
-        gps_ok = 0;
-        sscanf(GPS_msg, "GA,%f,%f,%c,%f,%c,%f,%f,%f,%f,%f", Kor_time,latitude,ns,longitude,ew,fix,sat,x,alt_GPS,lock);
-        if(*ns == 'S') {*latitude  *= -1.0; }
-        if(*ew == 'W') {*longitude *= -1.0; }
-        float degrees = trunc(*latitude / 100.0f);
-        float minutes = (*latitude - (degrees * 100.0f));
-        *latitude = degrees + minutes / 60.0f;    
-        degrees = trunc(*longitude / 100.0f);
-        minutes = *longitude - (degrees * 100.0f);
-        *longitude = degrees + minutes / 60.0f;
-        *Kor_time = *Kor_time + 90000;
+    if (Blue_ok == 1){
+        Blue_ok = 0;
+        sscanf(Blue_msg, "*%f,%f,%f,%f\n", input_roll, input_pitch, input_yaw, input_thr);
     }
-}
+} 
 
 void blue_trig_isr(){
-    blue_ok=1;   // AHRS 20HZ
+    send_ok=1;   
 }
 
 void trans_blue_data(float in_data, int integer_point, int under_point){  // number of intefer and under_point
         unsigned int conv_trans_data;
         conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point));
-        if(in_data<0) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '-';}
-        else {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '+';}
+        if(in_data<0) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '-';}
+        else {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '+';}
         for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
-            if(cnt_num == under_point) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '.';   }        
-            while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48);       //convert to ASCII    
+            if(cnt_num == under_point) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '.';   }        
+            while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48);       //convert to ASCII    
         }
 }
 
@@ -79,29 +58,23 @@
 //    Barometer     3.3V p9(SDA) p10(SCL) //  
 ////////////////////////////////////////////
 Barometer barometer(p9, p10);
-float t, alt, del_alt;
+float alt=0.0;
 float alt_sum=0.0f, alt_zero=0.0f;
 int count = 0, baro_ok = 0;          // for zero-calibration
-float alt_buffer[4], w_alt=0.6;          // weight for LPF
+float alt_buffer[2], w_alt=0;          // weight for LPF
 
-void get_Baro(float*alt, float*del_alt)
+void get_Baro(float*alt)
 {   
     if (baro_ok==1){
         barometer.update();
         *alt = barometer.get_altitude_m();
         alt_buffer[1] = alt_buffer[0]; 
         alt_buffer[0] = *alt;
-        if(abs(alt_buffer[0]- alt_buffer[1])>100){
-            alt_buffer[0] = alt_buffer[1];
-            *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0];  // Low Pass Filter 
-            *del_alt = 0;      // for calculation of drop speed
+        if(abs(alt_buffer[0]- alt_buffer[1])>20){
+            *alt = alt_buffer[1];
             baro_ok = 0;
         }
         else{
-            *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0];  // Low Pass Filter 
-            alt_buffer[3] = alt_buffer[2];
-            alt_buffer[2] = *alt;
-            *del_alt = alt_buffer[3]-alt_buffer[2];      // for calculation of drop speed
             baro_ok = 0;
         }
     }
@@ -113,23 +86,19 @@
         if (count==1){count++;}
         else{
             if (count<=99){alt_sum = alt_sum + alt; count++;}
-            else {
-                alt_zero = alt_sum / (float)(count-2);
-                count++;
-            }
+            else {alt_zero = alt_sum/(float)(count-1); count++;}
         }
     }
 }
 
 ///////////////////////////////////////
-//         AHRS          5V p14(RX)  //   20Hz
+//         AHRS          5V p27(RX)  //   20Hz
 ///////////////////////////////////////
-Serial AHRS(p13, p14);
-float roll,pitch,yaw,accx,accy,accz;
+Serial AHRS(p28, p27);
+float roll,pitch,yaw,velx,vely,velz,velxyz;
 char AHRS_msg[150];
 int m=0, ahrs_ok=0, AHRS_flag=0;
 volatile unsigned char AHRS_buffer[2];
-float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.7;
 
 void AHRS_isr(){                      //inturupt 
     while(AHRS.readable()){
@@ -141,41 +110,69 @@
     }
 } 
 
-void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
+void get_AHRS(float*roll, float*pitch, float*yaw, float*velx, float*vely, float*velz, float*velxyz)
 {
     if (ahrs_ok == 1){
         ahrs_ok = 0;
-        sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
-        roll_buffer[1] = roll_buffer[0];
-        roll_buffer[0] = *roll;
-        *roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0];  // Low Pass Filter 
-        pitch_buffer[1] = pitch_buffer[0];
-        pitch_buffer[0] = *pitch;
-        *pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0];  // Low Pass Filter 
-        yaw_buffer[1] = yaw_buffer[0];
-        yaw_buffer[0] = *yaw;
-        *yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0];  // Low Pass Filter 
+        sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, velx, vely, velz);
+        *velxyz = (float)sqrt(pow(*velx,2)+pow(*vely,2)+pow(*velz,2));
         baro_ok = 1;
     }
 } 
 
 ///////////////////////////////////
-//      Servo         5V  PWM    //
+//      Servo         5V  PWM    // needed to check pin#
 ///////////////////////////////////
-Servo Micro_unf(p21);
-Servo Micro_gf(p22);
-Servo Linear(p23);
+Servo Throttle(p26);
+Servo Ctrl_1(p25); //below
+Servo Ctrl_2(p23); //upper
+Servo Ctrl_3(p21); //below
+Servo Ctrl_4(p22); //upper
+
+float thr_value = 0.0, cotrl1_value = 0.5, cotrl2_value = 0.5, cotrl3_value = 0.5, cotrl4_value = 0.5;
+float err_roll = 0.0, err_pitch = 0.0, err_yaw = 0.0, err_alt = 0.0, pre_er = 0.0, pre_ep = 0.0, pre_ey = 0.0;
+float kp1=1.0, kd1=0.0, kp2=1.0, kd2=0.0, kp3=1.0, kd3=0.0;   // PD Controller
+
+void ctrl_roll(){
+    err_roll = input_roll - roll;
+    cotrl1_value = (kp1*err_roll)+(kd1*(err_roll-pre_er)/dt);
+    pre_er = err_roll;
+    cotrl1_value = (cotrl1_value/180.0) + 0.5;
+    if (cotrl1_value<=0.0){cotrl1_value=0.0;}
+    else if (cotrl1_value>=1.0) {cotrl1_value=1.0;}
+    Ctrl_1 = cotrl1_value;
+    Ctrl_3 = -cotrl1_value;
+}
 
-float unf_value=1.0, gf_value = 0.35, linear_value = 0.25;
-float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
+void ctrl_pitch(){
+    err_pitch = input_pitch - pitch;
+    cotrl2_value = (kp2*err_pitch)+(kd2*(err_pitch-pre_ep)/dt);
+    pre_ep = err_pitch;
+    cotrl2_value = (cotrl2_value/180.0) + 0.5;
+    if (cotrl2_value<=0.0){cotrl2_value=0.0;}
+    else if (cotrl2_value>=1.0) {cotrl2_value=1.0;}
+    Ctrl_2 = cotrl2_value;
+    Ctrl_4 = -cotrl2_value;
+}
+  
+void ctrl_yaw(){
+    err_yaw = input_yaw - yaw;
+    cotrl3_value = (kp3*err_yaw)+(kd3*(err_yaw-pre_ey)/dt);
+    pre_ey = err_yaw;
+    cotrl3_value = (cotrl3_value/180.0) + 0.5;
+    if (cotrl3_value<=0.0){cotrl3_value=0.0;}
+    else if (cotrl3_value>=1.0) {cotrl3_value=1.0;}
+    Ctrl_1 = cotrl3_value;
+    Ctrl_2 = cotrl3_value;
+    Ctrl_3 = cotrl3_value;
+    Ctrl_4 = cotrl3_value;
+}
 
-void ctl_gf(){
-    err_yaw = yaw - tg_yaw;
-    gf_value = 0.35*exp(0.0039*p*err_yaw);
-    if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));}
-    if (gf_value<=0.0){gf_value=0.0;}
-    else if (gf_value>=0.7) {gf_value=0.7;}
-    Micro_gf = gf_value;
+void neutral(){
+    Ctrl_1 = cotrl1_value;
+    Ctrl_2 = cotrl2_value;
+    Ctrl_3 = cotrl3_value;
+    Ctrl_4 = cotrl4_value;
 }
 
 ///////////////////////////////
@@ -199,40 +196,25 @@
 }
 
 void Log_data(){
-    fprintf(fp, "%i,%.0f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
+    fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",roll,pitch,yaw,alt,input_roll,input_pitch,input_yaw,input_thr,velx,vely,velz);
 }
 
 void send_Blue(){
-    if (blue_ok == 1){
-        blue_ok = 0;
-        trans_blue_data((float)stat,1,0);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; // basic formation of way to send data
-        trans_blue_data(Kor_time,6,0);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(latitude,2,6);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; 
-        trans_blue_data(longitude,3,6);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(alt,3,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(del_alt,3,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(alt_GPS,3,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+    if (send_ok == 1){
+        send_ok = 0;
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '*';
         trans_blue_data(roll,3,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
         trans_blue_data(pitch,3,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
         trans_blue_data(yaw,3,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(accz,2,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(unf_value,1,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(gf_value,1,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
-        trans_blue_data(linear_value,1,2);
-        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '\n';
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
+        trans_blue_data(alt,2,2);
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
+        trans_blue_data(velxyz,2,2);
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
+        trans_blue_data(velz,2,2);
+        while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '\n';
     }
 }
 
@@ -244,80 +226,20 @@
 int main(void)
 {   
     AHRS.baud(9600);
-    Blue_GPS.baud(9600);
-    Blue_GPS.attach(&GPS_isr);
+    Blue.baud(9600);
+    Blue.attach(&Blue_isr);
     AHRS.attach(&AHRS_isr);
-    blue_trig.attach(&blue_trig_isr, 0.125); 
-    Micro_gf = gf_value; //Initial setting of servo
+    blue_trig.attach(&blue_trig_isr, 0.1);
+    neutral();
+    //Log_file();
         while(1) {
-        switch(stat){
-            case 1 :   //Bluetooth connection and calibration
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
-                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                get_Baro(&alt, &del_alt);
-                calb_alt();
-                send_Blue();
-                if (100<count) {stat=2;}
-                break;
-            case 2 :   //Wait for launch and keep the order get_AHRS, it sets trigger of get_Baro and send_Blue and Log-data save raw_alt
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
-                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                get_Baro(&alt, &del_alt);
-                alt = alt - alt_zero;
-                send_Blue();
-                if (100.0<alt_GPS && del_alt>1){
-                    stat=3;
-                    Log_file();
-                    Micro_unf = unf_value;
-                    unf_value = 0.0;   //checking
-                    wait(0.5);
-                    Micro_gf = gf_value;
-                }
-                break;
-            case 3 :   //Grid fin and landing leg spreading and control
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
-                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                get_Baro(&alt, &del_alt);
-                Log_data();
-                alt = alt - alt_zero;
+                get_AHRS(&roll,&pitch,&yaw,&velx,&vely,&velz,&velxyz);
+                get_Blue(&input_roll,&input_pitch,&input_yaw,&input_thr);
+                get_Baro(&alt);
+                if (input_roll!=0){ctrl_roll();}
+                if (input_pitch!=0){ctrl_pitch();}
+                if (input_yaw!=0){ctrl_yaw();}
                 send_Blue();
-                if (alt<10){ // need to modify trigger 
-                    fclose(fp);
-                    Log_file();
-                    stat=4;
-                    Linear=linear_value;
-                    linear_value=0.0;
-                } 
-                break;
-            case 4 :   //Reverse thrust for soft landing
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
-                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                get_Baro(&alt, &del_alt);
-                Log_data();
-                alt = alt - alt_zero;
-                send_Blue();
-                if (accx<0.1 && accy<0.1 && alt<3){
-                    fclose(fp);
-                    Log_file();
-                    end.start();
-                    stat=5;
-                }
-                break;
-            case 5 :   //Landing
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
-                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                get_Baro(&alt, &del_alt);
-                Log_data();
-                alt = alt - alt_zero;
-                send_Blue();
-                end.read();
-                if (end.read()>=10) {
-                    fclose(fp);
-                    stat=6;
-                }
-                break;
-            case 6 :   //Shut down
-                break;
-        }
+                if (input_roll==180.0 && input_pitch==180.0 && input_yaw==180.0 && input_thr==0){break;}
     }
 }
\ No newline at end of file