|
|
最近在玩倒立擺項(xiàng)目，硬件是在平衡小車(chē)之家上直接買(mǎi)回來(lái)的，用的主控板是stm32f103，做到那個(gè)自動(dòng)起擺的時(shí)候，就出現(xiàn)問(wèn)題了，用的是店家給的程序，起擺的時(shí)候特別不穩(wěn)定，就是他能夠擺上來(lái)，但是不到一秒鐘就擺下去了，然后整個(gè)電機(jī)都停止運(yùn)轉(zhuǎn)，起先覺(jué)得是那個(gè)倒立平衡（就是串口服務(wù)函數(shù)）那里面判斷300ms的時(shí)間太長(zhǎng)，所以把他改小了，但是也沒(méi)用，后來(lái)覺(jué)得是擺上去的時(shí)候慣性太大了，觸碰到傾角保護(hù)了（ if(Turn_Off(Voltage)==0) //===低壓和傾角過(guò)大保護(hù)），就是這個(gè)函數(shù)，然后把他注釋掉，擺上去的時(shí)候，整個(gè)電機(jī)確實(shí)繼續(xù)運(yùn)行，但是那個(gè)擺還是堅(jiān)持不到一秒鐘，然后整個(gè)硬件一直在做無(wú)敵風(fēng)火輪的那種，隨便轉(zhuǎn)，還是達(dá)不到要求，現(xiàn)在樓樓很頭疼，不知道是哪里出現(xiàn)了問(wèn)題，自己功底還是太差了，請(qǐng)求大佬指點(diǎn)下。（對(duì)哦，還有呢，就是我在程序的每一步都加了printf函數(shù)看看哪一步出現(xiàn)了問(wèn)題，就是在自動(dòng)起擺的步驟都運(yùn)行成功了，然后就到了倒立平衡的函數(shù)，也就是串口服務(wù)函數(shù)，發(fā)現(xiàn)進(jìn)入之后，很快就跳出來(lái)了）
/////////////////////////自動(dòng)起擺
if(Flag_qb==0) //第1步，采樣
{
printf("hehe");
if(Target_Position<13120&&Flag_Back==0) Target_Position+=2, Ratio=0.01;
if(Encoder>12000)Flag_Back=1;
if(Flag_Back==1)
{
Ratio=0.1; //使用較小的PID參數(shù)
Target_Position=Position_Max;//回到之前讀取的角位移傳感器數(shù)據(jù)最大的地方
if(Count_Next++>600) //3s之后 進(jìn)入下一步
{
Flag_qb++;
TIM2->CNT=10000;
Flag_Back=0;
}
}
Encoder=Read_Encoder(4); //===更新編碼器位置信息
Moto_qb=Position_PID(Encoder,Target_Position); //位置閉環(huán)控制
if(Moto_qb>1200)Moto_qb=1200; //控制位置閉環(huán)控制過(guò)程的速度
if(Moto_qb<-1200)Moto_qb=-1200;
Set_Pwm(Moto_qb); //賦值給PWM寄存器
if(Encoder>10260&&Encoder<11300&&D_Angle_Balance==0&&Flag_Back==0)//采樣相關(guān)的初始值
{
if(Angle_Balance>Angle_Max) Angle_Max=Angle_Balance,Position_Max=Encoder;
}
Data_Show2=Position_Max;//賦值到全局變量顯示
Data_Show=Angle_Max;
}
if(Flag_qb==1) //第2步，擺桿自由擺動(dòng)，振幅越來(lái)越大
{
printf("xixi");
Ratio=2; //正常的PID參數(shù)
Count_qb+=Count_Big_Angle; //自變量
Count_Big_Angle-=0.0000027; //振幅越大，擺動(dòng)周期略微減小
Count_FZ+=0.025; //振幅越來(lái)越大
Target_Position=0.6*Count_FZ*sin(Count_qb)+10000; //運(yùn)動(dòng)公式
Encoder=Read_Encoder(4); //===更新編碼器位置信息
Moto_qb=Position_PID(Encoder,Target_Position); //位置閉環(huán)控制
if(Moto_qb>7200)Moto_qb=7200;//控制位置閉環(huán)控制過(guò)程的速度
if(Moto_qb<-7200)Moto_qb=-7200;//控制位置閉環(huán)控制過(guò)程的速度
if(Angle_Balance>(Angle_Max+710)&&Angle_Balance<2100&&D_Angle_Balance<=-1) //振幅大于閾值時(shí)，進(jìn)入下一步
{
Flag_qb++;
Count_qb=0;
TIM2->CNT=10000; //復(fù)位一下計(jì)數(shù)寄存器
Count_FZ=0;
}
Set_Pwm(Moto_qb); //賦值給PWM寄存器
}
if(Flag_qb==2) //第3步，通過(guò)位置控制，利用慣性，自動(dòng)起擺
{
printf("enen");
Target_Position=10400; //設(shè)定目標(biāo)值
Encoder=Read_Encoder(4); //===更新編碼器位置信息
Moto_qb=Position_PID(Encoder,Target_Position);//===位置PID控制器
if(Moto_qb>7200) Moto_qb=7200;
if(Moto_qb<-7200)Moto_qb=-7200;
Set_Pwm(Moto_qb); //賦值給PWM寄存器
if(Angle_Balance<(ZHONGZHI+400)&&Angle_Balance>(ZHONGZHI-400)) //到底接近平衡位置 即可開(kāi)啟平衡系統(tǒng)
{
State=1; //倒立狀態(tài)置1
Way_Turn=0;//自動(dòng)起擺標(biāo)志位清零
Flag_qb=0; //自動(dòng)起擺步驟清零
Angle_Max=0;
}
}
}
/////////////////////////////////////////////////////
int TIM1_UP_IRQHandler(void)
{
if(TIM1->SR&0X0001)//5ms定時(shí)中斷
{
TIM1->SR&=~(1<<0); //===清除定時(shí)器1中斷標(biāo)志位
if(delay_flag==1)
{
if(++delay_50==10) delay_50=0,delay_flag=0; //===給主函數(shù)提供50ms的精準(zhǔn)延時(shí)
}
Angle_Balance=Get_Adc_Average(3,15); //===更新姿態(tài)
D_Angle_Balance= Angle_Balance-Last_Angle_Balance ; //===獲取微分值
if(State==0) Run(Way_Turn);//起擺 由入口參數(shù)控制 1：自動(dòng)起擺 2：手動(dòng)起擺
if(State==1) //起擺成功之后，進(jìn)行倒立控制
{
Balance_Pwm =balance(Angle_Balance); //開(kāi)啟平衡控制
if(Flag_qb2==0) //位置控制延時(shí)啟動(dòng)
{
printf("lolo");
if(Angle_Balance<(ZHONGZHI+300)&&Angle_Balance>(ZHONGZHI-200))Count_Position++; //
if(Count_Position>1)Flag_qb2=1, Count_Position=0,TIM2->CNT=10000; //在平衡位置倒立超過(guò)300ms 開(kāi)啟位置控制
}
if(Flag_qb2==1) //開(kāi)啟位置控制
{
printf("qiqo");
Encoder=Read_Encoder(4); //===更新編碼器位置信息
if(++Count_P2>4) Position_Pwm=Position(Encoder),Count_P2=0; //===位置PD控制 25ms進(jìn)行一次位置控制
}
printf("sdsd");
Moto=Balance_Pwm-Position_Pwm; //===計(jì)算電機(jī)最終PWM
Xianfu_Pwm(); //===PWM限幅
if(Turn_Off(Voltage)==0) //===低壓和傾角過(guò)大保護(hù)
Set_Pwm(Moto); //===賦值給PWM寄存器
}
//==========以下是一些時(shí)序要求不嚴(yán)格的，在中斷的最后執(zhí)行===============//
Led_Flash(100); //===LED閃爍指示系統(tǒng)正常運(yùn)行
Voltage_Temp=Get_battery_volt(); //=====讀取電池電壓
Voltage_Count++; //=====平均值計(jì)數(shù)器
Voltage_All+=Voltage_Temp; //=====多次采樣累積
if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====求平均值
Key(); //===掃描按鍵變化
Last_Angle_Balance=Angle_Balance; //===保存上一次的傾角值
}
return 0;
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
