1、感想：
我想：我的一系列痛苦源泉都從這里出發，我為什么要做這個車，電路板你看你焊的是個啥子玩意，最后還不是用的是開發板下的程序，鐵混子一個，好的，還是來看下我的開發經歷和過程吧！
潑個圖：
還請自動忽視掉這雙好看的手與可愛的機器貓
2、元器件：
1、硬件資源：
帶霍爾編碼器的直流電機兩個（偏心輪和帶減速器），支架若干，杜邦線若干，L298N，降壓模塊JM34RPLM2596S，stm32核心板：（當時准備c8t6，但看了它原理圖后去發現好像定時器只有一個這就讓我突然懵逼，后來就改成了vct6的大容量的核心版，但后來的我其實發現STM32的板子其實所有功能差不多，只有flash的容量上的差別，潘總說得，說得我當場死去，沒事）
MPU6050，12v航模電池。
2、軟件資源：
定時器3個（定時器1用兩個通道產生10KHZ的PWM波，定時器2，4設置為編碼器模式，通過編碼器得到兩個輪子的轉速，轉速轉換可見下面代碼，定時器3設置為輸入捕獲模式用於超聲波避障），串口通信兩個，分別是USART1和USART3主要考慮引腳才用這兩個串口，USART1用於接收數據到電腦上位機上顯示波形，波特率為128000，用來調節PID參數，（一定要注意USART1he題目都是高級的，都掛接在APB2總線上）USART3用來與藍牙連接，外部中斷EXTI0，用於MPU6050 INT0產生下降沿后觸發，用於控制電機的速度控制，模擬IIC（用於處理和接收MPU6050數據）
void Encoder_Init_TIM2(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; GPIO_Init(GPIOA, &GPIO_InitStructure); TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0x0; TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; TIM_ICInit(TIM2, &TIM_ICInitStructure); TIM_ClearFlag(TIM2, TIM_FLAG_Update); TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //Reset counter TIM_SetCounter(TIM2,0); TIM_Cmd(TIM2, ENABLE); } void Encoder_Init_TIM4(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; GPIO_Init(GPIOB, &GPIO_InitStructure); TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0x0; TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; TIM_ICInit(TIM4, &TIM_ICInitStructure); TIM_ClearFlag(TIM4, TIM_FLAG_Update); TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //Reset counter TIM_SetCounter(TIM4,0); TIM_Cmd(TIM4, ENABLE); } int Read_Encoder(u8 TIMX) { int Encoder_TIM; switch(TIMX) { case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break; case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0;break; case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0;break; default: Encoder_TIM=0; } return Encoder_TIM; }
3、主要算法 PID，DMP，濾波
1、什么是PID
PID應該都知道是比例積分微分吧，直接來干貨吧！
比如：讓一個直木棒在手指尖上保持直立。這需要兩個條件：一個是托着木棒的手掌可以移動；另一個是眼睛可以觀察到木棒的傾斜角度和傾斜趨勢（角速度）。通過手掌移動抵消木棒的傾斜角度和趨勢，從而保持木棒的直立。這兩個條件缺一不可，
此時MPU6050傳過來的位置數據就是我們的眼睛所采集的數據，而處理信息的大腦就變成了我們的單片機，大腦下達命令給手腳去做相應的移動，我們的單片機也通過產生時序不一的高低電平信號下達相應的指令去控制電機的轉動，那怎么去下達這樣的一個命令，下達一個怎樣的命令才能符合我們對電機的控制，這就是我們要解決的核心問題
所以車模平衡控制也是通過負反饋來實現的，與上面保持木棒直立比較則相對簡單。因為車模有兩個輪子着地，車體只會在輪子滾動的方向上發生傾斜。控制輪子轉動，抵消在一個維度上傾斜的趨勢便可以保持車體平衡了
至於為什么，還請參考相關資料，我下次在更，
2、如何調節PID參數
我以前總認為ＰＩＤ參數，讓它工作應該不難，難的是想讓它工作的好一點，更加好一點。但現在看來很是錯，對於象我這樣的初學者往往只重視這個ＰＩＤ，卻很是忽略ＰＩＤ能工作的“基礎環境”。
１、所謂的“基礎環境”就是控制對象的工作范圍，我就說說我自己在搞的這個電流環吧，比如說電流的基准值是０－５００，也就是說我的ＰＷＭ波的周期為１０００，哦，忘了說了，我控制方式是雙極性Ｈ橋控制。這樣在０－５００為正轉，５００－１０００為反轉，因為剛開始也沒考慮那么多，結果去檢測電流后的ＡＤ轉換是０－１０２４，更要命的是我以為ＰＩＤ是萬能的，只要把這些參數整一下也應該能搞定的，結果可想而知了，感嘆自己的無知啊！！保險絲斷了一大籮，這才恍然大悟，“萬能的”ＰＩＤ參數給了我重重的打擊。各個參數的工作范圍設計是多么的重要啊。所謂的ＰＩＤ參數只是在這個基礎環境之上工作與控制的。。。。
２、現在我開始想搞速度控制了（速度環），結果你想好了，速度環的輸出控制目標是電流環的給定電流參考值，但我搞的速度是０－３０００轉速，現在不知道是怎么樣把轉速和這個電流參考輸出值對應起來，就是說我的工作參數又不在一個范圍了，轉速是０－３０００，而電流值是０－５００，該怎么控制對應啊，我現在都暈了。。。。 望大家給點建議和思路，謝謝！！！
1、經驗數據法
PID控制器的參數整定在大量的工程實踐中，逐漸被廣大工程技術人員經過大量的經驗積累找到了一種快捷的整定方法，就是我們現在介紹的所謂“經驗法”。實際上比例、積分和微分三部分作用是相互影響的，應用經驗法可避免一些重復工作，節省調試時間，尤其是在缺少一些資料和試驗數據的時候。從應用的角度看，只要被控對象主要指標達到設計要求，能滿足現場要求即可。長期的實踐經驗發現，各種不同被控對象的PID的參數都是有一定規律的，也就是說有一定的數據范圍。這樣就為現場調試提供了一個大致基准，可方便依據此基准迅速查找。
2、試湊法
顧名思義，試湊法就是根據過渡過程中被調參數變化的情況進行再調整PID參數的方法。此法邊觀察過程曲線（過程變量變化情況），邊修改參數，直到滿意為止。
大家都知道，增大比例系數Kp會加快系統的響應速度，提高系統的快速性。但過大的比例系數會使系統有較大的超調，有可能產生振盪使穩定性變差，並且有穩態誤差。減小積分系數KI將減少積分作用（與積分時間常數的變化相反），有利於減少超調使系統穩定，減小系統穩態誤差，但系統消除靜差的速度慢。增加微分系數KD有利於加快系統的響應，使系統提前作出響應，使超調減少，穩定性增加，但缺點明顯，對干擾的抑制能力差，而且整定不當反而使系統處於不穩定狀態。試湊時，一般可根據以上各參數特點，對參數實行先比例、后積分、再微分的步驟進行整定。
（1） 比例部分整定。首先將積分系數KI和微分系數KD置零，取消微分和積分作用而采用純比例控制。將比例系數Kp由小到大變化，觀察系統的響應，直至響應速度快，且有一定范圍的超調為止。如果系統靜差在規定范圍之內，且響應曲線已滿足設計要求，那么只需用純比例調節器即可。
（2） 積分部分整定。如果比例控制系統的靜差達不到設計要求，這時可以加入積分作用。在整定時將積分系數KI由小逐漸增加（積分作用就逐漸增強），觀察輸出，系統的靜差應逐漸減少直至消除（在性能指標要求下）。反復試驗幾次，直到消除靜差的速度滿意為止。注意這時的超調量會比原來加大，可能需要適當降低一些比例系數Kp。
（3） 微分部分整定。若使用比例積分（PI）控制器經反復調整仍達不到設計要求，應考慮加入微分作用。整定時先將微分系數KD從零逐漸增加（微分作用逐漸增強），觀察超調量和穩定性，同時相應地微調比例系數Kp、積分系數KI，逐步試湊，直到滿意為止。注意，在設計控制系統時，應使微分環節為實際微分環節，而不可以是理想微分環節。
這種方法的基本程序是先根據運行經驗，確定一組調節器參數，並將系統投入閉環運行，然后人為地加入階躍擾動（如改變調節器的給定值），觀察被調量或調節器輸出的階躍響應曲線。若認為控制質量不滿意，則根據各整定參數對控制過程的影響改變調節器參數。這樣反復試驗，直到滿意為止。
經驗法簡單可*，但需要有一定現場運行經驗，整定時易帶有主觀片面性。當采用PID調節器時，有多個整定參數，反復試湊的次數增多，不易得到最佳整定參數。
下面以PID調節器為例，具體說明經驗法的整定步驟：
⑴讓調節器參數積分系數S0=0，實際微分系數k=0，控制系統投入閉環運行，由小到大改變比例系數S1，讓擾動信號作階躍變化，觀察控制過程，直到獲得滿意的控制過程為止。
⑵取比例系數S1為當前的值乘以0.83，由小到大增加積分系數S0，同樣讓擾動信號作階躍變化，直至求得滿意的控制過程。
⑶積分系數S0保持不變，改變比例系數S1，觀察控制過程有無改善，如有改善則繼續調整，直到滿意為止。否則，將原比例系數S1增大一些，再調整積分系數S0，力求改善控制過程。如此反復試湊，直到找到滿意的比例系數S1和積分系數S0為止。
⑷引入適當的實際微分系數k和實際微分時間TD，此時可適當增大比例系數S1和積分系數S0。和前述步驟相同，微分時間的整定也需反復調整，直到控制過程滿意為止。
注意：仿真系統所采用的PID調節器與傳統的工業 PID調節器有所不同，各個參數之間相互隔離，互不影響，因而用其觀察調節規律十分方便。
PID參數是根據控制對象的慣量來確定的。大慣量如：大烘房的溫度控制，一般P可在10以上,I=3-10,D=1左右。小慣量如：一個小電機帶一水泵進行壓力閉環控制，一般只用PI控制。P=1-10,I=0.1-1,D=0,這些要在現場調試時進行修正的。
======================================================================
一般是這樣子調節：先逐漸增大P增益，調到振盪發生前的最大值，
再逐漸減小I增益，調節到振盪發生前的最大值，
最后逐漸增大D增益，調節到振盪發生前的最大值。
======================================================================
下次寫吧！還有很多細節沒寫，我會補上的
貼上我的控制代碼：
#include "control.h" #include "filter.h" /************************************************************************** ×÷ÕߣºÆ½ºâС³µÖ®¼Ò ÎÒµÄÌÔ±¦Ð¡µê£ºhttp://shop114407458.taobao.com/ **************************************************************************/ int Balance_Pwm,Velocity_Pwm,Turn_Pwm; u8 Flag_Target; int Voltage_Temp,Voltage_Count,Voltage_All; /************************************************************************** º¯Êý¹¦ÄÜ£ºËùÓеĿØÖÆ´úÂ붼ÔÚÕâÀïÃæ 5ms¶¨Ê±ÖжÏÓÉMPU6050µÄINTÒý½Å´¥·¢ Ñϸñ±£Ö¤²ÉÑùºÍÊý¾Ý´¦ÀíµÄʱ¼äͬ²½ **************************************************************************/ int EXTI0_IRQHandler(void) { if(PCin(0)==0) { EXTI_ClearITPendingBit(EXTI_Line0); //Çå³ýLINE5ÉϵÄÖжϱê־λ Flag_Target=!Flag_Target; if(delay_flag==1) { if(++delay_50==10) delay_50=0,delay_flag=0; //¸øÖ÷º¯ÊýÌṩ50msµÄ¾«×¼ÑÓʱ } if(Flag_Target==1) //5ms¶ÁÈ¡Ò»´ÎÍÓÂÝÒǺͼÓËٶȼƵÄÖµ£¬¸ü¸ßµÄ²ÉÑùƵÂÊ¿ÉÒÔ¸ÄÉÆ¿¨¶ûÂüÂ˲¨ºÍ»¥²¹Â˲¨µÄЧ¹û { Get_Angle(Way_Angle); //===¸üÐÂ×Ë̬ Voltage_Temp=Get_battery_volt(); //=====¶ÁÈ¡µç³Øµçѹ Voltage_Count++; //=====ƽ¾ùÖµ¼ÆÊýÆ÷ Voltage_All+=Voltage_Temp; //=====¶à´Î²ÉÑùÀÛ»ý if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====Çóƽ¾ùÖµ return 0; } //10ms¿ØÖÆÒ»´Î£¬ÎªÁ˱£Ö¤M·¨²âËÙµÄʱ¼ä»ù×¼£¬Ê×ÏȶÁÈ¡±àÂëÆ÷Êý¾Ý Encoder_Left=-Read_Encoder(2); //===¶ÁÈ¡±àÂëÆ÷µÄÖµ£¬ÒòΪÁ½¸öµç»úµÄÐýתÁË180¶ÈµÄ£¬ËùÒÔ¶ÔÆäÖÐÒ»¸öÈ¡·´£¬±£Ö¤Êä³ö¼«ÐÔÒ»Ö Encoder_Right=Read_Encoder(4); //===¶ÁÈ¡±àÂëÆ÷µÄÖµ Get_Angle(Way_Angle); //===¸üÐÂ×Ë̬ Read_Distane(); //===»ñÈ¡³¬Éù²¨²âÁ¿¾àÀëÖµ if(Bi_zhang==0)Led_Flash(100); //===LEDÉÁ˸;³£¹æģʽ 1s¸Ä±äÒ»´ÎָʾµÆµÄ״̬ if(Bi_zhang==1)Led_Flash(0); //===LEDÉÁ˸;±ÜÕÏģʽ ָʾµÆ³£ÁÁ // Key(); //===ɨÃè°´¼ü״̬ µ¥»÷Ë«»÷¿ÉÒԸıäС³µÔËÐÐ״̬ Balance_Pwm =balance(Angle_Balance,Gyro_Balance); //===ƽºâPID¿ØÖÆ Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===ËٶȻ·PID¿ØÖÆ ¼Çס£¬Ëٶȷ´À¡ÊÇÕý·´À¡£¬¾ÍÊÇС³µ¿ìµÄʱºòÒªÂýÏÂÀ´¾ÍÐèÒªÔÙÅÜ¿ìÒ»µã Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===תÏò»·PID¿ØÖÆ Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; //===¼ÆËã×óÂÖµç»ú×îÖÕPWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; //===¼ÆËãÓÒÂÖµç»ú×îÖÕPWM Xianfu_Pwm(); //===PWMÏÞ·ù // if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===¼ì²éÊÇ·ñС³µ±»ÄÇÆð // Flag_Stop=1; //===Èç¹û±»ÄÃÆð¾Í¹Ø±Õµç»ú // if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right)) //===¼ì²éÊÇ·ñС³µ±»·ÅÏ // Flag_Stop=0; //===Èç¹û±»·ÅϾÍÆô¶¯µç»ú if(Turn_Off(Angle_Balance,Voltage)==0) //===Èç¹û²»´æÔÚÒì³£ Set_Pwm(Moto1,Moto2); //===¸³Öµ¸øPWM¼Ä´æÆ÷ } return 0; } /************************************************************************** º¯Êý¹¦ÄÜ£ºÖ±Á¢PD¿ØÖÆ Èë¿Ú²ÎÊý£º½Ç¶È¡¢½ÇËÙ¶È ·µ»Ø Öµ£ºÖ±Á¢¿ØÖÆPWM ×÷ ÕߣºÆ½ºâС³µÖ®¼Ò **************************************************************************/ int balance(float Angle,float Gyro) { float Bias; int balance; Bias=Angle-ZHONGZHI; //===Çó³öƽºâµÄ½Ç¶ÈÖÐÖµ ºÍ»úеÏà¹Ø balance=Balance_Kp*Bias+Gyro*Balance_Kd; //===¼ÆËãƽºâ¿ØÖƵĵç»úPWM PD¿ØÖÆ kpÊÇPϵÊý kdÊÇDϵÊý return balance; } /************************************************************************** º¯Êý¹¦ÄÜ£ºËÙ¶ÈPI¿ØÖÆ ÐÞ¸ÄÇ°½øºóÍËËٶȣ¬ÇëÐÞTarget_Velocity£¬±ÈÈ磬¸Ä³É60¾Í±È½ÏÂýÁË Èë¿Ú²ÎÊý£º×óÂÖ±àÂëÆ÷¡¢ÓÒÂÖ±àÂëÆ÷ ·µ»Ø Öµ£ºËٶȿØÖÆPWM ×÷ ÕߣºÆ½ºâС³µÖ®¼Ò **************************************************************************/ int velocity(int encoder_left,int encoder_right) { static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral,Target_Velocity; //=============Ò£¿ØÇ°½øºóÍ˲¿·Ö=======================// if(Bi_zhang==1&&Flag_sudu==1) Target_Velocity=45; //Èç¹û½øÈë±ÜÕÏģʽ,×Ô¶¯½øÈëµÍËÙģʽ else Target_Velocity=110; if(1==Flag_Qian) Movement=Target_Velocity/Flag_sudu; //===Ç°½ø±ê־λÖÃ1 else if(1==Flag_Hou) Movement=-Target_Velocity/Flag_sudu; //===ºóÍ˱ê־λÖÃ1 else Movement=0; if(Bi_zhang==1&&Distance<500&&Flag_Left!=1&&Flag_Right!=1) //±ÜÕϱê־λÖÃ1ÇÒ·ÇÒ£¿ØתÍäµÄʱºò£¬½øÈë±ÜÕÏģʽ Movement=-Target_Velocity/Flag_sudu; //=============ËÙ¶ÈPI¿ØÖÆÆ÷=======================// Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===»ñÈ¡×îÐÂËÙ¶ÈÆ«²î==²âÁ¿Ëٶȣ¨×óÓÒ±àÂëÆ÷Ö®ºÍ£©-Ä¿±êËٶȣ¨´Ë´¦ÎªÁ㣩 Encoder *= 0.8; //===Ò»½×µÍͨÂ˲¨Æ÷ Encoder += Encoder_Least*0.2; //===Ò»½×µÍͨÂ˲¨Æ÷ Encoder_Integral +=Encoder; //===»ý·Ö³öλÒÆ »ý·Öʱ¼ä£º10ms Encoder_Integral=Encoder_Integral-Movement; //===½ÓÊÕÒ£¿ØÆ÷Êý¾Ý£¬¿ØÖÆÇ°½øºóÍË if(Encoder_Integral>10000) Encoder_Integral=10000; //===»ý·ÖÏÞ·ù if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===»ý·ÖÏÞ·ù Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki; //===ËٶȿØÖÆ if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1) Encoder_Integral=0; //===µç»ú¹Ø±ÕºóÇå³ý»ý·Ö return Velocity; } /************************************************************************** º¯Êý¹¦ÄÜ£º×ªÏò¿ØÖÆ ÐÞ¸ÄתÏòËٶȣ¬ÇëÐÞ¸ÄTurn_Amplitude¼´¿É Èë¿Ú²ÎÊý£º×óÂÖ±àÂëÆ÷¡¢ÓÒÂÖ±àÂëÆ÷¡¢ZÖáÍÓÂÝÒÇ ·µ»Ø Öµ£º×ªÏò¿ØÖÆPWM ×÷ ÕߣºÆ½ºâС³µÖ®¼Ò **************************************************************************/ int turn(int encoder_left,int encoder_right,float gyro)//תÏò¿ØÖÆ { static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=0; //=============Ò£¿Ø×óÓÒÐýת²¿·Ö=======================// if(1==Flag_Left||1==Flag_Right) //ÕâÒ»²¿·ÖÖ÷ÒªÊǸù¾ÝÐýתǰµÄËٶȵ÷ÕûËٶȵÄÆðʼËٶȣ¬Ôö¼ÓС³µµÄÊÊÓ¦ÐÔ { if(++Turn_Count==1) Encoder_temp=myabs(encoder_left+encoder_right); Turn_Convert=50/Encoder_temp; if(Turn_Convert<0.6)Turn_Convert=0.6; if(Turn_Convert>3)Turn_Convert=3; } else { Turn_Convert=0.9; Turn_Count=0; Encoder_temp=0; } if(1==Flag_Left) Turn_Target-=Turn_Convert; else if(1==Flag_Right) Turn_Target+=Turn_Convert; else Turn_Target=0; if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //===תÏòËÙ¶ÈÏÞ·ù if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude; if(Flag_Qian==1||Flag_Hou==1) Kd=0.5; else Kd=0; //תÏòµÄʱºòÈ¡ÏûÍÓÂÝÒǵľÀÕý ÓеãÄ£ºýPIDµÄ˼Ïë //=============תÏòPD¿ØÖÆÆ÷=======================// Turn=-Turn_Target*Kp -gyro*Kd; //===½áºÏZÖáÍÓÂÝÒǽøÐÐPD¿ØÖÆ return Turn; } /************************************************************************** º¯Êý¹¦ÄÜ£º¸³Öµ¸øPWM¼Ä´æÆ÷ Èë¿Ú²ÎÊý£º×óÂÖPWM¡¢ÓÒÂÖPWM ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Set_Pwm(int moto1,int moto2) { if(moto1<0) AIN2=1, AIN1=0; else AIN2=0, AIN1=1; PWMA=myabs(moto1); if(moto2<0) BIN1=0, BIN2=1; else BIN1=1, BIN2=0; PWMB=myabs(moto2); } /************************************************************************** º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Xianfu_Pwm(void) { int Amplitude=6900; //===PWMÂú·ùÊÇ7200 ÏÞÖÆÔÚ6900 if(Flag_Qian==1) Moto1-=DIFFERENCE; //DIFFERENCEÊÇÒ»¸öºâÁ¿Æ½ºâС³µµç»úºÍ»úе°²×°²îÒìµÄÒ»¸ö±äÁ¿¡£Ö±½Ó×÷ÓÃÓÚÊä³ö£¬ÈÃС³µ¾ßÓиüºÃµÄÒ»ÖÂÐÔ¡£ if(Flag_Hou==1) Moto2+=DIFFERENCE; if(Moto1<-Amplitude) Moto1=-Amplitude; if(Moto1>Amplitude) Moto1=Amplitude; if(Moto2<-Amplitude) Moto2=-Amplitude; if(Moto2>Amplitude) Moto2=Amplitude; } /************************************************************************** º¯Êý¹¦ÄÜ£º°´¼üÐÞ¸ÄС³µÔËÐÐ״̬ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ /*void Key(void) { u8 tmp,tmp2; tmp=click_N_Double(50); if(tmp==1)Flag_Stop=!Flag_Stop;//µ¥»÷¿ØÖÆС³µµÄÆôÍ£ if(tmp==2)Flag_Show=!Flag_Show;//Ë«»÷¿ØÖÆС³µµÄÏÔʾ״̬ tmp2=Long_Press(); if(tmp2==1) Bi_zhang=!Bi_zhang; //³¤°´¿ØÖÆС³µÊÇ·ñ½øÈ볬Éù²¨±ÜÕÏģʽ }*/ /************************************************************************** º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú Èë¿Ú²ÎÊý£ºÇã½ÇºÍµçѹ ·µ»Ø Öµ£º1£ºÒì³£ 0£ºÕý³£ **************************************************************************/ u8 Turn_Off(float angle, int voltage) { u8 temp; if(angle<-40||angle>40||1==Flag_Stop||voltage<1110)//µç³ØµçѹµÍÓÚ11.1V¹Ø±Õµç»ú { //===Çã½Ç´óÓÚ40¶È¹Ø±Õµç»ú temp=0; //===Flag_StopÖÃ1¹Ø±Õµç»ú // AIN1=0; // AIN2=1; // BIN1=0; // BIN2=1; } else temp=0; return temp; } /************************************************************************** º¯Êý¹¦ÄÜ£º»ñÈ¡½Ç¶È ÈýÖÖËã·¨¾¹ýÎÒÃǵĵ÷У£¬¶¼·Ç³£ÀíÏë Èë¿Ú²ÎÊý£º»ñÈ¡½Ç¶ÈµÄËã·¨ 1£ºDMP 2£º¿¨¶ûÂü 3£º»¥²¹Â˲¨ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Angle(u8 way) { float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z; Temperature=Read_Temperature(); //===¶ÁÈ¡MPU6050ÄÚÖÃζȴ«¸ÐÆ÷Êý¾Ý£¬½üËƱíʾÖ÷°åζȡ£ if(way==1) //===DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý²É¼¯ÖжÏÌáÐѵÄʱºò£¬Ñϸñ×ñÑʱÐòÒªÇó { Read_DMP(); //===¶ÁÈ¡¼ÓËٶȡ¢½ÇËٶȡ¢Çã½Ç Angle_Balance=Pitch; //===¸üÐÂƽºâÇã½Ç Gyro_Balance=gyro[1]; //===¸üÐÂƽºâ½ÇËÙ¶È Gyro_Turn=gyro[2]; //===¸üÐÂתÏò½ÇËÙ¶È Acceleration_Z=accel[2]; //===¸üÐÂZÖá¼ÓËÙ¶È¼Æ } else { Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //¶ÁÈ¡YÖáÍÓÂÝÒÇ Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //¶ÁÈ¡ZÖáÍÓÂÝÒÇ Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //¶ÁÈ¡ZÖá¼ÓËÙ¶È¼Æ if(Gyro_Y>32768) Gyro_Y-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» if(Gyro_Z>32768) Gyro_Z-=65536; //Êý¾ÝÀàÐÍת»» if(Accel_X>32768) Accel_X-=65536; //Êý¾ÝÀàÐÍת»» if(Accel_Z>32768) Accel_Z-=65536; //Êý¾ÝÀàÐÍת»» Gyro_Balance=-Gyro_Y; //¸üÐÂƽºâ½ÇËÙ¶È Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //¼ÆËãÇã½Ç Gyro_Y=Gyro_Y/16.4; //ÍÓÂÝÒÇÁ¿³Ìת»» if(Way_Angle==2) Kalman_Filter(Accel_Y,-Gyro_Y);//¿¨¶ûÂüÂ˲¨ else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //»¥²¹Â˲¨ Angle_Balance=angle; //¸üÐÂƽºâÇã½Ç Gyro_Turn=Gyro_Z; //¸üÐÂתÏò½ÇËÙ¶È Acceleration_Z=Accel_Z; //===¸üÐÂZÖá¼ÓËÙ¶È¼Æ } } /************************************************************************** º¯Êý¹¦ÄÜ£º¾ø¶ÔÖµº¯Êý Èë¿Ú²ÎÊý£ºint ·µ»Ø Öµ£ºunsigned int **************************************************************************/ int myabs(int a) { int temp; if(a<0) temp=-a; else temp=a; return temp; } /************************************************************************** º¯Êý¹¦ÄÜ£º¼ì²âС³µÊÇ·ñ±»ÄÃÆð Èë¿Ú²ÎÊý£ºint ·µ»Ø Öµ£ºunsigned int **************************************************************************/ //int Pick_Up(float Acceleration,float Angle,int encoder_left,int encoder_right) //{ // static u16 flag,count0,count1,count2; // if(flag==0) //µÚÒ»²½ // { // if(myabs(encoder_left)+myabs(encoder_right)<30) //Ìõ¼þ1£¬Ð¡³µ½Ó½ü¾²Ö¹ // count0++; // else // count0=0; // if(count0>10) // flag=1,count0=0; // } // if(flag==1) //½øÈëµÚ¶þ²½ // { // if(++count1>200) count1=0,flag=0; //³¬Ê±²»Ôٵȴý2000ms // if(Acceleration>26000&&(Angle>(-20+ZHONGZHI))&&(Angle<(20+ZHONGZHI))) //Ìõ¼þ2£¬Ð¡³µÊÇÔÚ0¶È¸½½ü±»ÄÃÆð // flag=2; // } // if(flag==2) //µÚÈý²½ // { // if(++count2>100) count2=0,flag=0; //³¬Ê±²»Ôٵȴý1000ms // if(myabs(encoder_left+encoder_right)>135) //Ìõ¼þ3£¬Ð¡³µµÄÂÖÌ¥ÒòΪÕý·´À¡´ïµ½×î´óµÄתËÙ // { // flag=0; // return 1; //¼ì²âµ½Ð¡³µ±»ÄÃÆð // } // } // return 0; //} /************************************************************************** º¯Êý¹¦ÄÜ£º¼ì²âС³µÊÇ·ñ±»·ÅÏ Èë¿Ú²ÎÊý£ºint ·µ»Ø Öµ£ºunsigned int **************************************************************************/ //int Put_Down(float Angle,int encoder_left,int encoder_right) //{ // static u16 flag,count; // if(Flag_Stop==0) //·ÀÖ¹Îó¼ì // return 0; // if(flag==0) // { // if(Angle>(-10+ZHONGZHI)&&Angle<(10+ZHONGZHI)&&encoder_left==0&&encoder_right==0) //Ìõ¼þ1£¬Ð¡³µÊÇÔÚ0¶È¸½½üµÄ // flag=1; // } // if(flag==1) // { // if(++count>50) //³¬Ê±²»Ôٵȴý 500ms // { // count=0;flag=0; // } // if(encoder_left>3&&encoder_right>3&&encoder_left<60&&encoder_right<60) //Ìõ¼þ2£¬Ð¡³µµÄÂÖÌ¥ÔÚδÉϵçµÄʱºò±»ÈËΪת¶¯ // { // flag=0; // flag=0; // return 1; //¼ì²âµ½Ð¡³µ±»·ÅÏ // } // } // return 0; //}