标签：避障 Angle Car 寻迹 while Run 跨校 speed void
代码结果：第二届跨校大学生双创训练营任务方案开源2——寻迹避障代码介绍
思考过程：第二届跨校大学生双创训练营任务方案开源3——方案思考过程
思考过程：第二届跨校大学生双创训练营任务方案开源4——代码思考过程（超声波避障+红外寻迹）
1、材料清单
由于只能买社区系统中有的物资，物资大致清单如下。
| 名称 | 图片 |
| 直流减速电机（无编码器！） |  |
| 蓝牙模块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 );
#endif5、避障代码（怎么实现避障）
主要调用以上几个自己添加的程序。
/************************************************************************************************************
当然这里还需要建立一个坐标系，方便看懂程序。坐标系如下图。其实就是陀螺仪初始化的时候，小车的车头所指的方向就是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)
{
}
}标签：避障,Angle,Car,寻迹,while,Run,跨校,speed,void 来源： https://blog.csdn.net/qq_41570901/article/details/119105722
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。