2024-07-14 21:03:50 +08:00
|
|
|
|
|
|
|
#include "app_line_seek.h"
|
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
#include "bluetooth.h"
|
|
|
|
#include "buzzer.h"
|
|
|
|
#include "led.h"
|
2024-07-14 21:03:50 +08:00
|
|
|
#include "line_seek.h"
|
|
|
|
#include "main.h"
|
|
|
|
#include "motor.h"
|
2024-07-17 21:19:00 +08:00
|
|
|
#include "path_plan.h"
|
|
|
|
#include "stm32f1xx_hal_tim.h"
|
2024-07-14 21:03:50 +08:00
|
|
|
#include "syscalls.h"
|
2024-07-17 21:19:00 +08:00
|
|
|
#include "tim.h"
|
2024-07-20 10:44:59 +08:00
|
|
|
#include "timer.h"
|
2024-07-14 21:03:50 +08:00
|
|
|
|
2024-07-17 21:19:00 +08:00
|
|
|
#define LOW_TUNE_SPEED -5
|
|
|
|
#define LOW_SPEED 35
|
|
|
|
#define MID_SPEED 40
|
|
|
|
#define HIGH_SPEED 45
|
2024-07-20 10:44:59 +08:00
|
|
|
#define MIN_TURN_TIME 200 // 最小转直角时间
|
|
|
|
#define MIN_CROSSING_TIME 300 // 最小走十字路口时间
|
|
|
|
#define MIN_TUNE_TIME 30 // 最小微调时间
|
2024-07-17 21:19:00 +08:00
|
|
|
|
|
|
|
typedef enum
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-17 21:19:00 +08:00
|
|
|
MOVE_STRAIGHT, // 直行
|
|
|
|
TURN_LEFT, // 左转
|
|
|
|
TURN_RIGHT, // 右转
|
|
|
|
TUNE_LEFT, // 左微调
|
|
|
|
TUNE_RIGHT, // 右微调
|
|
|
|
TURN_LEFT_BIG,
|
2024-07-20 10:44:59 +08:00
|
|
|
TURN_RIGHT_BIG,
|
|
|
|
MOVE_STOP
|
2024-07-17 21:19:00 +08:00
|
|
|
} MoveState;
|
2024-07-14 21:03:50 +08:00
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
static MoveState state, preState; // 运动状态
|
|
|
|
int turn_time; // 转弯时间
|
|
|
|
int is_crossing; // 是否正在走十字路口
|
|
|
|
static timer_event_id = -1;
|
2024-07-14 21:03:50 +08:00
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
// 每1ms一次回调
|
|
|
|
void LINESEEK_TURN_TIME()
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
if (turn_time < INT16_MAX)
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-20 10:44:59 +08:00
|
|
|
turn_time++;
|
2024-07-14 21:03:50 +08:00
|
|
|
}
|
2024-07-20 10:44:59 +08:00
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
// 巡线功能初始化
|
|
|
|
void LineSeek_Init(void)
|
|
|
|
{
|
|
|
|
// 注册定时器事件
|
2024-07-21 08:13:13 +08:00
|
|
|
TIMER_AddLoopEvent(EVENT_LINESEEK, LINESEEK_TURN_TIME, 1);
|
2024-07-20 10:44:59 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
void LineSeek_Start(void)
|
|
|
|
{
|
|
|
|
if (state == MOVE_STOP)
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-20 10:44:59 +08:00
|
|
|
state = MOVE_STRAIGHT;
|
2024-07-14 21:03:50 +08:00
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
}
|
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
void LineSeek_Stop(void)
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
2024-07-20 10:44:59 +08:00
|
|
|
state = MOVE_STOP;
|
2024-07-17 21:19:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
void App_LineSeek(void)
|
|
|
|
{
|
2024-07-20 10:44:59 +08:00
|
|
|
// LineSeek_GetStatusStr();
|
2024-07-17 21:19:00 +08:00
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
// if (state != MOVE_STOP)
|
|
|
|
// {
|
|
|
|
// BLUETOOTH_Send("line_seek state:%d, line_seek: %s\n", state, LineSeek_Status);
|
|
|
|
// // BLUETOOTH_Send("line_seek: %s \n", LineSeek_Status);
|
|
|
|
// }
|
2024-07-17 21:19:00 +08:00
|
|
|
// printf("%s %d \n", LineSeek_Status, turn_time);
|
|
|
|
|
|
|
|
// return;
|
|
|
|
|
|
|
|
if (turn_time > MIN_CROSSING_TIME)
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-17 21:19:00 +08:00
|
|
|
is_crossing = 0;
|
2024-07-14 21:03:50 +08:00
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
|
|
|
|
if (is_crossing && GetCurrentDirection() == STRAIGHT)
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-17 21:19:00 +08:00
|
|
|
LineSeek_Status[0] = '1';
|
|
|
|
LineSeek_Status[3] = '1';
|
2024-07-14 21:03:50 +08:00
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
|
2024-07-20 10:44:59 +08:00
|
|
|
if (LineSeek_Equals("0000") && !is_crossing && state != MOVE_STOP) // 十字路口
|
|
|
|
{
|
|
|
|
Hanlde_Crossroad();
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2024-07-17 21:19:00 +08:00
|
|
|
switch (state)
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-17 21:19:00 +08:00
|
|
|
case MOVE_STRAIGHT: // 直行
|
2024-07-20 10:44:59 +08:00
|
|
|
LED_Stop(LED_ALL);
|
|
|
|
LED_Start(LED_R);
|
2024-07-17 21:19:00 +08:00
|
|
|
MOTOR_SetDuty(HIGH_SPEED, HIGH_SPEED);
|
2024-07-21 08:13:13 +08:00
|
|
|
LED_SetDuty(0, 255, 0);
|
2024-07-20 10:44:59 +08:00
|
|
|
if (LineSeek_Equals("1011")) // 左微调
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
state = TUNE_LEFT;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("1101")) // 右微调
|
|
|
|
{
|
|
|
|
state = TUNE_RIGHT;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("0001")) // 左直角
|
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_LEFT;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("1000")) // 右直角
|
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("0xx1")) // 左锐角
|
|
|
|
{
|
|
|
|
state = TURN_LEFT_BIG;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("1xx0")) // 右锐角
|
|
|
|
{
|
|
|
|
state = TURN_RIGHT_BIG;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("1111")) // 冲出去了,使用上一个转向
|
|
|
|
{
|
|
|
|
state = preState;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case TURN_LEFT: // 左直角
|
2024-07-21 08:13:13 +08:00
|
|
|
BUZZER_StartTimed(200);
|
|
|
|
LED_StartNTimes(3, 100, 50, 0, 0, 255);
|
2024-07-17 21:19:00 +08:00
|
|
|
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED);
|
2024-07-20 10:44:59 +08:00
|
|
|
if (LineSeek_Equals("1001") &&
|
|
|
|
((preState != TUNE_LEFT && turn_time >= MIN_TURN_TIME - 1) ||
|
|
|
|
(preState == TUNE_LEFT && turn_time >= MIN_TUNE_TIME))) // 直行
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
state = MOVE_STRAIGHT;
|
|
|
|
preState = TURN_LEFT;
|
|
|
|
is_crossing = 0;
|
2024-07-20 10:44:59 +08:00
|
|
|
LED_Stop(LED_ALL);
|
|
|
|
LED_Start(LED_B);
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("xxx0") && turn_time < MIN_TURN_TIME && !is_crossing) // 侧丁字路口
|
|
|
|
{
|
|
|
|
preState = TUNE_RIGHT;
|
|
|
|
Hanlde_Crossroad();
|
2024-07-20 10:44:59 +08:00
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("xxx0") && turn_time < MIN_TURN_TIME && !is_crossing) // 侧丁字路口
|
|
|
|
{
|
|
|
|
preState = TUNE_RIGHT;
|
|
|
|
Hanlde_Crossroad();
|
2024-07-17 21:19:00 +08:00
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("xxx0") && !is_crossing) // 转过头了
|
|
|
|
{
|
|
|
|
turn_time = MIN_TURN_TIME;
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
preState = TURN_LEFT;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case TURN_RIGHT: // 右直角
|
2024-07-21 08:13:13 +08:00
|
|
|
BUZZER_StartTimed(200);
|
|
|
|
LED_StartNTimes(3, 100, 50, 0, 0, 255);
|
2024-07-17 21:19:00 +08:00
|
|
|
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED);
|
2024-07-20 10:44:59 +08:00
|
|
|
if (LineSeek_Equals("1001") &&
|
|
|
|
((preState != TUNE_RIGHT && turn_time >= MIN_TURN_TIME - 1) ||
|
|
|
|
(preState == TUNE_RIGHT && turn_time >= MIN_TUNE_TIME))) // 直行
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
state = MOVE_STRAIGHT;
|
|
|
|
preState = TURN_RIGHT;
|
|
|
|
is_crossing = 0;
|
|
|
|
}
|
2024-07-20 10:44:59 +08:00
|
|
|
else if (LineSeek_Equals("0xxx") && turn_time < MIN_TURN_TIME && !is_crossing) // 侧丁字路口
|
|
|
|
{
|
|
|
|
preState = TUNE_LEFT;
|
|
|
|
Hanlde_Crossroad();
|
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
else if (LineSeek_Equals("0xxx") && !is_crossing) // 转过头了
|
|
|
|
{
|
|
|
|
turn_time = MIN_TURN_TIME;
|
|
|
|
state = TURN_LEFT;
|
|
|
|
preState = TURN_LEFT;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case TURN_LEFT_BIG: // 左锐角
|
|
|
|
MOTOR_SetDuty(-MID_SPEED, MID_SPEED);
|
2024-07-20 10:44:59 +08:00
|
|
|
if (LineSeek_Equals("0001")) // 左直角
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_LEFT;
|
|
|
|
preState = TURN_LEFT_BIG;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("xxx0")) // 转过头了
|
|
|
|
{
|
|
|
|
state = TUNE_RIGHT;
|
|
|
|
// preState = TURN_LEFT_BIG;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case TURN_RIGHT_BIG: // 右锐角
|
|
|
|
MOTOR_SetDuty(MID_SPEED, -MID_SPEED);
|
2024-07-20 10:44:59 +08:00
|
|
|
if (LineSeek_Equals("1000")) // 右直角
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
preState = TURN_RIGHT_BIG;
|
|
|
|
}
|
|
|
|
else if (LineSeek_Equals("0xxx")) // 转过头了
|
|
|
|
{
|
|
|
|
state = TUNE_LEFT;
|
|
|
|
preState = TURN_RIGHT_BIG;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case TUNE_LEFT: // 左微调
|
|
|
|
MOTOR_SetDuty(LOW_TUNE_SPEED, HIGH_SPEED);
|
|
|
|
if (LineSeek_Equals("x00x"))
|
|
|
|
{
|
|
|
|
state = MOVE_STRAIGHT;
|
|
|
|
}
|
2024-07-20 10:44:59 +08:00
|
|
|
else if (LineSeek_Equals("0xx1")) // 左转
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_LEFT;
|
|
|
|
preState = TUNE_LEFT;
|
|
|
|
}
|
2024-07-20 10:44:59 +08:00
|
|
|
else if (LineSeek_Equals("1xx0")) // 右转
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
preState = TUNE_LEFT;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case TUNE_RIGHT: // 右微调
|
|
|
|
MOTOR_SetDuty(HIGH_SPEED, LOW_TUNE_SPEED);
|
|
|
|
if (LineSeek_Equals("x00x"))
|
|
|
|
{
|
|
|
|
state = MOVE_STRAIGHT;
|
|
|
|
}
|
2024-07-20 10:44:59 +08:00
|
|
|
else if (LineSeek_Equals("0xx1")) // 左转
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_LEFT;
|
|
|
|
preState = TUNE_RIGHT;
|
|
|
|
}
|
2024-07-20 10:44:59 +08:00
|
|
|
else if (LineSeek_Equals("1xx0")) // 右转
|
2024-07-17 21:19:00 +08:00
|
|
|
{
|
|
|
|
turn_time = 0;
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
preState = TUNE_RIGHT;
|
|
|
|
}
|
|
|
|
break;
|
2024-07-20 10:44:59 +08:00
|
|
|
case MOVE_STOP:
|
|
|
|
MOTOR_Stop();
|
|
|
|
break;
|
2024-07-17 21:19:00 +08:00
|
|
|
default:
|
|
|
|
state = MOVE_STRAIGHT;
|
|
|
|
MOTOR_SetDuty(MID_SPEED, MID_SPEED);
|
|
|
|
break;
|
2024-07-14 21:03:50 +08:00
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
|
|
|
|
// printf("curr state: %d \n", state);
|
|
|
|
}
|
|
|
|
|
|
|
|
void Hanlde_Crossroad(void)
|
|
|
|
{
|
2024-07-20 10:44:59 +08:00
|
|
|
// 开启蜂鸣器
|
2024-07-21 08:13:13 +08:00
|
|
|
// BUZZER_StartTimed(100);
|
2024-07-17 21:19:00 +08:00
|
|
|
|
|
|
|
// 根据规划的路径进行决断
|
|
|
|
is_crossing = 1;
|
|
|
|
turn_time = 0;
|
|
|
|
int direction = GetNextDirection();
|
|
|
|
switch (direction)
|
2024-07-14 21:03:50 +08:00
|
|
|
{
|
2024-07-17 21:19:00 +08:00
|
|
|
case STRAIGHT:
|
|
|
|
state = MOVE_STRAIGHT;
|
|
|
|
break;
|
|
|
|
case LEFT:
|
|
|
|
state = TURN_LEFT;
|
|
|
|
break;
|
|
|
|
case RIGHT:
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
state = TURN_RIGHT;
|
|
|
|
break;
|
2024-07-14 21:03:50 +08:00
|
|
|
}
|
2024-07-17 21:19:00 +08:00
|
|
|
// printf("state: %d, dir: %d \n", state, direction);
|
2024-07-20 10:44:59 +08:00
|
|
|
}
|