张家港网站建设制作,互联网外包公司,企业网站服务器选择,下载 wordpress插件怎么用目录 一.所需材料
二.PID算法#xff08;简单说明#xff09;
直立环
速度环
串级PID
三.使用到的外设 1.定时器输出比较-PWM
2.定时器编码器模式
3.编码器读取速度
4.电机驱动函数
5.外部中断
四、小车 调试 一.所需材料
1.陀螺仪MPU6050--读取三轴的加速度…目录 一.所需材料
二.PID算法简单说明
直立环
速度环
串级PID
三.使用到的外设 1.定时器输出比较-PWM
2.定时器编码器模式
3.编码器读取速度
4.电机驱动函数
5.外部中断
四、小车 调试 一.所需材料
1.陀螺仪MPU6050--读取三轴的加速度和角速度然而我们平衡车需要的不是加速度和角速度而是需要实时的角度即我们还需要进行姿态角转换可以直接移植正点原子的DMP函数
2.TB6612电机驱动l298N也可我用的是TB6612点击查看接线 3.OLED用来显示姿态角可以直接移植
4.12v锂电池和DCDC降压模块给电机和单片机供电
5.编码电机点击查看接线
6.c8t6单片机
二.PID算法简单说明 P比例算法Kp*误差。 可以看出误差越大P的输出就越大所以P的作用是减小误差但是只有P算法会发生超调现象即在不受外力下可以看作简谐运动一直震荡 I积分算法Ki*误差的积分。 可以看出只要存在误差一般叫做稳态误差比如小车在平衡位置偏左或右一点点但是又不会倒下等等的情况下产生的角度误差就会一直累加累加累加的总误差就变的很大滚雪球一样越来越大所以I可以消除稳态误差 D微分Kd*误差的微分。 可以看出两次误差之差反映的是系统的反应速度响应越快误差之差越大D输出很大反之很小所以D算法会抑制过冲消弱系统的反应速度。例如简谐运动以最下点为期望点小球从空中开始速度很小后面越来越快即可以理解为误差微分越来越大且为负的所以D输出就越来越大力的方向与小球运动方向相反若没有D算法小球就会继续简谐运动若此时有了D算法小球的运动就会收到阻碍而令小球到达的高度降低长此以往小球就会静止在最低点 也可以这样看在水中挥拳挥的速度越大受到的阻尼越大 直立环 PWM_OUTKp*误差Kd*误差的微分 误差真实角度-期望角度 误差的微分角速度姿态角转换得到 作用让小车保持短时间的平衡但是推一下就可能倒下 /*
直立环
PWM_OUT Kp*角度偏差Kd*角度偏差的微分
角度偏差求微分就是对角度偏差求导即为角速度
gyro_Y俯仰角的角速度
*/int Vertical(float Expect_Angle,float Angle,float gyro_Y)
{int PWM_out10;PWM_out1 Vertical_Kp*(Expect_Angle-Angle)Vertical_Kd*(gyro_Y-0);return PWM_out1;
}
/* 速度环 PWM_OUTKp*误差Ki*误差的积分 误差期望速度-真实速度 误差的积分误差累加 作用消除稳态误差加强系统的反应速度 极性问题 速度环在平衡一类的基本上都是用正反馈即积分误差放大误差先注释掉直立环则极性正确的现象就是转动一下轮子立刻就会加速到最大因为我们速度理论值是0我们转了一下产生了误差速度环不断放大误差使速度环输出越大就会让轮子不断加速到最大速度若是负反馈我们很难转动轮子 int Velocity(int Target,int left,int right)
{ static int Encoder_S,EnC_Err_Lowout_last,PWM_out3,Encoder_Err,EnC_Err_Lowout;float a 0.7;Encoder_Err leftright-Target;//速度偏差//对速度偏差进行一阶低通滤波EnC_Err_Lowout a*EnC_Err_Lowout_last(1-a)*Encoder_Err;EnC_Err_Lowout_lastEnC_Err_Lowout;//积分Encoder_SEnC_Err_Lowout;//限幅Encoder_SI_Limit(Encoder_S);PWM_out3 Velocity_Kp*EnC_Err_Lowout Velocity_Ki*Encoder_S;return PWM_out3;
}int I_Limit(int Encoder_S)
{if(Encoder_S10000)Encoder_S10000;else if(Encoder_S-10000)Encoder_S-10000;else Encoder_SEncoder_S;return Encoder_S;
}速度环必须滤波因为速度会突变突变的速度会对系统产生很大影响 滤波编码器测到的数据速度是存在一些突变的这些突变会导致系统的不稳定所以我们要滤除这些突变这就是滤波类似于模电里面的“滤除电路中的高频信号” 一阶低通滤波把权重给到上次滤波后的速度另一个这次的速度为1-a倍0.5a1 积分限幅误差的积分在一直累加若没有限幅积分会越来越大举个例子小车倒下了我们为了保护电机而关掉了电机但是单片机的电源并没有关掉误差一直在累加很大很大了当我们把小车放好并打开电机后由于I的作用根本无法平衡。所以要限幅。 串级PID 串级顾名思义就是一个环的输出作为另一个环的输入例如速度环的输出作为直立环的输入或者直立环的输出作为速度环的输入 速度环输入1.给定速度。2.速度反馈。 输出角度值直立环的期望速度输入 直立环输入1.给定角度速度环输出。2.角度反馈 输出PWM直接控制小车 Vertical_outKp1*(Angle-Expect_Angle)Kd* gyro_y 直立环输出Kp1*真实角度-期望角度机械中值Kd*角度偏差的微分 Velocity_out Kp2*(Encoder_ real- Encoder_ expect)Ki*(Encoder_ real- Encoder_ expect)的积分 速度环输出Kp2*反馈编码器值-期望编码器值Ki*编码器偏差的积分 合并推导Expect_Angle Velocity_out Vertical_out Kp1*{ Angle-[ Kp2*(Encoder_ real- Encoder_ expect)Ki*Σ(Encoder_ real- Encoder_ expect) ]}Kd*gyro_y Kp1*真实角度 Kd*角度偏差的微分-Kp1* [Kp2*编码器偏差- Ki *编码器偏差的积分] 三.使用到的外设 1.定时器输出比较-PWM
void PWM_Init_TIM1(uint16_t Psc,uint16_t Per)
{//开时钟,AFIORCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);GPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode GPIO_Mode_AF_PP;//复用推挽GPIO_InitStruct.GPIO_Pin GPIO_Pin_8 | GPIO_Pin_11;GPIO_InitStruct.GPIO_Speed GPIO_Speed_50MHz;GPIO_Init(GPIOA,GPIO_InitStruct);TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_TimeBaseStructInit(TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置所以这里先初始化一下TIM_TimeBaseInitStruct.TIM_ClockDivision TIM_CKD_DIV1;//不分频----滤波器的采样频率可以由内部时钟直接提供//也可以由内部时钟加一个时钟分频而来//分频系数就是由TIM_ClockDivision决定TIM_TimeBaseInitStruct.TIM_CounterMode TIM_CounterMode_Up;//向上计数TIM_TimeBaseInitStruct.TIM_Period Per;//ARR的值TIM_TimeBaseInitStruct.TIM_Prescaler Psc;//PSC的值//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器这里不需要TIM_TimeBaseInit(TIM1,TIM_TimeBaseInitStruct);TIM_OCInitTypeDef TIM_OCInitStruct;TIM_OCStructInit(TIM_OCInitStruct);//初始化输出比较结构体TIM_OCInitStruct.TIM_OCMode TIM_OCMode_PWM1;//输出比较模式 TIM_OCInitStruct.TIM_OCPolarity TIM_OCPolarity_High; //极性有效电平为高电平TIM_OCInitStruct.TIM_OutputState TIM_OutputState_Enable;//输出使能TIM_OCInitStruct.TIM_Pulse 0;//CCRTIM_OC1Init(TIM1,TIM_OCInitStruct);TIM_OC4Init(TIM1,TIM_OCInitStruct);TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器特有的必须使能//使能ARR影子寄存器TIM_ARRPreloadConfig(TIM1,ENABLE);//使能输出比较预装载值寄存器TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);TIM_Cmd(TIM1,ENABLE);
}
2.定时器编码器模式
//配置编码器
//编码器1-PA0/PA1-TIM2
//编码器2-PB6/PB7-TIM4
void Encoder_TIM2_Init()
{//开启GPIO时钟RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启定时器时钟RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);//初始化GPIOGPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode GPIO_Mode_IN_FLOATING;GPIO_InitStruct.GPIO_Pin GPIO_Pin_0 | GPIO_Pin_1;GPIO_Init(GPIOA,GPIO_InitStruct);//初始化定时器 TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_TimeBaseStructInit(TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置所以这里先初始化一下TIM_TimeBaseInitStruct.TIM_ClockDivision TIM_CKD_DIV1;//滤波分频系数TIM_TimeBaseInitStruct.TIM_CounterMode TIM_CounterMode_Up;//向上计数TIM_TimeBaseInitStruct.TIM_Period 65535;//ARR的值TIM_TimeBaseInitStruct.TIM_Prescaler 0;//PSC的值//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器这里不需要TIM_TimeBaseInit(TIM2,TIM_TimeBaseInitStruct);//编码器特有的配置函数,配置模式TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//初始化输入捕获TIM_ICInitTypeDef TIM_ICInitStruct;TIM_ICStructInit(TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置所以这里先初始化一下TIM_ICInitStruct.TIM_ICFilter 10;// 滤波器范围是 0x0 and 0xFTIM_ICInit(TIM2,TIM_ICInitStruct);//清除定时器溢出中断标志位TIM_ClearFlag(TIM2,TIM_FLAG_Update);//配置溢出中断TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//设置计数器初始值为0TIM_SetCounter(TIM2,0);//开启定时器TIM_Cmd(TIM2,ENABLE);
}void Encoder_TIM4_Init()
{//开启GPIO时钟RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启定时器时钟RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//初始化GPIOGPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode GPIO_Mode_IN_FLOATING;GPIO_InitStruct.GPIO_Pin GPIO_Pin_6 | GPIO_Pin_7;GPIO_Init(GPIOB,GPIO_InitStruct);//初始化定时器 TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_TimeBaseStructInit(TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置所以这里先初始化一下TIM_TimeBaseInitStruct.TIM_ClockDivision TIM_CKD_DIV1;//外部时钟滤波分频系数TIM_TimeBaseInitStruct.TIM_CounterMode TIM_CounterMode_Up;//向上计数TIM_TimeBaseInitStruct.TIM_Period 65535;//ARR的值TIM_TimeBaseInitStruct.TIM_Prescaler 0;//PSC的值//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器这里不需要TIM_TimeBaseInit(TIM4,TIM_TimeBaseInitStruct);//编码器特有的配置函数TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置IC输入TIM_ICInitTypeDef TIM_ICInitStruct;TIM_ICStructInit(TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置所以这里先初始化一下TIM_ICInitStruct.TIM_ICFilter 10;// 滤波器TIM_ICInit(TIM4,TIM_ICInitStruct);//清除定时器溢出中断标志位TIM_ClearFlag(TIM4,TIM_FLAG_Update);//配置溢出中断TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);//设置计数器初始值为0TIM_SetCounter(TIM4,0);//开启定时器TIM_Cmd(TIM4,ENABLE);
}3.编码器读取速度
/*******************
编码器速度读取函数*入口参数定时器2/4
*******************/
int Read_Speed(int TIMx)
{int value_1;switch(TIMx){//int ? shortcase 2:value_1(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break; //先读取编码器的计数值然后清零计数值目的是直接获得速度而不用计数值相减来算速度case 4:value_1(short)TIM_GetCounter(TIM4);TIM_SetCounter(TIM4,0);break;default :value_1 0;}//这里TIM_GetCounter()函数的返回值是uint16_t ,而我们的返回值是有符号的int类型所以这里强制类型转化一下return value_1;
}4.电机驱动函数 //电机初始化函数
void Motor_Init()
{RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);GPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode GPIO_Mode_Out_PP;//复用推挽GPIO_InitStruct.GPIO_Pin GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15 ;GPIO_InitStruct.GPIO_Speed GPIO_Speed_50MHz;GPIO_Init(GPIOB,GPIO_InitStruct);
}//限幅函数
void Limit(int *motorA ,int *motorB)
{if(*motorA PWM_MAX)*motorAPWM_MAX;if(*motorA PWM_MIN)*motorAPWM_MIN;if(*motorB PWM_MAX)*motorBPWM_MAX;if(*motorB PWM_MIN)*motorBPWM_MIN;
}//绝对值函数
int abs(int p)
{int q;q p0?p:(-p);return q;
}//电机驱动函数
//入口参数PID运算完成后最终PWM值
void Load(int moto1,int moto2)
{//判断正反转if(moto10)Ain11,Ain20;else Ain10,Ain21;//输入PWM的绝对值TIM_SetCompare1(TIM1,abs(moto1));if(moto20)Bin11,Bin20;else Bin10,Bin21;TIM_SetCompare4(TIM1,abs(moto2));
}
//电机为10KHZ
//72000000/7200 10 000 HZ
#define PWM_MAX 7200
#define PWM_MIN -7200extern int MOTO1,MOTO2;
#define Ain1 PBout(14)
#define Ain2 PBout(15)#define Bin1 PBout(13)
#define Bin2 PBout(12)
5.外部中断 dmp读取是用的中断读取10ms读取一次所以我们要配置MPU6050的外部中断 void MPU6050_EXTI_Init(void)
{//开启时钟//因为是复用功能所以开启AFIORCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//配置GPIOGPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode GPIO_Mode_IPU;//GPIO_Mode_AF_PP;复用推挽GPIO_InitStruct.GPIO_Pin GPIO_Pin_5;GPIO_InitStruct.GPIO_Speed GPIO_Speed_50MHz;GPIO_Init(GPIOB,GPIO_InitStruct); //映射GPIO和外部中断GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//配置EXTI结构体EXTI_InitTypeDef EXTI_InitStruct;EXTI_InitStruct.EXTI_Line EXTI_Line5;//中断线EXTI_InitStruct.EXTI_LineCmd ENABLE;//使能中断线EXTI_InitStruct.EXTI_Mode EXTI_Mode_Interrupt;//中断模式EXTI_InitStruct.EXTI_Trigger EXTI_Trigger_Falling;//下降沿触发EXTI_Init(EXTI_InitStruct);}void NVIC_Config()
{NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//外部中断NVIC_InitTypeDef NVIC_InitStruct;NVIC_InitStruct.NVIC_IRQChannel EXTI9_5_IRQn;NVIC_InitStruct.NVIC_IRQChannelCmd ENABLE;NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority 0;NVIC_InitStruct.NVIC_IRQChannelSubPriority 0;NVIC_Init(NVIC_InitStruct);//串口NVIC_InitStruct.NVIC_IRQChannel USART1_IRQn;NVIC_InitStruct.NVIC_IRQChannelCmd ENABLE;NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority 0;NVIC_InitStruct.NVIC_IRQChannelSubPriority 1;NVIC_Init(NVIC_InitStruct);
}void EXTI9_5_IRQHandler(void)
写中断服务函数里面做的是PID算法
void EXTI9_5_IRQHandler(void)
{ if(EXTI_GetITStatus(EXTI_Line5) SET)//MPU6050外部中断是PB5{if(PBin(5) 0)//引脚定义为外部上拉而外部中断触发模式是下降沿这里就是再进行下降沿的检测{EXTI_ClearITPendingBit(EXTI_Line5);//1.采集编码器数据和MPU6050的角度信息Encoder_Left -Read_Speed(2);//因为电机是相对安装的即左右电机相差180度为了让编码器输出极性一致Encoder_Right Read_Speed(4);//所以需要取反或者调转一下极性也可以mpu_dmp_get_data(Pitch,Roll,Yaw); MPU_Get_Gyroscope(gyrox,gyroy,gyroz);MPU_Get_Accelerometer(aacx,aacy,aacz);//2.将数据压入闭环控制中计算输出控制量 Velocity_out Velocity(Target_Speed,Encoder_Left,Encoder_Right);Vertical_out Vertical(Velocity_outMed_Angle,Pitch,gyroy);Turn_out Turn(gyroz); PWM_out Vertical_out;//3.把控制量加载到电机上MOTO1 PWM_out-Turn_out;//这里一加一减是因为两个电机是反向安装的实际上就是同向加减了MOTO2 PWM_outTurn_out;Limit(MOTO1,MOTO2);Load(MOTO1,MOTO2);}}
}四、小车
本人不会打板所以选择了做一回杜邦线战神
调了直立环和速度环但是不理想不能很好的回位 调试
串级的PID调试时应该断掉两个环的连接 机械中值把平衡小车放在地面上从前向后以及从后向前绕电机轴旋转平衡小车两次的向另一边倒下的角度的中值就是机械中值。 直立环 Kp极性 极性错误小车往哪边倒车轮就往反方向开会使得小车加速倒下。 极性正确小车往哪边倒车轮就往哪边开以保证小车有直立的趋势。 Kp大小 Kp一直增加直到出现大幅低频震荡。 Kd极性 极性错误拿起小车绕电机轴旋转车轮反向转动无跟随。 极性正确拿起小车绕电机轴旋转车轮同向转动有跟随。 Kd大小 Kd一直增加直到出现高频震荡。 直立环调试完毕后对所有确立的参数乘以0.6作为最终参数。 原因之前得到的参数都是Kp、Kd最大值根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。 结果乘以0.6后小车的抖动消失但同时直立效果也变差。待下面加入速度环就能得到更好的性能。 在调试速度环参数极性时需要去掉注释掉直立环运算 在调试速度环参数大小时再次引入取消注释直立环运算 KpKi 线性关系、Ki(1/200)*Kp、仅调Kp即可。 KpKi极性 极性错误手动转动其中一个车轮另一车轮会以同样速度反向旋转——典型负反馈。 极性正确手动转动其中一个车轮两个车伦会同向加速直至电机最大速度——典型正反馈。 KpKi大小 增加KpKi直至小车保持平衡的同时速度接近于零。且回位效果较好。 转向环 Kp极性 极性错误拿起小车并将小车绕Z轴旋转两车轮旋转的趋势与小车旋转趋势一致——典型正反馈。 极性正确拿起小车并将小车绕Z轴旋转两车轮旋转的趋势与小车旋转趋势相反——典型负反馈。 Kp大小 加大Kp直至走直线效果较好且无剧烈抖动。 参考于b站up主天下行走的平衡车教学
