ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

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

2021-07-27 13:03:17  阅读：248  来源： 互联网

标签：避障 Angle Car 寻迹 1500 while Run 跨校 speed


背景：第二届跨校大学生双创训练营任务方案开源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)
	{
	}
}

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

标签：避障,Angle,Car,寻迹,1500,while,Run,跨校,speed
来源： https://blog.csdn.net/qq_41570901/article/details/119138133

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有