公交车LED
This commit is contained in:
parent
e70663edb3
commit
8a6ecbc14c
|
@ -15,10 +15,13 @@ typedef enum
|
|||
READY_TO_PARK, // 准备进站停车(或者准备左转)
|
||||
PARKING, // 进站停车
|
||||
WAIT_PASSENGER, // 等候乘客
|
||||
EXIT_STATION, // 出站
|
||||
STOP // 停车
|
||||
} BusState;
|
||||
|
||||
#define MIN_CROSS_TIME 10
|
||||
static BusState state;
|
||||
static cross_time;
|
||||
|
||||
static int current_station;
|
||||
char* stations[] = {"太平园站", "西南交大站", "新业北街站", "终点站"};
|
||||
|
@ -33,7 +36,6 @@ void App_Bus(void)
|
|||
{
|
||||
case RUNNING: // 按路线运营
|
||||
{
|
||||
LineSeek_Start();
|
||||
if (Has_Block(FRONT)) // 前方有障碍物,停车
|
||||
{
|
||||
state = STOP;
|
||||
|
@ -58,17 +60,25 @@ void App_Bus(void)
|
|||
ReplacePathStep(nextDirs, 2);
|
||||
state = RUNNING;
|
||||
}
|
||||
else if (LineSeek_Equals("xxx0")) // 右方有路线,右转进站
|
||||
else if (LineSeek_Equals("xxx0")) // 右方有路线,要确保左方没路线,才右转进站
|
||||
{
|
||||
Direction nextDirs[] = {RIGHT, RIGHT};
|
||||
ReplacePathStep(nextDirs, 2);
|
||||
Direction nextDirs[] = {RIGHT};
|
||||
ReplacePathStep(nextDirs, 1);
|
||||
cross_time = 0;
|
||||
state = PARKING;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case PARKING: // 进站停车
|
||||
{
|
||||
if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物,停车
|
||||
if (LineSeek_Equals("0000")) // 左方有路线,左转
|
||||
{
|
||||
Direction nextDirs[] = {LEFT, LEFT};
|
||||
ReplacePathStep(nextDirs, 2);
|
||||
state = RUNNING;
|
||||
}
|
||||
|
||||
if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物,停车
|
||||
{
|
||||
state = WAIT_PASSENGER;
|
||||
}
|
||||
|
@ -79,16 +89,29 @@ void App_Bus(void)
|
|||
// TODO: 添加乘客等待逻辑
|
||||
LineSeek_Stop();
|
||||
SYN_FrameInfo(0, stations[current_station]);
|
||||
BUZZER_StartNTimes(3, 500, 500);
|
||||
MOTOR_Stop();
|
||||
HAL_Delay(5000);
|
||||
LED_StartNTimes(5, 400, 200, 0, 255, 255);
|
||||
HAL_Delay(2000);
|
||||
current_station++;
|
||||
state = RUNNING;
|
||||
state = EXIT_STATION;
|
||||
break;
|
||||
}
|
||||
case EXIT_STATION: // 出站
|
||||
{
|
||||
LineSeek_Start();
|
||||
if (LineSeek_Equals("xxx0")) // 右方有路线,右转出站
|
||||
{
|
||||
Direction nextDirs[] = {RIGHT};
|
||||
ReplacePathStep(nextDirs, 1);
|
||||
state = RUNNING;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case STOP: // 停车
|
||||
{
|
||||
LineSeek_Stop();
|
||||
BUZZER_StartNTimes(10, 1000, 500);
|
||||
LED_StartNTimes(10, 1000, 500, 255, 0, 0);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
|
|
|
@ -51,7 +51,7 @@ void LINESEEK_TURN_TIME()
|
|||
void LineSeek_Init(void)
|
||||
{
|
||||
// 注册定时器事件
|
||||
timer_event_id = TIMER_AddInfiniteLoopEvent(timer_event_id, LINESEEK_TURN_TIME, 1);
|
||||
TIMER_AddLoopEvent(EVENT_LINESEEK, LINESEEK_TURN_TIME, 1);
|
||||
}
|
||||
|
||||
void LineSeek_Start(void)
|
||||
|
@ -62,6 +62,7 @@ void LineSeek_Start(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
void LineSeek_Stop(void)
|
||||
{
|
||||
state = MOVE_STOP;
|
||||
|
@ -100,9 +101,8 @@ void App_LineSeek(void)
|
|||
switch (state)
|
||||
{
|
||||
case MOVE_STRAIGHT: // 直行
|
||||
LED_Stop(LED_ALL);
|
||||
LED_Start(LED_R);
|
||||
MOTOR_SetDuty(HIGH_SPEED, HIGH_SPEED);
|
||||
LED_SetDuty(0, 255, 0);
|
||||
if (LineSeek_Equals("1011")) // 左微调
|
||||
{
|
||||
state = TUNE_LEFT;
|
||||
|
@ -135,9 +135,8 @@ void App_LineSeek(void)
|
|||
}
|
||||
break;
|
||||
case TURN_LEFT: // 左直角
|
||||
|
||||
LED_Stop(LED_ALL);
|
||||
LED_Start(LED_G);
|
||||
BUZZER_StartTimed(200);
|
||||
LED_StartNTimes(3, 100, 50, 0, 0, 255);
|
||||
MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED);
|
||||
if (LineSeek_Equals("1001") &&
|
||||
((preState != TUNE_LEFT && turn_time >= MIN_TURN_TIME - 1) ||
|
||||
|
@ -146,8 +145,6 @@ void App_LineSeek(void)
|
|||
state = MOVE_STRAIGHT;
|
||||
preState = TURN_LEFT;
|
||||
is_crossing = 0;
|
||||
LED_Stop(LED_ALL);
|
||||
LED_Start(LED_B);
|
||||
}
|
||||
else if (LineSeek_Equals("xxx0") && turn_time < MIN_TURN_TIME && !is_crossing) // 侧丁字路口
|
||||
{
|
||||
|
@ -162,6 +159,8 @@ void App_LineSeek(void)
|
|||
}
|
||||
break;
|
||||
case TURN_RIGHT: // 右直角
|
||||
BUZZER_StartTimed(200);
|
||||
LED_StartNTimes(3, 100, 50, 0, 0, 255);
|
||||
MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED);
|
||||
if (LineSeek_Equals("1001") &&
|
||||
((preState != TUNE_RIGHT && turn_time >= MIN_TURN_TIME - 1) ||
|
||||
|
@ -264,7 +263,7 @@ void App_LineSeek(void)
|
|||
void Hanlde_Crossroad(void)
|
||||
{
|
||||
// 开启蜂鸣器
|
||||
BUZZER_StartTimed(1);
|
||||
// BUZZER_StartTimed(100);
|
||||
|
||||
// 根据规划的路径进行决断
|
||||
is_crossing = 1;
|
||||
|
|
|
@ -129,9 +129,14 @@ int main(void)
|
|||
LineSeek_Init();
|
||||
BlockDetect_Init();
|
||||
SERVO_Init();
|
||||
SERVO_Rotate(90);
|
||||
LED_Start(LED_ALL);
|
||||
|
||||
HAL_Delay(2000);
|
||||
SERVO_Rotate(0);
|
||||
HAL_Delay(500);
|
||||
SERVO_Rotate(180);
|
||||
HAL_Delay(500);
|
||||
SERVO_Rotate(90);
|
||||
HAL_Delay(500);
|
||||
|
||||
// BUZZER_StartNTimes(20, 500, 500);
|
||||
/* USER CODE END 2 */
|
||||
|
|
|
@ -6,9 +6,10 @@
|
|||
void BUZZER_Start();
|
||||
void BUZZER_Stop();
|
||||
|
||||
|
||||
/// @brief 蜂鸣器定时鸣叫
|
||||
/// @param time 鸣叫时间,单位毫秒
|
||||
void BUZZER_StartTimed(uint8_t time);
|
||||
void BUZZER_StartTimed(uint16_t time);
|
||||
|
||||
/// @brief 蜂鸣器鸣叫数次
|
||||
/// @param loop 鸣叫次数
|
||||
|
|
|
@ -11,10 +11,6 @@
|
|||
void LED_Start(uint8_t led);
|
||||
void LED_Stop(uint8_t led);
|
||||
|
||||
|
||||
void LED_StartTimed(uint8_t led, uint16_t time);
|
||||
void LED_StartNTimes(uint8_t led, uint16_t on_time, uint16_t off_time);
|
||||
|
||||
// duty in [0, 255]
|
||||
void LED_SetDuty(uint8_t r, uint8_t g, uint8_t b);
|
||||
|
||||
|
|
|
@ -35,8 +35,8 @@ void sendSensor(void)
|
|||
|
||||
void BLUETOOTH_Init(void)
|
||||
{
|
||||
HAL_UARTEx_ReceiveToIdle_IT(&huart2, rxBuffer, 100);
|
||||
timer_event_id = TIMER_AddInfiniteLoopEvent(timer_event_id, sendSensor, 1);
|
||||
HAL_UARTEx_ReceiveToIdle_DMA(&huart2, rxBuffer, 100);
|
||||
TIMER_AddLoopEvent(EVENT_BLUETOOTH, sendSensor, 200);
|
||||
}
|
||||
|
||||
void BLUETOOTH_Send(const char *str, ...)
|
||||
|
|
|
@ -1,4 +1,5 @@
|
|||
#include "buzzer.h"
|
||||
|
||||
#include "timer.h"
|
||||
|
||||
#define OFF 0
|
||||
|
@ -24,7 +25,9 @@ inline void BUZZER_Stop()
|
|||
state = OFF;
|
||||
}
|
||||
|
||||
void BUZZER_StartTimed(uint8_t time)
|
||||
/// @brief 启动蜂鸣器鸣叫
|
||||
/// @param time 鸣叫时间,单位1毫秒
|
||||
void BUZZER_StartTimed(uint16_t time)
|
||||
{
|
||||
state = ONCE;
|
||||
BUZZER_Start();
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
#include "led.h"
|
||||
|
||||
#include "tim.h"
|
||||
#include "timer.h"
|
||||
|
||||
|
@ -37,16 +36,6 @@ void LED_Stop(uint8_t led)
|
|||
HAL_TIM_PWM_Stop(LED_TIM, LED_B_CHAN);
|
||||
}
|
||||
|
||||
void LED_StartTimed(uint8_t led, uint16_t time)
|
||||
{
|
||||
led_timed_id = TIMER_AddDelayEvent(led_timed_id, LED_Stop, time);
|
||||
}
|
||||
|
||||
void LED_StartNTimes(uint8_t led, uint16_t on_time, uint16_t off_time)
|
||||
{
|
||||
// led_times_id = TIMER_AddFiniteLoopEvent(led_times_id, LED_Stop, time);
|
||||
}
|
||||
|
||||
void LED_SetDuty(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
__HAL_TIM_SetCompare(LED_TIM, LED_R_CHAN, r);
|
||||
|
|
|
@ -3,13 +3,18 @@
|
|||
|
||||
#include "main.h"
|
||||
|
||||
#define EVENT_MAX 20
|
||||
#define EVENT_LED 0
|
||||
#define EVENT_BUZZER 1
|
||||
#define EVENT_BLUETOOTH 2
|
||||
#define EVENT_SERVO 3
|
||||
#define EVENT_LINESEEK 4
|
||||
#define EVENT_INFRARED 5
|
||||
#define EVENT_SONAR 6
|
||||
#define EVENT_BLOCK 7
|
||||
#define EVENT_MAX 8
|
||||
|
||||
void TIMER_Init(void);
|
||||
int8_t TIMER_AddInfiniteLoopEvent(int8_t id, void (*func)(void), uint16_t loop_time);
|
||||
int8_t TIMER_AddFiniteLoopEvent(int8_t id, void (*func)(void), uint16_t loop_time, uint16_t loop_cnt);
|
||||
int8_t TIMER_AddFiniteLoopEventWithParam(int8_t event_id, void (*func)(uint16_t param), uint16_t loop_time, uint16_t loop_cnt, uint16_t param);
|
||||
int8_t TIMER_AddDelayEvent(int8_t id, void (*func)(void), uint16_t delay_time);
|
||||
void TIMER_DelLoopEvent(int8_t id);
|
||||
void TIMER_AddLoopEvent(uint8_t type, void (*func)(void), uint16_t loop_time);
|
||||
void TIMER_DelLoopEvent(uint8_t type);
|
||||
|
||||
#endif
|
||||
|
|
|
@ -7,20 +7,27 @@
|
|||
#include "syscalls.h"
|
||||
#include "timer.h"
|
||||
|
||||
#define MIN_BLOCK_TIME 500
|
||||
|
||||
int has_block_front;
|
||||
int has_block_left;
|
||||
int has_block_right;
|
||||
static uint8_t timer_event_id = -1;
|
||||
static uint8_t timer_event_id2 = -1;
|
||||
int block_time;
|
||||
|
||||
void INFRARED_Detect();
|
||||
void Sonar_Detect();
|
||||
|
||||
void Block_Time_Inc()
|
||||
{
|
||||
block_time++;
|
||||
}
|
||||
|
||||
// 避障检测初始化
|
||||
void BlockDetect_Init()
|
||||
{
|
||||
timer_event_id = TIMER_AddInfiniteLoopEvent(timer_event_id, INFRARED_Detect, 20);
|
||||
timer_event_id2 = TIMER_AddInfiniteLoopEvent(timer_event_id2, Sonar_Detect, 150);
|
||||
TIMER_AddLoopEvent(EVENT_INFRARED, INFRARED_Detect, 5);
|
||||
TIMER_AddLoopEvent(EVENT_SONAR, Sonar_Detect, 150);
|
||||
TIMER_AddLoopEvent(EVENT_BLOCK, Block_Time_Inc, 1);
|
||||
}
|
||||
|
||||
// 每20ms红外检测一次障碍物
|
||||
|
@ -48,16 +55,37 @@ int Has_Block(BlockDirection direction)
|
|||
switch (direction)
|
||||
{
|
||||
case FRONT:
|
||||
BUZZER_StartTimed(10);
|
||||
return has_block_front;
|
||||
// BUZZER_StartTimed(10);
|
||||
if (has_block_front)
|
||||
{
|
||||
if (block_time > MIN_BLOCK_TIME)
|
||||
{
|
||||
block_time = 0;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case LEFT_FRONT:
|
||||
BUZZER_StartTimed(2);
|
||||
return has_block_left;
|
||||
// BUZZER_StartTimed(2);
|
||||
if (has_block_left)
|
||||
{
|
||||
if (block_time > MIN_BLOCK_TIME)
|
||||
{
|
||||
block_time = 0;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case RIGHT_FRONT:
|
||||
BUZZER_StartTimed(2);
|
||||
return has_block_right;
|
||||
// BUZZER_StartTimed(2);
|
||||
if (has_block_right)
|
||||
{
|
||||
if (block_time > MIN_BLOCK_TIME)
|
||||
{
|
||||
block_time = 0;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
|
|
|
@ -30,8 +30,13 @@ void PathPlanner_Init()
|
|||
void SetPath()
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 100; i++)
|
||||
for (i = 0; i < 10; i++)
|
||||
{
|
||||
// AddPathStep(LEFT);
|
||||
// AddPathStep(LEFT);
|
||||
// AddPathStep(LEFT);
|
||||
|
||||
|
||||
AddPathStep(STRAIGHT);
|
||||
AddPathStep(STRAIGHT);
|
||||
AddPathStep(STRAIGHT);
|
||||
|
|
|
@ -1,35 +1,22 @@
|
|||
#include "timer.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "tim.h"
|
||||
|
||||
typedef enum
|
||||
{
|
||||
INFINITE_LOOP_EVENT, // 无限循环事件
|
||||
FINITE_LOOP_EVENT, // 有限循环事件
|
||||
FINITE_LOOP_EVENT_WITH_PARAM,
|
||||
DELAY_EVENT // 延迟事件
|
||||
} EventType;
|
||||
#include <math.h>
|
||||
|
||||
typedef struct Event
|
||||
{
|
||||
uint8_t id;
|
||||
void (*func)(void);
|
||||
void (*param_func)(uint16_t);
|
||||
uint16_t time;
|
||||
EventType type;
|
||||
uint8_t enabled;
|
||||
uint16_t loop_cnt;
|
||||
uint32_t begin_time;
|
||||
uint16_t param;
|
||||
} Event;
|
||||
|
||||
static Event event[EVENT_MAX];
|
||||
static int event_cnt;
|
||||
static uint32_t current_time;
|
||||
static uint32_t time_cnt;
|
||||
static uint32_t time_lcm = 1;
|
||||
|
||||
void TIMER_Count(TIM_HandleTypeDef *htim);
|
||||
uint32_t gcd(uint32_t a, uint32_t b);
|
||||
uint32_t lcm(uint32_t a, uint32_t b);
|
||||
|
||||
void TIMER_Count(void);
|
||||
|
||||
void TIMER_Init(void)
|
||||
{
|
||||
|
@ -37,124 +24,41 @@ void TIMER_Init(void)
|
|||
HAL_TIM_Base_Start_IT(&htim4);
|
||||
}
|
||||
|
||||
/// @brief 注册无限循环定时回调事件
|
||||
/// @brief 注册定时回调事件
|
||||
/// @param func 回调函数 void func(void)
|
||||
/// @param loop_time 定时时间,单位毫秒
|
||||
int8_t TIMER_AddInfiniteLoopEvent(int8_t event_id, void (*func)(void), uint16_t loop_time)
|
||||
void TIMER_AddLoopEvent(uint8_t type, void (*func)(void), uint16_t loop_time)
|
||||
{
|
||||
if (event_id == -1) event_id = event_cnt++;
|
||||
event[event_id].type = INFINITE_LOOP_EVENT;
|
||||
event[event_id].id = event_id;
|
||||
event[event_id].func = func;
|
||||
event[event_id].time = loop_time;
|
||||
event[event_id].begin_time = current_time;
|
||||
event[event_id].enabled = 1;
|
||||
return event_id;
|
||||
event[type].func = func;
|
||||
event[type].time = loop_time;
|
||||
event[type].enabled = 1;
|
||||
time_lcm = lcm(time_lcm, loop_time);
|
||||
}
|
||||
|
||||
/// @brief 注册有限循环定时回调事件
|
||||
/// @param func 回调函数 void func(void)
|
||||
/// @param loop_time 定时时间,单位毫秒
|
||||
/// @param loop_cnt 循环次数
|
||||
int8_t TIMER_AddFiniteLoopEventWithParam(int8_t event_id, void (*func)(uint16_t param), uint16_t loop_time, uint16_t loop_cnt, uint16_t param)
|
||||
void TIMER_DelLoopEvent(uint8_t type)
|
||||
{
|
||||
if (event_id == -1) event_id = event_cnt++;
|
||||
event[event_id].type = FINITE_LOOP_EVENT_WITH_PARAM;
|
||||
event[event_id].id = event_id;
|
||||
event[event_id].param_func = func;
|
||||
event[event_id].time = loop_time;
|
||||
event[event_id].begin_time = current_time;
|
||||
event[event_id].enabled = 1;
|
||||
event[event_id].param = param;
|
||||
event[event_id].loop_cnt = loop_cnt;
|
||||
return event_id;
|
||||
event[type].enabled = 0;
|
||||
time_lcm /= event[type].time;
|
||||
}
|
||||
|
||||
int8_t TIMER_AddFiniteLoopEvent(int8_t event_id, void (*func)(void), uint16_t loop_time, uint16_t loop_cnt)
|
||||
void TIMER_Count(void)
|
||||
{
|
||||
if (event_id == -1) event_id = event_cnt++;
|
||||
event[event_id].type = FINITE_LOOP_EVENT;
|
||||
event[event_id].id = event_id;
|
||||
event[event_id].func = func;
|
||||
event[event_id].time = loop_time;
|
||||
event[event_id].begin_time = current_time;
|
||||
event[event_id].enabled = 1;
|
||||
event[event_id].loop_cnt = loop_cnt;
|
||||
return event_id;
|
||||
++time_cnt;
|
||||
for (uint8_t i = 0; i < EVENT_MAX; ++i)
|
||||
if (event[i].enabled && time_cnt % event[i].time == 0)
|
||||
event[i].func();
|
||||
if (time_cnt == time_lcm)
|
||||
time_cnt = 0;
|
||||
}
|
||||
|
||||
/// @brief 注册延迟定时回调事件(单次)
|
||||
/// @param func 回调函数 void func(void)
|
||||
/// @param loop_time 定时时间,单位毫秒
|
||||
int8_t TIMER_AddDelayEvent(int8_t event_id, void (*func)(void), uint16_t delay_time)
|
||||
uint32_t gcd(uint32_t a, uint32_t b)
|
||||
{
|
||||
if (event_id == -1) event_id = event_cnt++;
|
||||
event[event_id].type = DELAY_EVENT;
|
||||
event[event_id].id = event_id;
|
||||
event[event_id].func = func;
|
||||
event[event_id].time = delay_time;
|
||||
event[event_id].begin_time = current_time;
|
||||
event[event_id].enabled = 1;
|
||||
return event_id;
|
||||
if (b == 0)
|
||||
return a;
|
||||
return gcd(b, a % b);
|
||||
}
|
||||
|
||||
void TIMER_DelLoopEvent(int8_t id)
|
||||
uint32_t lcm(uint32_t a, uint32_t b)
|
||||
{
|
||||
event[id].enabled = 0;
|
||||
}
|
||||
|
||||
void TIMER_Count(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
++current_time;
|
||||
for (uint8_t i = 0; i < event_cnt; ++i)
|
||||
{
|
||||
if (event[i].enabled == 0)
|
||||
continue;
|
||||
|
||||
switch (event[i].type)
|
||||
{
|
||||
case INFINITE_LOOP_EVENT: // 无限循环事件
|
||||
if ((current_time - event[i].begin_time) % event[i].time == 0)
|
||||
{
|
||||
event[i].func();
|
||||
}
|
||||
break;
|
||||
case FINITE_LOOP_EVENT: // 有限循环事件
|
||||
if ((current_time - event[i].begin_time) % event[i].time == 0)
|
||||
{
|
||||
event[i].func();
|
||||
if (event[i].loop_cnt-- <= 0)
|
||||
{
|
||||
TIMER_DelLoopEvent(event[i].id);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case FINITE_LOOP_EVENT_WITH_PARAM:
|
||||
if ((current_time - event[i].begin_time) % event[i].time == 0)
|
||||
{
|
||||
printf("FINITE_LOOP_EVENT_WITH_PARAM %d, %d, %d\n",
|
||||
current_time, event[i].begin_time, event[i].time);
|
||||
event[i].param_func(event[i].param);
|
||||
if (--event[i].loop_cnt <= 0)
|
||||
{
|
||||
printf("remove FINITE_LOOP_EVENT_WITH_PARAM %d, %d, %d\n",
|
||||
current_time, event[i].begin_time, event[i].time);
|
||||
TIMER_DelLoopEvent(event[i].id);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case DELAY_EVENT: // 延迟事件
|
||||
if (current_time > event[i].begin_time &&
|
||||
(current_time - event[i].begin_time) % event[i].time == 0)
|
||||
{
|
||||
event[i].func();
|
||||
printf("DELAY_EVENT %d, %d, %d\n",
|
||||
current_time, event[i].begin_time, event[i].time);
|
||||
TIMER_DelLoopEvent(event[i].id);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
return a / gcd(a, b) * b;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue
Block a user