89 lines
2.2 KiB
C
89 lines
2.2 KiB
C
|
|
||
|
|
#include "app_line_seek.h"
|
||
|
|
|
||
|
|
#include "line_seek.h"
|
||
|
|
#include "main.h"
|
||
|
|
#include "motor.h"
|
||
|
|
#include "path_plan.h"
|
||
|
|
#include "syscalls.h"
|
||
|
|
|
||
|
|
// #define LOW_SPEED 30
|
||
|
|
// #define MID_SPEED 35
|
||
|
|
// #define HIGH_SPEED 40
|
||
|
|
|
||
|
|
#define LOW_SPEED 25
|
||
|
|
#define MID_SPEED 30
|
||
|
|
#define HIGH_SPEED 35
|
||
|
|
|
||
|
|
|
||
|
|
void App_LineSeek(void)
|
||
|
|
{
|
||
|
|
int LineL1 = 1, LineL2 = 1, LineR1 = 1, LineR2 = 1;
|
||
|
|
|
||
|
|
LineSeek_GetStatus(&LineL1, &LineL2, &LineR1, &LineR2); // 获取黑线检测状态
|
||
|
|
|
||
|
|
LineSeek_GetStatusStr();
|
||
|
|
printf("%s\n", LineSeek_Status);
|
||
|
|
|
||
|
|
// return;
|
||
|
|
|
||
|
|
if (LineSeek_Equals("0000")) // 丁字（十字）路口
|
||
|
|
{
|
||
|
|
buzzer(1);
|
||
|
|
|
||
|
|
// 根据规划的路径进行决断
|
||
|
|
switch (GetNextDirection())
|
||
|
|
{
|
||
|
|
case STRAIGHT:
|
||
|
|
MOTOR_SetDuty(HIGH_SPEED, HIGH_SPEED); // 直走
|
||
|
|
break;
|
||
|
|
case LEFT:
|
||
|
|
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED); // 左转
|
||
|
|
break;
|
||
|
|
case RIGHT:
|
||
|
|
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED); // 右转
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
MOTOR_Stop();
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
buzzer(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (LineSeek_Equals("1000")) // 右直角
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED); // 右旋
|
||
|
|
// HAL_Delay(80);
|
||
|
|
}
|
||
|
|
else if (LineSeek_Equals("0001")) // 左直角
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED); // 左旋;
|
||
|
|
// HAL_Delay(80);
|
||
|
|
}
|
||
|
|
else if (LineSeek_Equals("0xx1")) // 左最外侧检测(锐角)
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(-HIGH_SPEED, MID_SPEED); // 左旋
|
||
|
|
// HAL_Delay(80);
|
||
|
|
}
|
||
|
|
else if (LineSeek_Equals("1xx0")) // 右最外侧检测(锐角)
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(MID_SPEED, -HIGH_SPEED); // 右旋
|
||
|
|
// HAL_Delay(80);
|
||
|
|
}
|
||
|
|
else if (LineSeek_Equals("x01x")) // 中间黑线上的传感器微调车左转
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(0, LOW_SPEED); // 左转
|
||
|
|
}
|
||
|
|
else if (LineSeek_Equals("x10x")) // 中间黑线上的传感器微调车右转
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(LOW_SPEED, 0); // 右转
|
||
|
|
}
|
||
|
|
else if (LineSeek_Equals("x00x")) // 都是黑色, 加速前进
|
||
|
|
{
|
||
|
|
MOTOR_SetDuty(MID_SPEED, MID_SPEED); // 直走
|
||
|
|
}
|
||
|
|
}
|