RoboGame参赛总结-2
实现思路
STM32F103ZET6拥有TIM1~8共8个定时器,其中包含TIM8和TIM1两个高级定时器和剩余的普通定时器,高级定时器的初始化更为复杂,当然,性能更好,实现的功能更加丰富。
四轮小车PID控制其实思路很简单,通过定时器捕获电机编码器得到的脉冲数据,转换为速度值,进行PID运算后转换成针对PWM波的控制量,进而实现闭环控制。
值得注意的是,由于捕获的信号是离散的,同时单片机的内存很有限,因此建议采用离散形式的PID函数。
这是经典的PID函数,换为离散形式:
看到sigma就知道了,可以换成增量形式:
这里,我们就有很明确的代码编写思路了:
先定义一个结构体:
typedef struct PID{
int SetPoint; //目标值
float Proportion; //比例常数
float Integral; //积分常数
float Derivative; //微分常数
int LastErr;
int PrevErr;
} PID_DataStructure;
然后定义函数:
// An highlighted block
float Drive_PID_Calc(int NextPoint, unsigned int index){ //增量式PID计算
int iErr;
long iIncpid;
iErr=PID_WheelP[index]->SetPoint-NextPoint;
iIncpid=PID_WheelP[index]->Proportion*iErr - PID_WheelP[index]->Integral*PID_WheelP[index]->LastErr
+PID_WheelP[index]->Derivative*PID_WheelP[index]->PrevErr;
PID_WheelP[index]->PrevErr=PID_WheelP[index]->LastErr;
PID_WheelP[index]->LastErr=iErr;
return(iIncpid);
}
但是,很重要的轮子我们还没造好,那就是中断,让main函数一直盯着这种机械的东西运行是很糟糕的事情。
先说一下我们选用的驱动模块:
BTS7960是一款非常常用的驱动板,用来驱动24V电机没有压力,可靠性很好,缺憾是只能拖一个电机。具体的管脚定义在网上可以很方便的找到,这里不再赘述。我们只需要知道的是,连接单片机的就是两路PWM波,当两路都有PWM或都没有,就会短接刹车,否则某一路PWM决定就决定了给电的方向。
一个定时器拥有四路PWM通道,我们选用TIM3和TIM4实现对四个驱动板的控制,这里贴一段TIM3的初始化代码:
void TIM3_PWM_Init(u16 arr,u16 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); //使能定时器3时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE); //使能GPIO外设和AFIO复用功能模块时钟
GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE); //Timer3完全重映射是使能
//设置引脚为复用输出功能,TIM3四个通道分别对应PC6,PC7,PC8,PC9
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);//初始化GPIO
//初始化TIM3
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
//初始化TIM3PWM模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
TIM_OC1Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3的四个通道
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
TIM_OC3Init(TIM3, &TIM_OCInitStructure);
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在各通道CCR上的预装载寄存器
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_Cmd(TIM3, ENABLE); //使能TIM3
}
arr和psc两个参数决定了PWM波的周期特性,这里就不展开了,其实也很简单,技术手册上就能查询。
最关键的地方,输入捕获,同时也肩负着周期性采集运算速度及调控PWM波的任务,非常关键。
先上代码:
void Encoder_Init(u16 arr, u16 psc){
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM2_ICInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能 TIM2 时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_AFIO, ENABLE); //使能 GPIOA|GPIOB 时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_PinRemapConfig(GPIO_PartialRemap1_TIM2, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
//RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
TIM_TimeBaseStructure.TIM_Period = arr; //设定计数器自动重装值
TIM_TimeBaseStructure.TIM_Prescaler = psc; //设置预分频值
//溢出周期
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; // TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM 向上计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据指定的参数初始化 Tim3
TIM2_ICInitStructure.TIM_Channel = TIM_Channel_1; //选择输入端 IC1 映射到 TI1 上
TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; //上升沿捕获
TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; //映射到 TI1 上
TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; //配置输入分频,不分频
TIM2_ICInitStructure.TIM_ICFilter = 0x0f;//IC1F=0011 配置输入滤波器 不滤波
TIM_ICInit(TIM2, &TIM2_ICInitStructure);
TIM2_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInit(TIM2, &TIM2_ICInitStructure);
TIM2_ICInitStructure.TIM_Channel = TIM_Channel_3;
TIM_ICInit(TIM2, &TIM2_ICInitStructure);
TIM2_ICInitStructure.TIM_Channel = TIM_Channel_4;
TIM_ICInit(TIM2, &TIM2_ICInitStructure);
TIM_Cmd(TIM2, ENABLE);
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
TIM_ITConfig(TIM2, TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4, ENABLE);
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; //NVIC配置
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
//定义相关管脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_3;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_2;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_3;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
这个定时器的输入捕获使用有个非常关键的地方,当时也是查询了很久才搞定的:
GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
这里关闭了其中一个管脚的特殊复用功能,才能正常实现PB3对应通道的捕获。
贴个大佬的帖子,可以去看看
链接: PB3管脚的使用.
如果通读了上面的代码,可以看到我使能了4个输入捕获中断和更新溢出中断,捕获中断很好理解,就是个计数器:
if(TIM_GetITStatus(CAPTURE_CHANNEL1) != RESET) {
WheelRound_Monitor(0, times);
wheel_counter[0]++;
TIM_ClearITPendingBit(CAPTURE_CHANNEL1);
}
void WheelRound_Monitor(u8 index, u16* times){
static float err[4] = {0, 0, 0, 0};
static float nowRound[4]={0, 0, 0, 0};
static u8 PauseCounter=0;
if(PWMControlNode[index]->counter_en == 1){
times[index]++;
nowRound[index] = (float)times[index]/ppr;
err[index] = PWMControlNode[index]->wheelround - nowRound[index];
if(err[index] <= 0){
PWMControlNode[index]->pwm_compare = 0; // 清空比较值
PWMControlNode[index]->PID_en = 0; //停止PID计算
PWMControlNode[index]->pwm1 = 0; //PWM停止
PWMControlNode[index]->pwm2 = 0;
PWMControlNode[index]->counter_en = 0; //关闭计数
PauseCounter++; //暂停标志位加1
times[index] = 0; //清空计数器
}
}
else{
times[index] = 0;
}
if(PauseCounter==4){
//Drive_AdjustIrsensor(IRsensor_sta);//姿态校正
ObjectSearchNode->IRsensor_en=0;
PauseCounter = 0;
ObjectSearchNode->engine_sta = 0;
//行进完成
}
}
可能对那个结构体有些迷惑,定义在这里,其实就是把一些使能开关和计数器啥的堆在一起,方便调用:
typedef struct PWMStructure{
u8 pwm1;
u8 pwm2;
int pwm_compare;//PWM比较值
u16 SpeedTarget;//设定目标值
int SpeedControl;//循迹控制量
float wheelround;//目标圈数值
u8 PID_en;//PID使能
u8 counter_en;//计数器使能
float wheel_speed;//当前转速
}PWMStructure;
pwm1和pwm2就是两路PWM的使能,也许注意到了,在TIM2的溢出中断中,拥有一个这样的函数:
void Wheelcontrol(void){
if(PWMControlNode[0]->pwm2==1)
TIM_SetCompare1(TIM3, PWMControlNode[0]->pwm_compare);
else
TIM_SetCompare1(TIM3, 0);
if(PWMControlNode[0]->pwm1==1)
TIM_SetCompare2(TIM3, PWMControlNode[0]->pwm_compare);
else
TIM_SetCompare2(TIM3, 0);
if(PWMControlNode[2]->pwm2==1)
TIM_SetCompare1(TIM4, PWMControlNode[2]->pwm_compare);
else
TIM_SetCompare1(TIM4, 0);
if(PWMControlNode[2]->pwm1==1)
TIM_SetCompare2(TIM4, PWMControlNode[2]->pwm_compare);
else
TIM_SetCompare2(TIM4, 0);
if(PWMControlNode[1]->pwm1==1)
TIM_SetCompare3(TIM3, PWMControlNode[1]->pwm_compare);
else
TIM_SetCompare3(TIM3, 0);
if(PWMControlNode[1]->pwm2==1)
TIM_SetCompare4(TIM3, PWMControlNode[1]->pwm_compare);
else
TIM_SetCompare4(TIM3, 0);
if(PWMControlNode[3]->pwm1==1)
TIM_SetCompare3(TIM4, PWMControlNode[3]->pwm_compare);
else
TIM_SetCompare3(TIM4, 0);
if(PWMControlNode[3]->pwm2==1)
TIM_SetCompare4(TIM4, PWMControlNode[3]->pwm_compare);
else
TIM_SetCompare4(TIM4, 0);
}
其实就是很蠢的根据使能情况往比较值的寄存器里面写入值,但也实在没想到特别好的办法。
接下来就是最关键的PID执行了:
注意:
Drive_Run(PWMControlNode);
就是将四个电机的信息都写入这个函数然后进行PID计算:
在此之前先算一下最新获取的速度值:
for(index=0; index<4; index++){
PWMControlNode[index]->wheel_speed = wheel_counter[index]*60/(ppr*captureT);
wheel_counter[index] = 0;
}
很简单的一个循环,得到速度值。
然后:
void Drive_Run(PWMStructure* PWMControlNode[4]){ //PID运行
int index;
for(index=0; index<4; index++){
if(PWMControlNode[index]->PID_en == 0) continue; //使能检查
PID_WheelP[index]->SetPoint=PWMControlNode[index]->SpeedTarget + PWMControlNode[index]->SpeedControl;
if(PID_WheelP[index]->SetPoint<0){
PID_WheelP[index]->SetPoint=0;
//防止溢出
}
PWMControlNode[index]->pwm_compare = PWMControlNode[index]->pwm_compare + (int)Drive_PID_Calc((int)PWMControlNode[index]->wheel_speed,index);
//溢出保护
if(PWMControlNode[index]->pwm_compare<0){
PWMControlNode[index]->pwm_compare = 0;
}
}
//inform_test(pwm_compare[0]);
}
注意一定要保证速度值不会为负,否则写入寄存器的时候会有问题,出现无法刹车的情况,当时我们也是思考了挺久才意识到这个问题的。
至此就差不多完成了,但为了方便后面的调试,我尝试封装了一下,加入了定距离行进的功能,其实上面的代码就有:
if(PWMControlNode[index]->counter_en == 1){
times[index]++;
nowRound[index] = (float)times[index]/ppr;
err[index] = PWMControlNode[index]->wheelround - nowRound[index];
if(err[index] <= 0){
PWMControlNode[index]->pwm_compare = 0; // 清空比较值
PWMControlNode[index]->PID_en = 0; //停止PID计算
PWMControlNode[index]->pwm1 = 0; //PWM停止
PWMControlNode[index]->pwm2 = 0;
PWMControlNode[index]->counter_en = 0; //关闭计数
PauseCounter++; //暂停标志位加1
times[index] = 0; //清空计数器
}
}
其实实现很简单,就是设置一个计数器,当到达设定值的时候就把PWM关掉,触发刹车,这样也能很好的保证车体最终姿态和初始位置的一致,还是很好用的。但要注意不能任由PID计算进行下去,这样可能会出问题。
接下来介绍一下在实际使用时候运用的函数,其实就是把一些乱七八糟的初始化,设定值结合到一个函数里,通过参数实现各种运动功能:
void Drive_Control(int commond, float distance, u16 speed){
//commond 运动方向:Go Back RollR RollL Right Left SIDEGO**需要调用mirror_direction检查方向
//distance 运动距离 存在宏定义 STEP(基本距离)TURN(旋转90度)SIDE(侧移距离)
//channal侧向移动时的通道 -> sideway_index 全局变量
//channal: 侧向循迹通道
while(ObjectSearchNode->engine_sta);
//等待上一个位移命令结束
delay_ms(200);
ObjectSearchNode->engine_sta = 1; //电机任务占用
Drive_Start();
//驱动使能位开
Drive_DistanceSet(distance);
//设置距离
Drive_PWMInit();
//清空比较值
Drive_SetSpeed(speed); //基础转速
//重设速度值
Control_Init();
//清除控制速度
Drive_PIDStart();
//PID开始执行
switch(commond){
case Go: Drive_Go(); break;
case Back: Drive_Back(); break;
case Right: Drive_Right(); break;
case Left: Drive_Left(); break;
case RollR: Drive_RollR(); break;
case RollL: Drive_RollL(); break;
}
}
左右移动之类按照麦克纳姆轮的运动规则控制正反转就可以了:
void Drive_Right(void){
ObjectSearchNode->drive_direction = 2;
//侧向循迹线路由commond控制
PWMControlNode[0]->pwm1 = 0;
PWMControlNode[0]->pwm2 = 1;
PWMControlNode[1]->pwm1 = 1;
PWMControlNode[1]->pwm2 = 0;
PWMControlNode[2]->pwm1 = 1;
PWMControlNode[2]->pwm2 = 0;
PWMControlNode[3]->pwm1 = 0;
PWMControlNode[3]->pwm2 = 1;
}
在实际使用中,效果还是不错的,也很好维护或着加入一些其他的功能。