Revision 3:3fa7882a5fd0, committed 2019-07-24

Comitter:

Joeatsumi

Date:

Wed Jul 24 12:00:01 2019 +0000

Parent:

2:e6496a794bde

Commit message:

AlteredUAV_Logger1;

Changed in this revision

--- a/main.cpp	Fri May 24 05:57:12 2019 +0000
+++ b/main.cpp	Wed Jul 24 12:00:01 2019 +0000
@@ -42,6 +42,12 @@
 Timer passed_time;
 /*---------------------*/
 /*-------ピン指定------------*/
+//declare PWM pins here
+PwmOut ESC (p21);
+PwmOut Elevator_servo(p22);
+PwmOut Rudder_servo(p23);
+
+
 SDBlockDevice   blockDevice(p5, p6, p7, p8);  // mosi, miso, sck, cs
 MPU6050 mpu(p28, p27);//sda scl
 HMC5883L compass(p28, p27);//sda scl
@@ -55,6 +61,9 @@
 DigitalOut led2(LED2);
 
 /*-----------グローバル変数-----------*/
+double Euler_angle[3];
+double Euler_angle_Initiated[3];
+
 float g_hokui,g_tokei;
 int fp_count=0;
 int gps_flag=0;
@@ -91,6 +100,27 @@
 
 /*-------------------------------------------------------------------*/
 
+/*----------PID コントロール-----------------*/
+#define Gain_Kp 0.8377
+#define Gain_Ki  0.4450
+#define Gain_Kd  0.3890
+ 
+double Prop_p,Int_p,Dif_p;
+double Pre_Prop_p;
+/*-----------------------------------------*/
+
+/*-----------サーボコントロール----------------*/
+//define period of servo here
+#define SERVO_PERIOD 0.020 // servo requires a 20ms period
+#define NUTRAL  0.0015
+#define UPPER_DUTY  0.0020
+#define LOWER_DUTY  0.0010
+
+double pitch_duty,roll_duty,yaw_duty;
+
+/*------------------------------------------*/
+
+
 /*プロトタイプ宣言*/
 void toString_V(Vector& v);   // ベクトルデバッグ用
 void gps_rx();
@@ -275,6 +305,77 @@
     }//while ends
 }
 
+void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){
+    
+    //float pitch,roll,yaw;
+    
+    Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) );
+    Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2)));
+    Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3));
+
+    }
+    
+
+double Degree_to_PWM_duty(double degree){
+    
+    double duty=0.0;
+    
+    duty=(0.2/1000)/20.0*degree;
+    
+    if((duty+NUTRAL)>=UPPER_DUTY){
+        duty=UPPER_DUTY-NUTRAL;
+    }else if((duty+NUTRAL)<=LOWER_DUTY){
+        duty=NUTRAL-LOWER_DUTY;
+    }else{}
+    
+    return duty;
+    
+    }
+    
+double PID_duty(double target,double vol,float dt){
+            //ここで角度の単位で偏差は入力される
+            //出力はPWMで
+            double duty=0.0;
+            double add_duty=0.0;
+            
+            Prop_p=target-vol;
+            
+            pc.printf("%f/r/n",Prop_p);
+            
+            Int_p+=Prop_p*double(dt);
+            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+            
+            Pre_Prop_p=Prop_p;
+            
+            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
+            
+            add_duty=1.0*Prop_p;
+            
+            duty+=add_duty;
+            duty=Degree_to_PWM_duty(duty);
+            
+            return duty;
+            
+}
+
+void Initialize_ESC(){
+    
+    ESC.pulsewidth(0.002);
+    wait(3);
+    
+    ESC.pulsewidth(0.001);
+    wait(5);
+    
+    }
+    
+    
+void Activate_ESC(){
+    
+    ESC.pulsewidth(0.001);
+    wait(5);    
+    
+    }
+
 // Entry point for the example
 int main()
 {
@@ -287,6 +388,16 @@
     Thread thread1 (gps_convertion);
     gps.attach(gps_rx, Serial::RxIrq);
     
+        /*define period of serve and ESC*/
+    ESC.period(SERVO_PERIOD);    
+    Elevator_servo.period(SERVO_PERIOD);
+    Rudder_servo.period(SERVO_PERIOD);
+    /*------------------------------*/
+    Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
+    Rudder_servo.pulsewidth(NUTRAL);
+    
+    Activate_ESC();
+    
     float Time_old=0.0;//時間初期化
     passed_time.start();//タイマー開始
     
@@ -393,7 +504,7 @@
             Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
             
             
-            Time_old=Time_new;//時間の更新
+            //Time_old=Time_new;//時間の更新
             
             /*
             pc.printf("%f,%f,%f,%f\r\n"
@@ -418,7 +529,7 @@
                 
                 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
                     
-                pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);                
+                //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);                
                 
                 double pitch_and_roll[2];
                 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
@@ -460,10 +571,10 @@
                 
                                 
                 /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
-                /*
+                
                 Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
                                     +Complement_matrix_2*Qua_acc;
-                */
+                
                 /*----------------------------------------------------------------------------*/
                 
                 
@@ -477,6 +588,7 @@
                      
             }else{}
             
+            
             /*
             pc.printf("%f,%f,%f,%f,%f\r\n"
                      ,Time_new
@@ -485,6 +597,30 @@
                      ,Quaternion_atitude_from_omega.GetComp(3)
                      ,Quaternion_atitude_from_omega.GetComp(4));
             */
+            /*-----------Quaternionからオイラー角への変換-----------*/
+            Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1)
+                                ,Quaternion_atitude_from_omega.GetComp(2)
+                                ,Quaternion_atitude_from_omega.GetComp(3)
+                                ,Quaternion_atitude_from_omega.GetComp(4)
+                                ,Euler_angle);
+                                
+            /*-----------オイラー角の表示----------------------*/
+            
+            pc.printf("%f,%f,%f,%f,%f\r\n"
+                     ,Time_new
+                     ,Euler_angle[0]
+                     ,Euler_angle[1]
+                     ,Euler_angle[2]
+                     );
+            
+            /*--------------------------------------------------*/
+            //Pitch control
+            pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old));
+            Elevator_servo.pulsewidth(pitch_duty); 
+             
+             
+            Time_old=Time_new;//時間の更新
+            
             wait(0.001);
             
             /*----------------------------------------------------------------*/