最近在玩倒立摆项目，硬件是在平衡小车之家上直接买回来的，用的主控板是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;
}
|
