Go one by one

  The map of the game is lattice, so it is obvious that we need to write such a function:
 

/*
First parameter aim_direction is the direction of trolley movement
 The second parameter is aim_line_number is the number of squares the car needs to move

Example code: I_Will_Always_Love_You(1,3);
The trolley moves forward three grids
*/
void I_Will_Always_Love_You(char aim_direction, char aim_line_number);

My imagination is limited. I really can't think of how to name the function. It happens that my teammates are drunk and full of "I always like yxn...", so I took such a function name. Of course, the function name does not affect the function implementation, right~~

  I installed gray-scale sensors around the car, with 7 channels on one side and 28 channels in total, using an array huidu_value[4][7] to store the 28 gray values. Row 0 of the array is the front, 1 is the right, 2 is the back, 3 is the left, column 0 of the array is the first on the left, and the sixth is the rightmost.

  Gray scale installation diagram of trolley and gray scale array diagram of trolley

 

The whole control idea is as follows:
Modify the current state (speed, operation mode, angle, etc.) in the motion control function, and then there is a 100 Hz speed controller to keep the motor speed up with the modified expected speed in the motion control function while constantly refreshing the pwm value. The motion control function is blocked, and the next function can be executed only after the whole function is executed.

Direct release code:

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);//Let the car leave the first black line
	
	while(passed_line < aim_line_number)
	{
		read_huidu_value();//Read the value of the grayscale sensor
		line_correct(aim_direction);//Line finding motion
		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();
}

First, a static variable is defined: passed_line, as the name suggests, is how many squares you walk through.
There are also two status flag bits:
①: if_approach, defined in speedcontrol.c, is sum in the previous chapter_ It will be used in the speed () function. When the trolley is about to reach the end point, slow down in advance, otherwise the trolley will slide out of the outgoing line due to the large inertia (6KG). When this sign is at position 1, the trolley will choose to reduce the running speed.
②: move_direction, defined in speedcontrol.c, is sum in the previous chapter_ It will be used in the speed () function. This value is 1234, which represents four moving directions. According to this value, the front and back of the wheel change.

The three lines in the while loop are:
① : read the value of the grayscale sensor
② : line patrol. This function was written in the previous chapter
③ : count, line_ The return value of count is whether the middle gray level meets the line. If it meets the line, it returns 1. At this time, it is passed_ Add one to line, otherwise it will always return 0
 

Read grayscale sensor value:

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

The line finding code is in the previous chapter

Number line code:

char line_count(char aim_direction, char aim_line_number,char passed_line)
{
	static char gate = 0;//Trigger threshold: it can be counted only after the trigger threshold is opened
	
	
	if(passed_line == (aim_line_number - 1))//When there's still one line to go
		if(gate==1)//Threshold triggered
        	if_approach = 1;//Deceleration flag bit

	
	if(aim_direction == 1)
	{
		if(huidu_value[1][0]||huidu_value[3][6])//Left and right first two triggers
			 gate = 1;
		if(gate&&(huidu_value[1][3]||huidu_value[3][3]))
		{
			delay_ms(60);//Debounce
			gate = 0;
			return 1;
		}
		return 0;
	}
	else if(aim_direction == 2)
	{
		if(huidu_value[2][0]||huidu_value[0][6])//Left and right first two triggers
			gate  = 1;
		if(gate&&(huidu_value[0][3]||huidu_value[2][3]))
		{
			delay_ms(60);//Debounce
			gate = 0;
			return 1;
		}
		return 0;
	}
	else if(aim_direction == 3)
	{
		if(huidu_value[3][0]||huidu_value[1][6])//Left and right first two triggers
			gate  = 1;
		if(gate&&(huidu_value[3][4]||huidu_value[1][4]))
		{
			delay_ms(60);//Debounce
			gate = 0;
			return 1;
		}
		return 0;
	}
	else if(aim_direction == 4)
	{
		if(huidu_value[0][0]||huidu_value[2][6])//Left and right first two triggers
			gate  = 1;
		if(gate&&(huidu_value[0][4]||huidu_value[2][4]))
		{
			delay_ms(60);//Debounce
			gate = 0;
			return 1;
		}
		return 0;
	}
	return 0;
}

The first if... Is to judge whether the conditions for deceleration are met. If the conditions for deceleration are met, decelerate.

The second if... Returns 1 or 0 according to the sensor value. Delay a little before returning 1 each time to let the car leave the trigger area. Otherwise, many 1s will be added in an instant.

After the function is executed, the wheel_x.trace_line_speed set to 0, if_ The approach is set to 0 and the adjust() function is called. The adjust() function is shown below.

Hold the line firmly

When you write the function of walking several grids, a new problem will arise: the car will skew after walking and will not be stuck on the cross line, which will have a great impact on the subsequent action of the manipulator. Therefore, we need to write a small function using the gray sensor to firmly clamp the car on the line.

void adjust(void)
{
	move_direction = 0;
	adjust_time = 0;
	while(1)//The four gray level sensors in the middle are not on the black line
	{
		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;//Adjust sign position 1
		read_adjust_error();
		//buzzer_bi_flag = 1;
		
		if(level_error>0)//The trolley moves forward
		{
			adjust_direction[0] = 1;//The trolley moves forward
			adjust_direction[2] = 0;//The trolley does not move backward
		}
		else if(level_error<0)
		{
			adjust_direction[0] = 0;//Trolley does not move forward
			adjust_direction[2] = 1;//The trolley moves backward
		}
		else if(level_error==0)
		{
			adjust_direction[0] = 0;//Trolley does not move forward
			adjust_direction[2] = 0;//The trolley does not move backward
		}
		
		if(vertical_error>0)//The trolley moves to the right
		{
			adjust_direction[1] = 1;
			adjust_direction[3] = 0;
		}
		else if(vertical_error<0)//The trolley moves to the left
		{
			adjust_direction[1] = 0;
			adjust_direction[3] = 1;
		}
		else if(vertical_error==0)//The trolley does not move vertically
		{
			adjust_direction[1] = 0;
			adjust_direction[3] = 0;
		}
		
		
		
		
	}
	for(int i=0;i<4;i++)
	{
		adjust_direction[i] = 0;
	}
	if_adjust = 0;//Adjust flag position 0
	move_direction = 0;
}

Obviously, the entry is a while cycle, and the exit of the cycle is to meet ① the execution of the card line for more than 50ms ② the values of the four gray levels in the middle are on the cross line.

① Because after walking for a while, the four gray levels in the middle are just online, but the speed is not zero at this time. Without this condition, the adjust() function will exit instantly and have no effect.
② Obviously, the adjust() function is used to make the car firmly stuck on the line, that is, the value of the four gray sensors in the middle is 1

Since the Z-axis angle closed loop controlled by the gyroscope is not mentioned yet, it will be written soon. Therefore, the adjust function only needs to control the car to move at a lower speed according to the sensor to make the car stuck on the line.

The first is a read_adjust_error(); Function, as follows, modify the value of the two errors

void read_adjust_error()//Because the gyroscope is reliable, there is no need to calculate the Z-axis error
{
	read_huidu_value();//Read the value of the grayscale sensor
	//The vertical error is greater than 0: the car needs to move to the right
	//If the horizontal error is greater than 0, the vehicle needs to move forward
	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 and max_vertical_error_2. These two are calculated from the front and back respectively. Take the maximum of the two and pass it to vertical_error. Similarly, get level_error.

level_error (there is an error in the horizontal direction, i.e. it needs to move back and forth)
vertical_error (there is an error in the vertical direction, that is, it needs to move left and right)

As shown in the figure above, the blue dotted line is the center of the car and the black line is the map. At this time, the car is not stuck on the line. At this time, the level will be calculated according to the above function_ error<0，vertical_ error<0. Of course, the positive and negative, horizontal and vertical here may be understood differently by everyone, but the general idea is this. First determine the moving direction of the car according to the error, and then let the car move in that direction.

  Return to the adjust() function:
According to the positive and negative error of horizontal and vertical return, give adjust_direction[4] this array is assigned a value. 0123 of the array is the front, right, rear and left directions. In sum_ The speed() function will be adjusted according to_ Calculation speed of the value of 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;
		}

So far, the first function of the work training competition is realized: go in a certain direction and a certain grid.

Next, it is necessary to realize trolley rotation, open-loop walking in any direction, turning while walking, arc walking, etc

Here is the video of my car:

Hysteresis loop_ Beep beep beep_ bilibilihttps://www.bilibili.com/video/bv1fb4y1h73KThe national car is running_ Beep beep beep_ bilibilihttps://www.bilibili.com/video/BV1nq4y1R7Us?spm_id_from=333.999.0.0Turn and run_ Beep beep beep_ bilibilihttps://www.bilibili.com/video/BV1m3411r7xB/