1、賽題回顧

2、硬件說明

1. 整個模塊分上下三層：最下層為亞克力板固定四個8520電機(jī)，中間層為電機(jī)驅(qū)動電路板、各個模塊接口等，上層為主控核心板；

2. 電機(jī)兩個正轉(zhuǎn)，兩個反轉(zhuǎn)，分別對應(yīng)X、Y的+/-兩個方向；螺旋槳兩正兩反，在這里螺旋槳做推力方向，可以良好的避免異常情況下的射槳等極端情況；

3. 傳感器：MPU6050陀螺儀模塊

4. 點狀激光頭 –固定在底部亞克力板上

5. 電機(jī)使用8520空心杯電機(jī)，電氣參數(shù)如下：
   電機(jī)型號：8520空心杯電機(jī)（2S） 重量：5.3g
   工作電壓：5~7.4V DC 堵轉(zhuǎn)電流：8A
   空載電流：100mA 空載轉(zhuǎn)速：50000RPM
   負(fù)載電流：1.2A 負(fù)載轉(zhuǎn)速：44000RPM

3、算法分析

3.1、基礎(chǔ)知識

• 當(dāng)只有一個方向的水平擺動的情形如x方向的擺動時；根據(jù)定義，我們可將系統(tǒng)理想化為單擺模型，因此有：

  

  
  上述（1）為簡諧運(yùn)動的典型運(yùn)動學(xué)公式，質(zhì)點做簡諧運(yùn)動，其位移與時間的函數(shù)關(guān)系；
  f(x)：表示位移

A：表示振幅
w：表示角速度
φ：表示相位
周期T與w之間的關(guān)系為：T=2π/w。

• 為單擺周期計算公式，結(jié)合實物參數(shù)推算大致的運(yùn)動周期：
  L=擺桿長度=0.3m
  g=重力加速度，我們?nèi)?0m/s2
  因此，推導(dǎo)出下面公式：

  

  
  顯然，上面的擺桿我們選取的長度是相對比較短的，假設(shè)極端一點，我們選取擺桿長度為2m，此時公式：

  

  
  周期T的取值范圍：1.09s ~2.81s

• 根據(jù)單擺周期的實驗法去測得周期：這里我們測量的是十次單擺周期秒表計時為11.5s；因此在程序中定義

#define Period 1150 //單擺周期 ms 

如果修改了擺桿長度、機(jī)械結(jié)構(gòu)，這個值要去重新測得，然后看公式（1）中的變量

A：振幅，是我們需要設(shè)定的擺動幅度；
φ：相位，在這里可以理解為開始時對應(yīng)的位移，我們設(shè)為0；
w：角速度，w=2π/T 已知
t：自變量，u32 TimeCnt;//運(yùn)動時間計數(shù)器

綜上，可以推導(dǎo)出f(x)

公式（3）是一種典型的數(shù)據(jù)離散歸一的處理方法，具體的意義我們可以理解為在一個運(yùn)動周期內(nèi)每隔5ms（5ms中斷）取點，映射到2π的范圍上，記錄對應(yīng)點位移隨著時間在A范圍內(nèi)正弦函數(shù)周期性變化情況；

3.2、PID算法

3.2.1、參數(shù)設(shè)置

圖1
根據(jù)上圖做設(shè)置以下參數(shù)定義 :

• 離地高度 float Height=675（單位 mm）；

• 角度由陀螺儀模塊反饋獲得，X 方向為 Roll，Y 方向為 Pitch

• 激光點位置 float Measure_X，可以通過測得的角度三角轉(zhuǎn)換得出測量值：Measure_X=tan(Roll)Height；加入初始值、弧度單位換算即：Measure_X=(float)tan((Roll-ZHONGZHI_B)/1802PI)Height;

3.2.2、控制簡諧運(yùn)動

• 控制激光點的位置滿足單擺運(yùn)動因此有目標(biāo)值 Target_X：

Alpha=(float)TimeCnt/Period2PI;
Target_X=Amplitude_xsin(Alpha);    //X 方向目標(biāo)值函數(shù) PID 公式 ：pwm=Kpe(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]

此時可以通過控制 Amplitude_x 來控制擺動的幅度，導(dǎo)入 PID 這里就不做具體的敘述了；這樣就可以控制風(fēng)力擺每個方向做單擺運(yùn)動（簡諧運(yùn)動）；

3.2.3、李薩如圖形

定義：李薩如圖形由在互相垂直的方向上的兩個頻率成簡單整數(shù)比的簡諧振動所合成的規(guī)則的、穩(wěn)定的閉合曲線；
要點：互相垂直、 簡諧運(yùn)動、頻率成整數(shù)比
系統(tǒng)滿足要求，我們來看下李薩如圖形的數(shù)學(xué)定義：

設(shè)n=q/p; n稱為曲線的參數(shù)，是兩個正弦振動的頻率比; 參數(shù)方程可以寫作:

又有李薩如圖形特殊情形：

若a=b，n=1，則曲線是橢圓
若 φ=π/2 ，則這橢圓其實是圓
若 φ=0 ，則這橢圓其實是線段

對應(yīng)我們的需求，回到我們風(fēng)力擺系統(tǒng)中，當(dāng)風(fēng)力擺同時參與兩個互相垂直方向上的簡諧運(yùn)動，風(fēng)力擺的位移是這兩個振動的矢量和：

1. n=1，我們沒有必要去調(diào)節(jié)頻率比；因此有我們的目標(biāo)值函數(shù)：Target_X=Amplitude_xsin(Alpha); Target_Y=Amplitude_ysin(Alpha+Phase);

2. 畫直線，相位差φ=0；

3. 畫圓，a=b，n=1，φ=π/2或者3π/2

• 這部分內(nèi)容較多，這里就不展開講解了，詳細(xì)的可以查看該資料：https://mbd.pub/o/bread/ZJiakp5y

4、電路設(shè)計

4.1、STM32最小系統(tǒng)

4.2、電源電路

4.3、OLED顯示電路

5、程序設(shè)計

5.1、位置式PID控制

int Position_PID_X (float value,float Target) { static float Bias,Pwm,Integral_bias,Last_Bias;
 Bias=value-Target; //計算偏差 Integral_bias+=Bias; //求出偏差的積分 Pwm=Position_KP*Bias+ //PID控制器比例項 Position_KI*Integral_bias+ //PID控制器積分項 Position_KD*(Bias-Last_Bias); //PID控制器微分項  Last_Bias=Bias; //保存上一次偏差  return Pwm; //增量輸出 } int Position_PID_Y (float value,float Target) { static float Bias,Pwm,Integral_bias,Last_Bias;
 Bias=value-Target; //計算偏差 Integral_bias+=Bias; //求出偏差的積分 Pwm=Position_Kp*Bias+ //PID控制器比例項 Position_Ki*Integral_bias+ //PID控制器積分項 Position_Kd*(Bias-Last_Bias); //PID控制器微分項  Last_Bias=Bias; //保存上一次偏差  return Pwm; //增量輸出 }

5.2、讀取MPU6050姿態(tài)信息

void Read_DMP(void) { unsigned long sensor_timestamp; unsigned char more; long quat[4];

 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); if (sensors & INV_WXYZ_QUAT )
 { 
 q0=quat[0] / q30;
 q1=quat[1] / q30;
 q2=quat[2] / q30;
 q3=quat[3] / q30;
 Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 
 Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw }
}

6、資料下載

• https://mbd.pub/o/bread/ZJiakp5y