Atsumi Toda
UAV_Logger1から改造。サーボ機構のプログラムを追加
Dependencies: MPU6050_alter HMC5883L
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
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r e6496a794bde -r 3fa7882a5fd0 main.cpp
--- 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);
/*----------------------------------------------------------------*/