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