RoboGame参赛总结-2

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;
}

在实际使用中,效果还是不错的,也很好维护或着加入一些其他的功能。

  • 2
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值