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

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

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

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

1、材料清单

        由于只能买社区系统中有的物资，物资大致清单如下。

社区大致物资清单（图片主要来源：telesky旗舰店）

名称	图片
直流减速电机（无编码器！）
蓝牙模块BT08
陀螺仪MPU6050
4路寻迹模块
4P超声波测距模块HC-SR04
红外避障模块
stm32f103c8t6模块
arduino
扩展板	无图

         附有套件提供的（即深圳市亚博智能科技有限公司）寻迹代码、超声波避障代码等等，所以超声波避障和寻迹的代码底层不用写，只需要移植即可。代码链接：第二届双创训练营套件代码，提取码为：ckkj。代码和套件由深圳市亚博智能科技有限公司提供。

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

       然而寻迹代码不是PID的，超声波避障程序有点占用CPU了。套件的寻迹代码和超声波避障代码并不是自己习惯的风格，后面的所有代码都是按照套件代码基础上修改的。

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

2、方案思路

        硬件部分：主控使用stm32单片机，避障用的超声波模块，寻迹使用的4路寻迹模块，还多加了个陀螺仪作为姿态传感器，电机使用的普通黄色小电机。

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

       至于为什么要加陀螺仪？经常玩电机的小伙伴应该知道，两个同时买来的电机加上同样的控制信号，两个电机的转速也绝对是不一样的。这是由于每个电机在生产的时候不可能是完全一样的，且两个电机驱动也不可能是完全一样的，甚至给电机驱动供电的电压也不可能是完全一样的，可怜的电机就因为这些因素，导致不可能完全转的一样快了。(╯﹏╰)b

        怎么让电机转速一样？在电机转速不完全一样的情况下，小车就不可能走直线，这时候就需要有个传感器来检测小车是否走了直线。一般电机加上速度传感器，控制每个电机的速度为一样的；或者在小车上安装姿态传感器。这里因为电机没有编码器，所以用的就是陀螺仪姿态传感器啦。╮(╯﹏╰）╭

        至于为啥一定要让小车走直线？因为本人感觉没有反馈的控制都不算控制，控制就一定要闭环呢✧(≖ ◡ ≖✿ 

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

        软件流程：（地图如下图所示）1、调用套件寻迹代码开始寻迹。2、当四路寻迹模块都检测到黑线时，表明进入了“*线”。3、关闭寻迹，开始避障，当碰到第一个障碍物时右转，碰到第二个障碍物时左转，以此类推。4、所有障碍物都走完时，关闭避障，开始寻迹，当四路寻迹模块都检测到黑线就设立标志位，直到四路寻迹模块又不全为黑线时，表明已经出了“*线”。然后重复1~4步骤。

        下面是程序流程图，若上面的文字看懂了，就不用看流程图了。程序的思想是这样的，但最终写出来不一定是这样的。

         所以根据以上编程思想，其实需要修改的就是 

        1、怎么判断是否结束寻迹，进入避障。

        2、完成避障需要小车准确旋转90度，并且能尽量走直线。

        3、怎么实现避障。

 3、寻迹代码（怎么判断是否结束寻迹，进入避障）

         本文提供的寻迹代码是经过一定改良的，降低了其运动速度，因为原版代码小车运动过快，可能会导致无法检测到四路寻迹模块都碰到了黑线。

        将app_linewalking.c文件中的函数进行修改，使其可以检测是否四路寻迹模块都检测到了黑线，代码如下（可直接复制粘贴替代app_linewalking.c文件的所有原代码）

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

       若想知道为啥这样改，请看第二届跨校大学生双创训练营任务方案开源3——方案思考过程

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

#include "app_linewalking.h"
#include "bsp_linewalking.h"
#include "sys.h"
#include "app_motor.h"
#include "delay.h"

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


/**
* Function       app_LineWalking
* @author        liusen
* @date          2017.07.20    
* @brief         巡线模式运动
* @param[in]     void
* @param[out]    void
* @retval        void
* @par History   无
*/
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 );
}

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

       app_linewalking.h函数就很简单改了，为了方便大家直接白嫖学习，这里也写出来，如下所示。（可直接复制粘贴替代app_linewalking.h文件的所有原代码）

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

#ifndef __APP_LINEWALKING_H__
#define __APP_LINEWALKING_H__	

#include "stm32f10x.h"

u8 app_LineWalking(void);
void Go_Line( void );

#endif

       

        所以现在只需要在主函数中运行Go_Line()函数，小车沿着循迹线走到相应位置时就会停下了。代码如下

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

	/*寻迹*/
    Go_Line();

	/*死循环*/
    while( 1 );
}

  4、小车运动代码（完成避障需要小车准确旋转90度，并且能尽量走直线）

        由于在避障的时候很需要小车能够走直线，并且旋转的时候能准确旋转90度，用了PID+陀螺仪控制小车的直走和旋转任意度数。这里直接在app_motor.c文件中添加了一些函数，小车走直线，旋转固定角度代码都如下所示。（可直接复制粘贴替代app_motor.c文件的所有原代码）

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

       当然由于我有点懒，只用了PID中的P。后来就有点后悔了(Ｔ▽Ｔ)

        陀螺仪代码直接移植的正点原子mpu6050代码

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

#include "app_motor.h"
#include "sys.h"
#include "bsp_motor.h"
#include "mpu6050.h"
#include "app_ultrasonic.h"
#include "bsp_ultrasonic.h"
#include "app_linewalking.h"

#define  LeftMotor_Go()			{GPIO_SetBits(Motor_Port, Left_MotoA_Pin); GPIO_ResetBits(Motor_Port, Left_MotoB_Pin);}
#define  LeftMotor_Back()		{GPIO_ResetBits(Motor_Port, Left_MotoA_Pin); GPIO_SetBits(Motor_Port, Left_MotoB_Pin);}
#define  LeftMotor_Stop()		{GPIO_ResetBits(Motor_Port, Left_MotoA_Pin); GPIO_ResetBits(Motor_Port, Left_MotoB_Pin);}

#define  RightMotor_Go()		{GPIO_SetBits(Motor_Port, Right_MotoA_Pin); GPIO_ResetBits(Motor_Port, Right_MotoB_Pin);}
#define  RightMotor_Back()		{GPIO_ResetBits(Motor_Port, Right_MotoA_Pin); GPIO_SetBits(Motor_Port, Right_MotoB_Pin);}
#define  RightMotor_Stop()		{GPIO_ResetBits(Motor_Port, Right_MotoA_Pin); GPIO_ResetBits(Motor_Port, Right_MotoB_Pin);}

#define  LeftMotorPWM(Speed)	TIM_SetCompare2(TIM4, Speed);
#define  RightMotorPWM(Speed)	TIM_SetCompare1(TIM4, Speed);		

extern float pitch, roll, yaw;
float I = 0;

// 小车旋转固定角度，当转到该角度以后返回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) )	
	{
		count++;
		if( count >= 10 )
		{
			count = 0;
			I = 0;
			Car_Stop();
			return( 0 );
		}
		
	}
	
	if( speed > 180 )			speed -= 360;
	else if( speed < -180 )		speed += 360;
	
	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 );
	}
  
	if( speed_error > 180 )			speed_error -= 360;
	else if( speed_error < -180 )		speed_error += 360;
	
	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 );
}

// 小车向某个角度直线运动。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;
	
	while( mpu_dmp_get_data( &pitch, &roll, &yaw ) )
	{
	}
	
	if( app_LineWalking() == 0 )
	{
		mode = 1;
	}
	else
	{
		if( mode == 1 )
		{
			mode = 0;
			Car_Stop();
			return( 0 );
		}
	}
  
	if( speed_error > 180 )			speed_error -= 360;
	else if( speed_error < -180 )		speed_error += 360;
	
	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 );
}

/**
* Function       Car_Run
* @author        liusen
* @date          2017.07.17    
* @brief         小车前进
* @param[in]     Speed  （0~7200） 速度范围
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_Run(int Speed)
{
	LeftMotor_Go();
	RightMotor_Go();
	LeftMotorPWM(Speed);		  
	RightMotorPWM(Speed);	
}

/**
* Function       Car_Back
* @author        liusen
* @date          2017.07.17    
* @brief         小车后退
* @param[in]     Speed  （0~7200） 速度范围
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_Back(int Speed)
{
	LeftMotor_Back();
	RightMotor_Back();

	LeftMotorPWM(Speed);		  
	RightMotorPWM(Speed);	
}

/**
* Function       Car_Left
* @author        liusen
* @date          2017.07.17    
* @brief         小车左转
* @param[in]     Speed  （0~7200） 速度范围
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_Left(int Speed)
{
	LeftMotor_Back();
	RightMotor_Go();

	LeftMotorPWM(Speed);		  
	RightMotorPWM(Speed);		
}

/**
* Function       Car_Right
* @author        liusen
* @date          2017.07.17    
* @brief         小车右转
* @param[in]     Speed  （0~7200） 速度范围
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_Right(int Speed)
{
	LeftMotor_Go();
	RightMotor_Back();

	LeftMotorPWM(Speed);		  
	RightMotorPWM(Speed);		
}

/**
* Function       Car_Stop
* @author        liusen
* @date          2017.07.17    
* @brief         小车刹车
* @param[in]     void
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_Stop(void)
{
	LeftMotor_Stop();
	RightMotor_Stop();

	LeftMotorPWM(0);		  
	RightMotorPWM(0);		
}

/**
* Function       Car_SpinLeft
* @author        liusen
* @date          2017.07.17    
* @brief         小车左旋
* @param[in]     LeftSpeed：左电机速度  RightSpeed：右电机速度 取值范围：（0~7200）
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_SpinLeft(int LeftSpeed, int RightSpeed)
{
	LeftMotor_Back();
	RightMotor_Go();

	LeftMotorPWM(LeftSpeed);		  
	RightMotorPWM(RightSpeed);		
}

/**
* Function       Car_SpinRight
* @author        liusen
* @date          2017.07.17    
* @brief         小车右旋
* @param[in]     LeftSpeed：左电机速度  RightSpeed：右电机速度 取值范围：（0~7200）
* @param[out]    void
* @retval        void
* @par History   无
*/

void Car_SpinRight(int LeftSpeed, int RightSpeed)
{
	LeftMotor_Go();
	RightMotor_Back();

	LeftMotorPWM(LeftSpeed);		  
	RightMotorPWM(RightSpeed);		
}

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

       一样提供h文件方便大家白嫖学习。（可直接复制粘贴替代app_motor.h文件的所有原代码）

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

#ifndef __APP_MOTOR_H__
#define __APP_MOTOR_H__


#include "bsp_motor.h"



void Car_Run(int Speed);
void Car_Back(int Speed);
void Car_Left(int Speed);
void Car_Right(int Speed);
void Car_Stop(void);
void Car_SpinStop(void);
void Car_SpinLeft(int LeftSpeed, int RightSpeed);
void Car_SpinRight(int LeftSpeed, int RightSpeed);

u8 Car_Rotate_Angle( float aim_angle );
u8 Car_Run_Angle( float aim_angle, uint16_t set_speed, uint16_t stop_dis );
u8 Car_Run_Angle_Line( float aim_angle, uint16_t set_speed );

#endif

5、避障代码（怎么实现避障）

        主要调用以上几个自己添加的程序。

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

       当然这里还需要建立一个坐标系，方便看懂程序。坐标系如下图。其实就是陀螺仪初始化的时候，小车的车头所指的方向就是0°，小车向右旋转90°就为-90°，左旋转90°就为90°。所以小车从出发区出发的时候，就是先向0°走，然后慢慢转向-90°。这就是为什么“*线”的程序第一句就是往-90°直走。(｀・ω・´)

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

// *线避障代码
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( -10 ) );
	while( Car_Run_Angle( -10, 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 ) );
	
}

6、主函数

        函数都写好后，实现第二届跨校大学生双创训练营任务方案开源1——任务介绍视频中演示的效果就很简单了。主函数如下

#include "stm32f10x.h"
#include "app_motor.h"
#include "app_bluetooth.h"
#include "bsp.h"
#include "sys.h"

#include "app_linewalking.h"
#include "bsp_ultrasonic.h"
#include "mpu6050.h"

float pitch = 0, roll = 0, yaw = 0;


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