中国大学生工程实践与创新能力竞赛（工程训练大赛）——智慧物流搬运小车 ④ 数格、卡线

最新推荐文章于 2024-06-13 08:30:00 发布

ycznjust

最新推荐文章于 2024-06-13 08:30:00 发布

阅读量5.4k

点赞数 13

分类专栏：工程实践与创新能力竞赛文章标签：单片机嵌入式硬件

本文链接：https://blog.csdn.net/ycznjust/article/details/121040596

版权

工程实践与创新能力竞赛专栏收录该内容

5 篇文章 43 订阅

订阅专栏

一格一格的走

比赛的地图是格子状的，所以很明显，我们需要写这样一个函数：

/*
第一个参数aim_direction是小车运动方向
第二个参数aim_line_number是小车要运动几格

示例代码：I_Will_Always_Love_You(1,3);
小车向前运动三格
*/
void I_Will_Always_Love_You(char aim_direction, char aim_line_number);

想象力有限，实在想不出来怎么给函数命名，恰逢我的队友（杨xtb）喝醉，满口“我永远喜欢刘x楠....”，所以取了这样一个函数名，当然函数名并不影响功能实现功能对吧~~

我在小车的四周都安装了灰度传感器，一边7路，合计28路，使用了一个数组huidu_value[4][7]来存放这28个灰度的值，数组第0行是前、1是右、2是后、3是左，数组第0列是左边第一个，到第6个是最右边的一个。

小车灰度安装示意图、小车灰度数组示意图

整个控制的思路是这样的：
在运动控制的函数内修改当前的状态（速度、运行方式、角度等等），然后有一个100hz的速度控制器，在不断的刷新pwm值保持电机转速跟上运动控制函数内修改好的期望速度。其中运动控制函数是堵塞的，之后执行完整个函数才可以执行下一个函数。

直接放代码：

void I_Will_Always_Love_You(char aim_direction, char aim_line_number)
{
	static char passed_line = 0;
	
	if_approach = 0;
	move_direction = aim_direction;
	delay_ms(200);//让小车离开第一条黑线
	
	while(passed_line < aim_line_number)
	{
		read_huidu_value();//读取灰度传感器的值
		line_correct(aim_direction);//寻线运动
		passed_line += line_count(aim_direction,aim_line_number,passed_line);
	}

	wheel_1.trace_line_speed = 0;
	wheel_2.trace_line_speed = 0;
	wheel_3.trace_line_speed = 0;
	wheel_4.trace_line_speed = 0;

	if_approach = 0;
	adjust();
}

先定义了一个静态变量：passed_line，顾名思义就是走过多少个格子。
还有两个状态标志位：
①：if_approach，定义在speedcontrol.c里，在上一章sum_speed()函数里会用到。当小车即将到达终点时提前减速，不然由于车惯量较大（6KG），车会滑出线外，当这个标志位置1后，小车会选择降低运行速度。
②：move_direction，定义在speedcontrol.c里，在上一章sum_speed()函数里会用到。这个值取1234，代表四个移动方向，根据这个值的不同，轮子正反转会发生变化。

在while循环里的三行分别是：
①：读取灰度传感器的值
②：巡线，这个函数在上一章有写
③：计数，line_count的返回值就是中间灰度是否遇到线，遇到的话返回1，此时passed_line就加一，不然一直返回0

读取灰度传感器值：

void read_huidu_value(void)
{
	
	huidu_value[0][0] = gpio_get(IO1);
	huidu_value[0][1] = gpio_get(IO2);
	huidu_value[0][2] = gpio_get(IO3);
	huidu_value[0][3] = gpio_get(IO4);
	huidu_value[0][4] = gpio_get(IO5);
	huidu_value[0][5] = gpio_get(IO6);
	huidu_value[0][6] = gpio_get(IO7);
	
	huidu_value[1][0] = gpio_get(IO8);
	huidu_value[1][1] = gpio_get(IO9);
	huidu_value[1][2] = gpio_get(IO10);
	huidu_value[1][3] = gpio_get(IO11);
	huidu_value[1][4] = gpio_get(IO12);
	huidu_value[1][5] = gpio_get(IO13);
	huidu_value[1][6] = gpio_get(IO14);
	
	huidu_value[2][0] = gpio_get(IO15);
	huidu_value[2][1] = gpio_get(IO16);
	huidu_value[2][2] = gpio_get(IO17);
	huidu_value[2][3] = gpio_get(IO18);
	huidu_value[2][4] = gpio_get(IO19);
	huidu_value[2][5] = gpio_get(IO20);
	huidu_value[2][6] = gpio_get(IO21);
	
	huidu_value[3][0] = gpio_get(IO22);
	huidu_value[3][1] = gpio_get(IO23);
	huidu_value[3][2] = gpio_get(IO24);
	huidu_value[3][3] = gpio_get(IO25);
	huidu_value[3][4] = gpio_get(IO26);
	huidu_value[3][5] = gpio_get(IO27);
	huidu_value[3][6] = gpio_get(IO28);
}

寻线代码在上一章

数线代码：

char line_count(char aim_direction, char aim_line_number,char passed_line)
{
	static char gate = 0;//触发门限，首先触发门限打开才能计数
	
	
	if(passed_line == (aim_line_number - 1))//还差一条线的时候
		if(gate==1)//已经触发门限
        	if_approach = 1;//降速标志位

	
	if(aim_direction == 1)
	{
		if(huidu_value[1][0]||huidu_value[3][6])//左右前两个触发
			 gate = 1;
		if(gate&&(huidu_value[1][3]||huidu_value[3][3]))
		{
			delay_ms(60);//消抖
			gate = 0;
			return 1;
		}
		return 0;
	}
	else if(aim_direction == 2)
	{
		if(huidu_value[2][0]||huidu_value[0][6])//左右前两个触发
			gate  = 1;
		if(gate&&(huidu_value[0][3]||huidu_value[2][3]))
		{
			delay_ms(60);//消抖
			gate = 0;
			return 1;
		}
		return 0;
	}
	else if(aim_direction == 3)
	{
		if(huidu_value[3][0]||huidu_value[1][6])//左右前两个触发
			gate  = 1;
		if(gate&&(huidu_value[3][4]||huidu_value[1][4]))
		{
			delay_ms(60);//消抖
			gate = 0;
			return 1;
		}
		return 0;
	}
	else if(aim_direction == 4)
	{
		if(huidu_value[0][0]||huidu_value[2][6])//左右前两个触发
			gate  = 1;
		if(gate&&(huidu_value[0][4]||huidu_value[2][4]))
		{
			delay_ms(60);//消抖
			gate = 0;
			return 1;
		}
		return 0;
	}
	return 0;
}

第一个if...里是判断是否到了减速的条件，如果到了该减速的条件，就减速。

第二个if...里根据传感器值返回1或者0，其中，每次返回1之前延时一点点，让小车离开那个触发区域，不然的话，会一瞬间加很多个1。

在函数执行完之后，要对wheel_x.trace_line_speed置0，if_approach置0，并调用adjust()函数，adjust()函数在下面。

牢牢地卡着线

当你写好走几格的函数后，新的问题即将产生：走完车会歪掉，并没有卡在十字线上，这将会对后续的机械臂动作抓取产生非常大的影响，所以我们需要利用灰度传感器写一个小函数，实现将车牢牢地卡到线上。

void adjust(void)
{
	move_direction = 0;
	adjust_time = 0;
	while(1)//中间四个灰度传感器不在黑线上
	{
		if((adjust_time>=50)&&(huidu_value[0][3]==1)&&(huidu_value[1][3]==1)&&(huidu_value[2][3]==1)&&(huidu_value[3][3]==1))
			break;
		
		if_adjust = 1;//调整标志位置1
		read_adjust_error();
		//buzzer_bi_flag = 1;
		
		if(level_error>0)//小车向前挪动
		{
			adjust_direction[0] = 1;//小车向前挪动
			adjust_direction[2] = 0;//小车不向后挪动
		}
		else if(level_error<0)
		{
			adjust_direction[0] = 0;//小车不向前挪动
			adjust_direction[2] = 1;//小车向后挪动
		}
		else if(level_error==0)
		{
			adjust_direction[0] = 0;//小车不向前挪动
			adjust_direction[2] = 0;//小车不向后挪动
		}
		
		if(vertical_error>0)//小车向右挪动
		{
			adjust_direction[1] = 1;
			adjust_direction[3] = 0;
		}
		else if(vertical_error<0)//小车向左挪动
		{
			adjust_direction[1] = 0;
			adjust_direction[3] = 1;
		}
		else if(vertical_error==0)//小车竖直方向不挪动
		{
			adjust_direction[1] = 0;
			adjust_direction[3] = 0;
		}
		
		
		
		
	}
	for(int i=0;i<4;i++)
	{
		adjust_direction[i] = 0;
	}
	if_adjust = 0;//调整标志位置0
	move_direction = 0;
}

显而易见，进去就是while循环，循环的出口是满足①执行卡线50ms以上②中间四个灰度的值在十字线上。

①因为执行完走一段路后，中间四个灰度刚好在线上，但是此时的速度不是零，如果没这个条件的话，adjust()函数会瞬间退出，没有起到作用。
②显而易见，adjust()函数的作用就是让车牢牢地卡在线上，也就是说中间四个灰度传感器的值是1

由于有陀螺仪控制的Z轴角度闭环，虽然现在还没说，但马上会写，所以adjust函数只需要根据传感器，控制小车以较低速度挪动，让车卡在线上。

首先是一个read_adjust_error();函数，如下，修改两个误差的值

void read_adjust_error()//因为陀螺仪可靠，所以不需要计算Z轴误差
{
	read_huidu_value();//读取灰度传感器的值
	//竖直误差大于0：车需要向右挪动
	//水平误差大于0：车需要向前挪动
	int max_vertical_error_1 = -3*huidu_value[0][0] -2*huidu_value[0][1] -huidu_value[0][2] + huidu_value[0][4] +2*huidu_value[0][5] +3*huidu_value[0][6];
	int max_vertical_error_2 = -3*huidu_value[2][6] -2*huidu_value[2][5] -huidu_value[2][4] + huidu_value[2][2] +2*huidu_value[2][1] +3*huidu_value[2][0];
	
	if(abs(max_vertical_error_1)>=abs(max_vertical_error_2))
		vertical_error = max_vertical_error_1;
	else 
		vertical_error = max_vertical_error_2;
	
	int max_level_error_1 = -3*huidu_value[3][0] -2*huidu_value[3][1] -huidu_value[3][2] + huidu_value[3][4] +2*huidu_value[3][5] +3*huidu_value[3][6];
	int max_level_error_2 = -3*huidu_value[1][6] -2*huidu_value[1][5] -huidu_value[1][4] + huidu_value[1][2] +2*huidu_value[1][1] +3*huidu_value[1][0];
	
	if(abs(max_level_error_1)>=abs(max_level_error_2))
		level_error = max_level_error_1;
	else 
		level_error = max_level_error_2;
}

max_vertical_error_1和max_vertical_error_2，这两个分别是由前后计算的，取两个中的最大值，传递给vertical_error。同理得到level_error。

level_error（水平方向上有误差，即需要前后挪动）
vertical_error（竖直方向上有误差，即需要左右挪动）

如上图蓝色虚线是小车中心，黑线是地图，此时小车并没有卡到线上，这时根据上面的函数会计算得到level_error<0，vertical_error<0。当然这里的正负、水平和竖直每个人理解的可能不一样，但是大概思路就是这样，先根据误差确定车的移动方向，然后让小车往那个方向挪就行。

回到adjust()函数：
根据水平和竖直返回的误差正负，给adjust_direction[4]这个数组赋值，数组的0123是前右后左四个方向。在sum_speed()函数里会根据adjust_direction[x]的值算速度：

if(if_adjust==1)
		{
			wheel_1.basic_speed = adjust_direction[0]*speed_choose + adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose - adjust_direction[3]*speed_choose;
			wheel_2.basic_speed = adjust_direction[0]*speed_choose - adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose + adjust_direction[3]*speed_choose;
			wheel_3.basic_speed = adjust_direction[0]*speed_choose - adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose + adjust_direction[3]*speed_choose;
			wheel_4.basic_speed = adjust_direction[0]*speed_choose + adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose - adjust_direction[3]*speed_choose;
		}

至此，工训赛的第一个功能实现：往某个方向走某格。

接下来要实现小车旋转、开环任意方向走、边走边转、走弧线等等

这里放我做的小车视频：

磁滞回线_哔哩哔哩_bilibilihttps://www.bilibili.com/video/bv1fb4y1h73K 国赛车跑起来_哔哩哔哩_bilibilihttps://www.bilibili.com/video/BV1nq4y1R7Us?spm_id_from=333.999.0.0 边转边跑_哔哩哔哩_bilibilihttps://www.bilibili.com/video/BV1m3411r7xB/

ycznjust

关注

13
点赞
踩
169

收藏

觉得还不错? 一键收藏
14
评论
中国大学生工程实践与创新能力竞赛（工程训练大赛）——智慧物流搬运小车 ④ 数格、卡线

一格一格的走比赛的地图是格子状的，所以很明显，我们需要写这样一个函数：/*第一个参数aim_direction是小车运动方向第二个参数aim_line_number是小车要运动几格示例代码：I_Will_Always_Love_You(1,3);小车向前运动三格*/void I_Will_Always_Love_You(char aim_direction, char aim_line_number);想象力有限，实在想不出来怎么给函数命名，恰逢我的队友喝醉，满口“我..
复制链接

扫一扫