STM32自平衡方块程序，分享资料

ID:505762 · 发表于 2020-4-16 22:42

自平衡方块，拿去自学

/*
知乎：天下行走
CSDN：技术创造无限可能
闲鱼：抉择之刃银
*/
/*
针对角度环的期望角度为速度环输出+机械中值的提问：
【一】
问：
当我在小车（平衡小车）前侧绑一块小铁块，这会使得机械中值后倾，=-27度，然后我将车子扶在机械中值的角度位置然后开机，但车子一开机就往前跑。为什么？
答：
问题分析：
起始速度偏差=0（因为没有动轮子呀）——>外环输出=0（即内环期望角度=0）
But：开局实际角度= -27度——>开局内环输出就等于(-27-0)*(-1) = +27 //Kp为负数，这里设为-1
即根本是因为开局时外环输出！=机械中值(这里机械中值可以等效说成实际角度，但不是任何时候都可以等效说成的。譬如我开机时小车不摆在机械中值位置时就不能等效说成。)
=====>>>>所以开局时车子肯定往前跑！！！
解决思想：
起始Target=Real就可以了。
解决方法：
测机械中值譬如 = -27度，则使得内环期望角度=外环输出 + 机械中值。
【二】
若：外环输出==机械中值，即开局速度偏差=0时，机械中值=0则外环输出=0，即内环输入=0。
又有内环反馈=0，所以这时候开局内环输出就等于(0-0)*(-1) = 0===>车子必然直立静止在原地。
【三】
若：外环输出！=机械中值，即开局速度偏差=0时，机械中值!=0，但我开局不把车子摆在机械中值位置。譬如举例当机械中值=-27度，我开机将车子实际角度摆在0度位置。则此时
速度偏差=0则外环输出=0，即内环输入=0。
又有内环反馈=0===>理论上小车直立静止，BUT！车子根本不在机械中值位置，又谈何直立静止，车子会根据重心向前倒下，直立环立刻发挥作用-->
-->若小铁快非常非常轻，还有可能救回来；若铁块不轻，则必然车子一直不断向前跑不会停下；若小铁块过重，系统来不及响应，车子会直接向前倒下；
这，就是结果。
*/
/*
【硬件占用的GPIO的框架】
编码器 ||AB相—PA0/PA1
电机 ||PWM——PA8/PA11、PA6/PA7
MPU6050 ||INT——PB5
MPU6050 ||IIC——SCL--PB4/SDA--PB3
OLED ||IIC——SCL--PB8/SDA--PB9
*/
#include "stm32f10x.h"
#include "sys.h"
float Pitch,Roll,Yaw; //角度
short gyrox,gyroy,gyroz; //陀螺仪--角速度
short aacx,aacy,aacz; //加速度
int Encoder_Momentum; //编码器数据（速度）
int PWM_MAX=7000,PWM_MIN=-7000; //PWM限幅变量
int MOTO1,MOTO2; //电机装载变量
extern int Velocity_Momentum_out; //输出变量
int main(void)
{
delay_init();
NVIC_Config();
uart1_init(115200);
OLED_Init();
OLED_Clear();
MPU_Init();
mpu_dmp_init();
MPU6050_EXTI_Init();
Encoder_TIM2_Init();
Encoder_TIM4_Init();
PWM_Init_TIM1(0,7199); //CH1——PA8、CH4——PA11
PWM_Init_TIM3(0,7199); //CH1——PA6、CH2——PA7
OLED_Clear();
OLED_ShowString(5,0,"P",12);
OLED_ShowChar(5,2,'M',12);
OLED_ShowString(64,2,"Stop:",12);
while(1)
{
OLED_Float(0,15,Pitch,1); //角度
OLED_Float(0,70,Velocity_Momentum_out,1); //外环输出
OLED_Float(2,5+15,Encoder_Momentum,1); //速度读取
OLED_Float(3,5,MOTO1,1 ); //电机PWM
OLED_ShowNumber(64+40,2,stop,1,12); //停止位
}
}

复制代码

帐号		自动登录	找回密码
密码			立即注册

STM32自平衡方块程序，分享资料

评分