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

一格一格的走

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

/*
第一个参数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;//调整标志位置1read_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;//调整标志位置0move_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/