由於項目需求，需要使用6050製作風力擺，因此寫此博客給大家提供一個快速搭建的方法，通過cubemx快速生成。
控制方案設計
按照題目的任務要求，需要設計一個風力擺控制系統，在具有一定抗干擾的能力的情況下完成自由擺動、指定長度擺動、快速制動以及半徑可設置的圓周運動等任務。我們採用STM32F103VET6單片機做爲主控芯片，用6位姿態傳感模塊MPU6050來計算角度和風機狀態，用直流風機帶動擺杆運動。當MPU6050檢測到擺杆的角度時，可根據三角函數公式計算出擺杆現在距離中心的具體位置（方向、距離），單片機控制PWM波的輸出大小來控制風機的風速與方向，使擺杆達到在特定位置靜止或按照一定的軌跡運動。當擺杆處於自然下垂狀態時，給四個風機同時上電且風向都向外，此時擺杆仍處於受力平衡——靜止狀態。此時降低X軸上一個風機的轉速，擺杆將會帶動激光頭在X軸上畫一條直線，當達到一定的傾斜角度時，單片機可根據角度計算出此時距離中心的距離是否>=25cm，若達到要求後，此風機減速，X軸反方向上電機逐漸加速，恢復到初始速度，反方向做相同的運動。在此過程中，單片機做出A/D採樣，Y軸方向方向風機隨時做出矯正，防止發生軌跡偏移。控制系統框圖如圖1.1所示。
機械結構方案設計
由於擺杆長度（60cm70cm）較長，且要求激光頭在地面畫出15cm35cm的圓，所以要求橫杆的距離要足夠長。橫杆長度較長加之擺杆重量較大，所以要求底座要有足夠的重量來支撐整個系統。如果結構不穩或者重量不夠，擺杆運動過程中將會產生傾倒或者抖動等現象，影響測試結果。於是，底座採用了“工”型結構，保證了整個系統的穩定性。擺杆材料方面，我們選用輕便的硬質鐵質材料與風機進行剛性連接，既能保證連接處的穩定，又可達到減輕底座負擔的目的。風機選擇方面，既要保證風力夠大，能夠實現題目基本要求中的自由擺運動、快速制動靜止。又要有一定的重量，質量越大，慣性越大，越不容易改變原來的運動狀態，在實現發揮部分（2）時，受到外界干擾，不容易改變原來的圓周運動狀態機械結構如圖1.2
1.底座；2.豎杆；3.橫杆；4.螺栓；5.萬向節；6.擺杆；7.風機（1）；8.風機（2）；9.風機（3）；10.風機（4）
電路與程序設計
- 處理器：STM32F103VET6,72MHz主頻，256KB內存，滿足數據處理和運算的要求。
-
傳感器：MPU6050六軸姿態傳感，3.3-5V供電電壓，IIC通訊，最高速率可達400KHz，100Hz有效數據輸出速率，陀螺儀範圍±250 500 1000 2000°/s，多維度高精度測量。
![在這裏插入圖片描述]()
-
電機：716空心杯電機，規格直徑7mm長16mm軸徑0.8mm,額定電壓3.4V，額定轉速34000，額定電流0.8A，反應靈敏，風力大。
-
電機驅動：L298N模塊,驅動部分端子供電範圍Vs：＋5V～＋35V；如需要板內取電，則供電範圍Vs：+7V～+35V；驅動部分峯值電流Io：2A；邏輯部分端子供電範圍Vss：＋5V～＋7V（可板內取電＋5V；邏輯部分工作電流範圍:0～36mA；控制信號輸入電壓範圍：低電平：－0.3V≤Vin≤1.5V，高電平：2.3V≤Vin≤Vss；使能信號輸入電壓範圍：低電平：－0.3≤Vin≤1.5V（控制信號無效），高電平：2.3V≤Vin≤Vss（控制信號有效）；最大功耗：20W（溫度T＝75℃時）
![在這裏插入圖片描述]()
5）按鍵模式選擇部分
按鍵名稱 S1 S2 S3 S4 S5
功能 + - 確認 退出當前模式 參數設置確定
- 顯示與提示部分
LED1爲黃燈，LED2爲紅燈。S1和S2每按下一次，黃燈亮一次表示模式切換或參數修改成功；S3、S4和S5每按下一次，紅燈亮3S表示按鍵有效。
![在這裏插入圖片描述]()
程序功能描述與設計思路
系統採用獨立按鍵控制進行模式選擇，設置有5個按鍵，按鍵1和2代表模式加減（或參數值加減），按鍵3表示模式確認，按鍵5代表參數設置確認，按鍵4表示模式退出，使用兩個LED燈做顯示提示。系統開機後進入初始化狀態兩個LED燈常亮四個風機以相同的速度轉動整個擺杆系統處於垂直平衡狀態。然後通過按鍵1、2進行模式選擇，按鍵3按下鎖定當前模式，進入參數設置環節。參數設置時按鍵1、2表示參數加減，按下按鍵5表示鎖定當前參數，系統開始執行任務。
2、程序設計思路
基本要求（1）可用閉環控制，設定傾角閾值，從低到高不斷增加X風機轉速，直到傾角超過閾值，記錄此時PWM脈寬級作用時間。
基本要求（2）需要採用閉環控制算法，計算公式與（1）相同，設定好範圍後，可與（1）使用相同的方法調試。
基本要求（3）要採用受力分析，使用力的合成。
基本要求（4）拉起一定角度後，如果不提供動力，擺杆將逐漸衰減，但是速度緩慢，因此需要提供與運動方向相反的力，阻礙擺杆運動。這個過程需要注意，實時採集擺杆的角度（比例項 P），進行微分從而確定角速度（陀螺儀直接測量亦可），爲微分項D，採用類似於自平衡的PD控制算法即可。
程序流程圖
PID算法框圖
CUBEMX配置
代碼部分
- 6050初始化
void DMP_Init(void)
{
uint8_t temp[1]={0};
IIC_ReadBytes(devAddr,0x75,1,&temp[0]);//0x68
//printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
//printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
//printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
//printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())
//printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
//printf("dmp_set_orientation complete ......\r\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
//printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
temp[0] = 1;//printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test();
if(!mpu_set_dmp_state(1))
temp[0] = 1;
//printf("mpu_set_dmp_state complete ......\r\n");
}
}
- DMP讀取角度數據
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
}
}
- 通過6050返回角度值進行電機打開與關閉
if(Pitch>20){
MOT1 = MOT1-Pitch;
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,MOT1);
}else if(Pitch<-20){
MOT3 = MOT3-Pitch;
__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,MOT3);
}else{
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,0);
__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,0);
}
if(Roll>40){
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,800);
}else if(Roll<-40){
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,800);
}else {
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_1,0);
}
最終實驗結果
根據代碼，該風力擺控制系統能達到基本部分和發揮部分的全部要求和性能指標，由此可以得出以下結論：
1.風機的性能是決定系統能否完成題目要求的關鍵，風機性能由其本身決定，包括重量、產生的最大風速、以及所需的電流。任何一個達不到要求，將會給程序調控大大增加難度，所以硬件搭建很重要。
2.好的算法，也起到至關重要的作用，爲了能達到題目要求，四個風機的速度分配比例很重要，這就需要良好的算法加合適的參數。
3.此控制系統，需要時間上的配合，時間參數同樣重要，要能保證在規定時間內完成任務。
綜上所述，本設計達到設計要求。
總結
這次比賽我們收穫頗豐，除了學到硬件搭建、電路調試、軟件測試相關的知識和技能，我們還親身體驗了團隊協作合理安排時間的重要性。開始的時候，由於我們對電機驅動模塊、716空心杯風機和供電導線瞭解不夠，驅動板並聯供電，使用延長杜邦線給電機供電，導致我們調試的時候，總達不到題目的要求還燒壞了不少電機。我們分頭查閱大量資料討論後，決定更換供電導線，並將兩個驅動模塊的供電提高分開，以減少干擾。更換之後，線路更可靠供電穩定了不少。從這一點來看，團隊之間溝通很重要；最重要的是遇到困難要不要被得失所困擾，要一心一意去解決這個問題。