背景：第二届跨校大学生双创训练营任务方案开源1——任务介绍

代码结果：第二届跨校大学生双创训练营任务方案开源2——寻迹避障代码介绍

思考过程：第二届跨校大学生双创训练营任务方案开源3——方案思考过程

思考过程：第二届跨校大学生双创训练营任务方案开源4——代码思考过程（超声波避障+红外寻迹）

        应该很少有人分析为什么代码这样写？代码换个方式写会怎么样？为什么要设定这个方案？写代码的时候就一定一帆风顺吗？感觉这些思考方式才是我们更需要了解知道的，所以特别设定一个章节写清楚整个方案的制定到代码编写时的思考流程。

1、避障代码编写过程

        首先有如下的基本代码，根据陀螺仪数据，能准确的旋转90度，甚至说想旋转到多少度就是多少度，每句话的意思都有注释。

/************************************************************************************************************

       因为本人代码写的太多了，这种代码一遍就写完了，几乎没有bug（主要是bug影响不大），就没啥思考过程了，嘿嘿（づ￣3￣）づ╭❤～

************************************************************************************************************/

// 小车旋转固定角度，当转到该角度以后返回0，否者返回1
#define CAR_ROTATE_PID_KP		60
#define CAR_ROTATE_PID_KI		0.0f
u8 Car_Rotate_Angle( float aim_angle )
{
	float speed = aim_angle - yaw;	// 误差计算，计算目标角度和当前角度的偏差
	
	static u8 count = 0;	// 计数变量
	
	while( mpu_dmp_get_data( &pitch, &roll, &yaw ) )	// 陀螺仪数据获取
  {
  }
	
	// 误差判断
	if( (speed < 1) && (speed > -1) )	// 当设定的角度与实际的角度偏差在±1°时
	{
		count++;	// 开始计数
		if( count >= 10 )	// 当10次检测出来都是这个误差，那么就可以肯定小车确实是转到这个角度了
		{
			count = 0;	// 变量清空
			I = 0;	// PID的I清空
			Car_Stop();	// 小车停下
			return( 0 );// 返回0，跳出while死循环，函数使用方法为：while( Car_Rotate_Angle() );，只有运行到这一步，才会跳出死循环
		}
		
	}
	
	// 角度换算
	if( speed > 180 )			speed -= 360;
	else if( speed < -180 )		speed += 360;
	
	// PID算法，但是I没用上，因为CAR_ROTATE_PID_KI为0
	I += speed;
	speed *= CAR_ROTATE_PID_KP;
	speed += I*CAR_ROTATE_PID_KI;
	
	// 最大限幅
	if( speed > 7000 )			speed = 7000;
	else if( speed < -7000 )	speed = -7000;
	// 最小速度限幅
	if( (speed > 0) && (speed < 2000 ) )			speed = 1500;
	else if( (speed < 0) && (speed > -2000 ) )		speed = -1500;
	
	// 设置小车旋转的速度与方向
	if( speed > 0 )
	{
		Car_Left( speed );
	}
	else
	{
		speed = -speed;
		Car_Right( speed );
	}
	
	return( 1 );
}

        下面的代码是根据陀螺仪的数据确保小车能走直线，当前方障碍物比较近时小车停下，每句话的意思都有注释。

// 小车向某个角度直线运动。set_speed为可以设置运动速度。stop_dis为当距离小于多少时小车停止并返回0，否者返回1
#define CAR_RUN_PID_KP			50
u8 Car_Run_Angle( float aim_angle, uint16_t set_speed, uint16_t stop_dis )
{
	float speed_error = aim_angle - yaw;	// 误差计算，计算目标角度和当前角度的偏差
	uint16_t Dis;	// 定义变量
	
	while( mpu_dmp_get_data( &pitch, &roll, &yaw ) )	// 陀螺仪数据获取
	{
	}
	Dis = bsp_getUltrasonicDistance(); 	// 获取超声波测距的距离值
	if( Dis < stop_dis )	// 如果障碍物距离很近？
	{
		Car_Stop();	// 小车停下
		return( 0 );// 返回0，跳出while死循环，函数使用方法为：while( Car_Run_Angle() );，只有运行到这一步，才会跳出死循环
	}
  
	// 角度换算
	if( speed_error > 180 )			speed_error -= 360;
	else if( speed_error < -180 )		speed_error += 360;
	
	// PID算法的P
	speed_error *= CAR_RUN_PID_KP;
	
	// 最大限幅
	if( speed_error > 7000 )			speed_error = 7000;
	else if( speed_error < -7000 )	speed_error = -7000;
	
	// 设置小车两边*的速度
	LeftMotor_Go();
	RightMotor_Go();
	LeftMotorPWM(set_speed-speed_error);		  
	RightMotorPWM(set_speed+speed_error);	
	return( 1 );
}

        下面详细介绍“*线”避障代码，地图如下：

         根据地图绿色的轨迹线我们可以发现，走出“*线”只要几步：

        1、 往-90°行走，速度假设为1500，当前方障碍物距离为20的时候小车停止

        2、旋转到-180°

        3、往 -180°直走，速度假设为1500，当前方障碍物距离为10的时候小车停止

        4、旋转到-90°

        5、往 -90°直走，速度假设为1500，当前方障碍物距离为10的时候小车停止

        6、旋转到0°

       7、往 0°直走，速度假设为1500，一直走就走出去了，不用管前方障碍物了，所以随便设定一个距离就行。

        根据以上的步骤，代码如下：

// *线避障代码
void FengShuoXian( void )
{
	while( Car_Run_Angle( -90, 1500, 20 ) );
	while(  Car_Rotate_Angle( -180 ) );
	while( Car_Run_Angle( -180, 1500, 10 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 10 ) );
	while(  Car_Rotate_Angle( 0 ) );
	while( Car_Run_Angle( 0, 1500, 10 ) );
}

        同理，其他地图是一样的，代码如下：（只要慢慢调参就行了）

// 河流避障代码
void HeLiu( void )
{
	while( Car_Run_Angle( 0, 1500, 10 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 13 ) );
	while(  Car_Rotate_Angle( 25 ) );
	while( Car_Run_Angle( 25, 3000, 30 ) );
	while( Car_Run_Angle( 25, 1500, 10 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 8 ) );
	while(  Car_Rotate_Angle( 150 ) );
	while( Car_Run_Angle( 150, 1500, 10 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 10 ) );
	
}

// 草地避障代码
void ChaoDi( void )
{
	while( Car_Run_Angle( 180, 1500, 10 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 10 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 5 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 5 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 8 ) );
	while(  Car_Rotate_Angle( -100 ) );
	while( Car_Run_Angle( -100, 1500, 8 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 8 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 10 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 10 ) );
}

// 雪山避障代码
void XueSan( void )
{
	while( Car_Run_Angle( 0, 1500, 15 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 5 ) );
	while(  Car_Rotate_Angle( 0 ) );
	while( Car_Run_Angle( 0, 1500, 8 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 8 ) );
	while(  Car_Rotate_Angle( 0 ) );
	while( Car_Run_Angle( 0, 1500, 10 ) );
	
}

2、寻迹代码编写过程

         寻迹代码其实只要加个四路黑线检测就行了，因为套件寻迹代码是像下面这样写的

int main(void)
{
    /*外设初始化*/
	bsp_init();

    while (1)
	{
        app_LineWalking();
	}
}

        也就是说其实套件代码也就是一直死循环的跑，要想跑出死循环，就还是要在函数里面设置return，就像之前的小车运动函数使用的一样。那就在官方代码里面加上return(0)和return(1)就行了。

        所以根据要求，当我们检测到四路都是黑线的时候，我们跳出死循环，就可以继续执行后面走地图的程序了。代码如下所示

u8 app_LineWalking(void)
{
	int LineL1 = 1, LineL2 = 1, LineR1 = 1, LineR2 = 1;

	bsp_GetLineWalking(&LineL1, &LineL2, &LineR1, &LineR2);	//获取黑线检测状态	

	if( (LineL1 == LOW) && (LineL2 == LOW) && (LineR2 == LOW) && (LineR1 == LOW) ) //四路寻线模块都为黑线时
    {
      	return(0);
    }
	else if( (LineL1 == LOW || LineL2 == LOW) && LineR2 == LOW) //左大弯
    {
      	Car_SpinLeft(2500, 2500);
		delay_ms(10);
    }
    else if ( LineL1 == LOW && (LineR1 == LOW || LineR2 == LOW)) //右大弯
	{ 
      	Car_SpinRight(2500, 2500);
		delay_ms(10);
    }  
    else if( LineL1 == LOW ) //左最外侧检测
    {  
		Car_SpinLeft(2000, 2000);
		delay_ms(10);
	}
    else if ( LineR2 == LOW) //右最外侧检测
    {  
		Car_SpinRight(2000,2000);
		delay_ms(10);
	}
    else if (LineL2 == LOW && LineR1 == HIGH) //中间黑线上的传感器微调车左转
    {   
		Car_Left(1500);   
	}
	else if (LineL2 == HIGH && LineR1 == LOW) //中间黑线上的传感器微调车右转
    {   
		Car_Right(1500);   
	}
    else if(LineL2 == LOW && LineR1 == LOW) // 都是黑色, 加速前进
    {  
		Car_Run(2000);
	}
	
	
	return( 1 );
}

        其实上面主要就是加了一个if语句，如下所示：

if( (LineL1 == LOW) && (LineL2 == LOW) && (LineR2 == LOW) && (LineR1 == LOW) ) //四路寻线模块都为黑线时
{
    return(0);
}

/************************************************************************************************************

        当时这里遇到点问题，就是把上面那个四路黑线判断代码换个顺序就无法正常运行了。比如下面展示的这样，把代码移动到最后一个 else if 的判断中。

        这是因为当四路都是黑线的时候，这种情况会先触发前面的判断语句，导致只是执行了前面的情况。

u8 app_LineWalking(void)
{
	int LineL1 = 1, LineL2 = 1, LineR1 = 1, LineR2 = 1;

	bsp_GetLineWalking(&LineL1, &LineL2, &LineR1, &LineR2);	//获取黑线检测状态	

	if( (LineL1 == LOW || LineL2 == LOW) && LineR2 == LOW) //左大弯
    {
      	Car_SpinLeft(2500, 2500);
		delay_ms(10);
    }
    else if ( LineL1 == LOW && (LineR1 == LOW || LineR2 == LOW)) //右大弯
	{ 
      	Car_SpinRight(2500, 2500);
		delay_ms(10);
    }  
    else if( LineL1 == LOW ) //左最外侧检测
    {  
		Car_SpinLeft(2000, 2000);
		delay_ms(10);
	}
    else if ( LineR2 == LOW) //右最外侧检测
    {  
		Car_SpinRight(2000,2000);
		delay_ms(10);
	}
    else if (LineL2 == LOW && LineR1 == HIGH) //中间黑线上的传感器微调车左转
    {   
		Car_Left(1500);   
	}
	else if (LineL2 == HIGH && LineR1 == LOW) //中间黑线上的传感器微调车右转
    {   
		Car_Right(1500);   
	}
    else if(LineL2 == LOW && LineR1 == LOW) // 都是黑色, 加速前进
    {  
		Car_Run(2000);
	}
    else if( (LineL1 == LOW) && (LineL2 == LOW) && (LineR2 == LOW) && (LineR1 == LOW) ) //四路寻线模块都为黑线时
    {
      	return(0);
    }
	
	
	return( 1 );
}

************************************************************************************************************/

        代码的用法如下，当检测到四路都是黑线的时候，就会跳出while死循环，然后小车停止。

// 寻迹函数，当遇到四路都是黑线时小车自动暂停
void Go_Line( void )
{
	while( app_LineWalking() );
	Car_Stop();
}

        然后我们主函数这样写代码就行了。就能实现先寻迹，然后走完“*线”地图。主函数是八是很简单(〃'▽'〃)

int main(void)
{
	/*外设初始化*/
	bsp_init();

    /*陀螺仪初始化*/
	MPU_Init();
	 while( mpu_dmp_init() )
	 {
	 }
	 while( mpu_dmp_get_data( &pitch, &roll, &yaw ) )
	  {
	  }

    /*寻迹+避障*/
    Go_Line();
    FengShuoXian();

    while (1)
	{
	}
}

3、程序优化

        程序执行完会发现，小车运行确实没问题，但问题是走出“*线”地图的时候没有识别是否走出来了。要识别到真的走出来以后，再继续执行寻迹代码走到下个地图。既然这样我们就把上面走直线的函数改一下吧，不再写成某个距离就return(0)，而是改成检测到四路黑线再return(0)。这个很方便改了，代码如下。

/************************************************************************************************************

       代码就一点点不一样哦~

************************************************************************************************************/

// 小车向某个角度直线运动。set_speed为可以设置运动速度。当四路寻迹模块都为黑色时继续行走，直到四路寻迹模块不全为黑色后小车停止并返回0，其余时候返回1
u8 Car_Run_Angle_Line( float aim_angle, uint16_t set_speed )
{
	float speed_error = aim_angle - yaw;	// 误差计算，计算目标角度和当前角度的偏差
	static u8 mode = 0;	// 定义flag变量
	
	while( mpu_dmp_get_data( &pitch, &roll, &yaw ) )	// 陀螺仪数据获取
	{
	}
	
	if( app_LineWalking() == 0 )	// 当检测到四路寻迹模块都是黑线时
	{
		mode = 1;	// 设立标志位
	}
	else	// 当检测到四路寻迹模块不全是黑线时
	{
		if( mode == 1 )	// 如果四路寻迹模块曾经全是是黑线时，就表明已经完全走出来了
		{
			mode = 0;	// 设立标志位
			Car_Stop();	// 小车停下
			return( 0 );// 返回0，跳出while死循环
		}
	}
  
	// 角度换算
	if( speed_error > 180 )			speed_error -= 360;
	else if( speed_error < -180 )		speed_error += 360;
	
	// PID算法的P
	speed_error *= CAR_RUN_PID_KP;
	
	// 最大限幅
	if( speed_error > 7000 )			speed_error = 7000;
	else if( speed_error < -7000 )	speed_error = -7000;
	
	// 设置小车两边*的速度
	LeftMotor_Go();
	RightMotor_Go();
	LeftMotorPWM(set_speed-speed_error);		  
	RightMotorPWM(set_speed+speed_error);	
	return( 1 );
}

        有了以上函数以后，咋们把避障代码换掉就是了，如下图所示。可以看到其实就是把每个函数最后一句话换掉了，参数都不需要怎么改的。

// *线避障代码
void FengShuoXian( void )
{
	while( Car_Run_Angle( -90, 1500, 20 ) );
	while(  Car_Rotate_Angle( -180 ) );
	while( Car_Run_Angle( -180, 1500, 10 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 5 ) );
	while(  Car_Rotate_Angle( 0 ) );
	while( Car_Run_Angle_Line( 0, 1500 ) );
}

// 河流避障代码
void HeLiu( void )
{
	while( Car_Run_Angle( 0, 1500, 10 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 13 ) );
	while(  Car_Rotate_Angle( 25 ) );
	while( Car_Run_Angle( 25, 3000, 30 ) );
	while( Car_Run_Angle( 25, 1500, 10 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 8 ) );
	while(  Car_Rotate_Angle( 150 ) );
	while( Car_Run_Angle( 150, 1500, 10 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle_Line( 90, 1500 ) );
	
}

// 草地避障代码
void ChaoDi( void )
{
	while( Car_Run_Angle( 180, 1500, 10 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 10 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 5 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 5 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 8 ) );
	while(  Car_Rotate_Angle( -100 ) );
	while( Car_Run_Angle( -100, 1500, 8 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle( 180, 1500, 8 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 10 ) );
	while(  Car_Rotate_Angle( 180 ) );
	while( Car_Run_Angle_Line( 180, 1500 ) );
}

// 雪山避障代码
void XueSan( void )
{
	while( Car_Run_Angle( 0, 1500, 15 ) );
	while(  Car_Rotate_Angle( -90 ) );
	while( Car_Run_Angle( -90, 1500, 5 ) );
	while(  Car_Rotate_Angle( 0 ) );
	while( Car_Run_Angle( 0, 1500, 8 ) );
	while(  Car_Rotate_Angle( 90 ) );
	while( Car_Run_Angle( 90, 1500, 8 ) );
	while(  Car_Rotate_Angle( 0 ) );
	while( Car_Run_Angle_Line( 0, 1500 ) );
	
}

        最后一步就是写主函数，让程序运行起来就行了。

int main(void)
{
	/*外设初始化*/
	bsp_init();
	
	/*陀螺仪初始化*/
	MPU_Init();
	 while( mpu_dmp_init() )
	 {
	 }
	 while( mpu_dmp_get_data( &pitch, &roll, &yaw ) )
	  {
	  }
	
	  /*寻迹+避障*/
	  Go_Line();
	  FengShuoXian();
	  Go_Line();
	  HeLiu();
	  Go_Line();
	  ChaoDi();
	  Go_Line();
	  XueSan();
	  Go_Line();
	 
	while (1)
	{
	}
}

        最后需要做的就是调参了，避障函数里面那么多的参数，一个个慢慢调吧，调的效果好，就能走出来啦(〃'▽'〃)