补充代码声光提示函数部分void sound_light_alert(void)，芯片是M0G3507，使用蜂鸣器发声（低电平触发），LED发光（高电平触发），使用I/O口为PA1,PA2。#include <math.h> // 添加数学库用于fabs函数
#include "board.h"
#include "my_time.h"
void go_straight(int dis);
void go_arc_ccd(void);
void go_ccd_line(void);
void turn_in_place(float angle);
void sound_light_alert(void);
u8 Car_Mode = Diff_Car;
int Motor_Left, Motor_Right; // 电机PWM变量 应是Motor的
u8 PID_Send; // 延时和调参相关变量
float RC_Velocity = 200, RC_Turn_Velocity, Move_X, Move_Y, Move_Z, PS2_ON_Flag; // 遥控控制的速度
float Velocity_Left, Velocity_Right; // 车轮速度(mm/s)
u16 test_num, show_cnt;
float Voltage = 0;
extern float Yaw; // 声明外部YAW角度变量
uint64_t left_encoder = 0, right_encoder = 0;
void SysTick_Handler(void) { hsu_time_systick_handler(); }
typedef enum { BEGIN, T1, T2, T3, T4 } TaskState;
typedef enum { STOP, GO_STRAIGHT, GO_CCD, TURN_IN_PLACE, WAIT_ALERT } DoingWhat;
typedef struct __TASK_NAMESPACE {
uint8_t is_running;
uint8_t finish;
TaskState state;
DoingWhat doing_what;
float target;
float vx;
float vz;
// 用于复杂任务
uint8_t sub_task_stage; // 子任务阶段
uint8_t lap_count; // 圈数计数
uint64_t start_encoder; // 起始编码器值
uint32_t alert_start_time; // 声光提示开始时间
float start_yaw; // 起始YAW角度
float target_yaw_diff; // 目标YAW角度差
} TaskNamespace;
void reset_task_namespace(TaskNamespace *t) {
t->is_running = 0;
t->finish = 0;
t->state = BEGIN;
t->doing_what = STOP;
t->vx = 0;
t->vz = 0;
t->sub_task_stage = 0;
t->lap_count = 0;
t->start_encoder = left_encoder;
t->alert_start_time = 0;
t->start_yaw = 0;
t->target_yaw_diff = 0;
}
void next_state(TaskNamespace *t) {
TaskState last_state = t->state;
reset_task_namespace(t);
if (last_state < T4) {
t->state = last_state + 1;
}
}
TaskNamespace task_namespace;
void main_key(void) {
u8 tmp;
tmp = key_scan(200); // click_N_Double(50);
if (tmp == 1) {
next_state(&task_namespace);
} else if (tmp == 2) {
reset_task_namespace(&task_namespace);
}
}
void show_task_now(void) {
OLED_ShowString(0, 0, (uint8_t *)"Task Now:");
switch (task_namespace.state) {
case BEGIN:
OLED_ShowString(90, 0, (uint8_t *)"NO TASK");
break;
case T1:
OLED_ShowString(90, 0, (uint8_t *)"TASK 1");
break;
case T2:
OLED_ShowString(90, 0, (uint8_t *)"TASK 2");
break;
case T3:
OLED_ShowString(90, 0, (uint8_t *)"TASK 3");
break;
case T4:
OLED_ShowString(90, 0, (uint8_t *)"TASK 4");
break;
default:
break;
}
}
void main_task(void);
int main(void) {
// 系统初始化
SYSCFG_DL_init(); // 初始化系统配置
hsu_time_init(); // 时间
// 清除所有外设的中断挂起状态
NVIC_ClearPendingIRQ(ENCODERA_INT_IRQN); // 编码器A中断
NVIC_ClearPendingIRQ(ENCODERB_INT_IRQN); // 编码器B中断
NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN); // UART0串口中断
NVIC_ClearPendingIRQ(UART_1_INST_INT_IRQN); // UART1串口中断
// 使能各外设的中断
NVIC_EnableIRQ(ENCODERA_INT_IRQN); // 开启编码器A中断
NVIC_EnableIRQ(ENCODERB_INT_IRQN); // 开启编码器B中断
NVIC_EnableIRQ(UART_0_INST_INT_IRQN); // 开启UART0中断
NVIC_EnableIRQ(UART_1_INST_INT_IRQN); // 开启UART1中断
// 定时器和ADC相关中断配置
NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); // 清除定时器0中断挂起
NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); // 开启定时器0中断
NVIC_EnableIRQ(ADC12_VOLTAGE_INST_INT_IRQN);
NVIC_EnableIRQ(ADC12_CCD_INST_INT_IRQN);
OLED_Init(); // 初始化OLED显示屏
MPU6050_initialize();
DMP_Init();
// 主循环
printf("Test delay 1s\n");
hsu_time_delay_ms(1000);
printf("Test delay 1s end\n");
printf("Test delay 500us\n");
hsu_time_delay_us(500);
printf("Test delay 500us end\n");
uint8_t main_task_timer = hsu_time_timer_create(10, true, main_task);
hsu_time_timer_start(main_task_timer);
while (1) {
hsu_time_timer_process();
RD_TSL(); // 读取CCD数据
Find_CCD_Median(); // 计算CCD数据中值
Read_DMP();
show_task_now();
printf("L=%d R=%d YAW=%.1f\n", left_encoder, right_encoder, Yaw);
}
}
void task_no(void);
void task_1(void);
void task_2(void);
void task_3(void);
void task_4(void);
void main_task(void) {
switch (task_namespace.state) {
case BEGIN:
task_no();
break;
case T1:
task_1();
break;
case T2:
task_2();
break;
case T3:
task_3();
break;
case T4:
task_4();
break;
default:
break;
}
switch (task_namespace.doing_what) {
case STOP:
Get_Target_Encoder(0, 0);
break;
case GO_STRAIGHT:
if (left_encoder < task_namespace.target) {
Get_Target_Encoder(0.2, 0); // 提高速度到200mm/s
} else {
Get_Target_Encoder(0, 0);
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
}
break;
case GO_CCD:
CCD_Mode(); // 使用CCD巡线
// CCD巡线没有固定的结束条件，需要在任务中手动设置finish
break;
case TURN_IN_PLACE:
// 原地转向控制
if (task_namespace.target_yaw_diff != 0) {
float current_yaw_diff = Yaw - task_namespace.start_yaw;
// 处理角度跨越±180度的情况
if (current_yaw_diff > 180) {
current_yaw_diff -= 360;
} else if (current_yaw_diff < -180) {
current_yaw_diff += 360;
}
printf("Turn: Start=%.1f Current=%.1f Diff=%.1f Target=%.1f\n", task_namespace.start_yaw, Yaw, current_yaw_diff,
task_namespace.target_yaw_diff);
// 检查是否达到目标角度
if ((task_namespace.target_yaw_diff > 0 && current_yaw_diff >= task_namespace.target_yaw_diff) ||
(task_namespace.target_yaw_diff < 0 && current_yaw_diff <= task_namespace.target_yaw_diff)) {
Get_Target_Encoder(0, 0); // 停止转向
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
} else {
// 继续转向
float turn_speed = (task_namespace.target_yaw_diff > 0) ? 0.5 : -0.5;
Get_Target_Encoder(0, turn_speed);
}
}
break;
case WAIT_ALERT:
Get_Target_Encoder(0, 0); // 停车
if (hsu_time_get_ms() - task_namespace.alert_start_time > 1000) { // 声光提示1秒
task_namespace.doing_what = STOP;
task_namespace.finish = 1;
}
break;
default:
break;
}
}
void task_no(void) { return; }
// 任务1：A点到B点直线行驶
void task_1(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
go_straight(1000); // A到B大约1000mm
return;
}
if (task_namespace.finish) {
sound_light_alert(); // 声光提示
reset_task_namespace(&task_namespace);
}
return;
}
// 任务2：A->B->C->D->A循环
void task_2(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0; // 开始第一阶段：A到B
go_straight(1000);
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0: // A到B完成，开始B到C弧线
sound_light_alert();
task_namespace.sub_task_stage = 1;
go_arc_ccd();
hsu_time_delay_ms(100); // 短暂延时
break;
case 1: // B到C弧线完成，开始C到D直线
sound_light_alert();
task_namespace.sub_task_stage = 2;
go_straight(1000);
break;
case 2: // C到D完成，开始D到A弧线
sound_light_alert();
task_namespace.sub_task_stage = 3;
go_arc_ccd();
hsu_time_delay_ms(100);
break;
case 3: // D到A弧线完成，任务结束
sound_light_alert();
reset_task_namespace(&task_namespace);
break;
}
}
return;
}
// 任务3：A->C->B->D->A循环
void task_3(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0; // 开始第一阶段：A到C弧线
go_arc_ccd();
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0: // A到C弧线完成，开始C到B弧线
sound_light_alert();
task_namespace.sub_task_stage = 1;
go_arc_ccd();
hsu_time_delay_ms(100);
break;
case 1: // C到B弧线完成，开始B到D直线
sound_light_alert();
task_namespace.sub_task_stage = 2;
go_straight(1000);
break;
case 2: // B到D完成，开始D到A弧线
sound_light_alert();
task_namespace.sub_task_stage = 3;
go_arc_ccd();
hsu_time_delay_ms(100);
break;
case 3: // D到A弧线完成，任务结束
sound_light_alert();
reset_task_namespace(&task_namespace);
break;
}
}
return;
}
// 任务4：重复任务3路径4圈
void task_4(void) {
if (!task_namespace.is_running) {
task_namespace.is_running = 1;
task_namespace.sub_task_stage = 0;
task_namespace.lap_count = 0;
turn_in_place(40.0f); // 开始第一圈：A点原地右转40°
return;
}
if (task_namespace.finish) {
switch (task_namespace.sub_task_stage) {
case 0: // A点右转40°完成，开始直线125cm到C
sound_light_alert();
task_namespace.sub_task_stage = 1;
go_straight(1250);
hsu_time_delay_ms(100);
break;
case 1: // A到C直线完成，原地左转40°
sound_light_alert();
task_namespace.sub_task_stage = 2;
turn_in_place(-40.0f);
hsu_time_delay_ms(100);
break;
case 2: // C点左转40°完成，CCD巡线到B
sound_light_alert();
task_namespace.sub_task_stage = 3;
go_ccd_line();
hsu_time_delay_ms(100);
break;
case 3: // C到B巡线完成，左转40°
sound_light_alert();
task_namespace.sub_task_stage = 4;
turn_in_place(-40.0f);
hsu_time_delay_ms(100);
break;
case 4: // B点左转40°完成，直线125cm到D
sound_light_alert();
task_namespace.sub_task_stage = 5;
go_straight(1250);
hsu_time_delay_ms(100);
break;
case 5: // B到D直线完成，右转40°
sound_light_alert();
task_namespace.sub_task_stage = 6;
turn_in_place(40.0f);
hsu_time_delay_ms(100);
break;
case 6: // D点右转40°完成，CCD巡线回A
sound_light_alert();
task_namespace.sub_task_stage = 7;
go_ccd_line();
hsu_time_delay_ms(100);
break;
case 7: // D到A巡线完成，一圈结束
sound_light_alert();
task_namespace.lap_count++;
if (task_namespace.lap_count < 4) {
// 还需要继续下一圈
task_namespace.sub_task_stage = 0;
turn_in_place(40.0f); // 开始下一圈：A点右转40°
} else {
// 4圈完成，任务结束
reset_task_namespace(&task_namespace);
}
break;
}
}
return;
}
void TIMER_0_INST_IRQHandler(void) {
if (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) {
if (DL_TIMER_IIDX_ZERO) {
main_key();
Get_Velocity_From_Encoder(Get_Encoder_countA, Get_Encoder_countB);
Get_Encoder_countA = Get_Encoder_countB = 0;
MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder);
MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder);
if (!Flag_Stop) {
Set_PWM(-MotorA.Motor_Pwm, -MotorB.Motor_Pwm);
} else {
Set_PWM(0, 0);
}
}
}
}
uint32_t gpio_interrup1, gpio_interrup2;
void GROUP1_IRQHandler(void) {
// 获取中断信号
gpio_interrup1 = DL_GPIO_getEnabledInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN);
gpio_interrup2 = DL_GPIO_getEnabledInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN);
// encoderB
if ((gpio_interrup1 & ENCODERA_E1A_PIN) == ENCODERA_E1A_PIN) {
if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1B_PIN)) {
Get_Encoder_countB--;
right_encoder--;
} else {
Get_Encoder_countB++;
right_encoder++;
}
} else if ((gpio_interrup1 & ENCODERA_E1B_PIN) == ENCODERA_E1B_PIN) {
if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1A_PIN)) {
Get_Encoder_countB++;
right_encoder++;
} else {
Get_Encoder_countB--;
right_encoder--;
}
}
// encoderA
if ((gpio_interrup2 & ENCODERB_E2A_PIN) == ENCODERB_E2A_PIN) {
if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2B_PIN)) {
Get_Encoder_countA--;
left_encoder--;
} else {
left_encoder++;
Get_Encoder_countA++;
}
} else if ((gpio_interrup2 & ENCODERB_E2B_PIN) == ENCODERB_E2B_PIN) {
if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2A_PIN)) {
left_encoder++;
Get_Encoder_countA++;
} else {
left_encoder--;
Get_Encoder_countA--;
}
}
DL_GPIO_clearInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN);
DL_GPIO_clearInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN);
}
// 直线行驶函数
void go_straight(int dis) {
task_namespace.doing_what = GO_STRAIGHT;
task_namespace.target = left_encoder + dis;
task_namespace.finish = 0;
}
// 原地转向函数
void turn_in_place(float angle) {
task_namespace.doing_what = TURN_IN_PLACE;
task_namespace.start_yaw = Yaw;
task_namespace.target_yaw_diff = angle; // 正值右转，负值左转
task_namespace.finish = 0;
}
// CCD巡线函数（需要外部条件结束）
void go_ccd_line(void) {
task_namespace.doing_what = GO_CCD;
task_namespace.start_encoder = left_encoder;
task_namespace.finish = 0;
// 设置一个安全的最大距离，防止无限巡线
// 可以根据实际场地调整这个值
static uint32_t ccd_start_time = 0;
if (ccd_start_time == 0) {
ccd_start_time = hsu_time_get_ms();
}
// 如果巡线时间超过10秒或距离超过2000mm，强制结束
if (hsu_time_get_ms() - ccd_start_time > 10000 || (left_encoder - task_namespace.start_encoder) > 2000) {
task_namespace.finish = 1;
ccd_start_time = 0;
}
}
// 弧线CCD巡线函数
void go_arc_ccd(void) {
task_namespace.doing_what = GO_CCD;
task_namespace.start_encoder = left_encoder;
task_namespace.start_yaw = Yaw; // 记录起始YAW角度
task_namespace.target_yaw_diff = 180.0f; // 半圆弧需要转180度
task_namespace.finish = 0;
}
// 声光提示函数
void sound_light_alert(void) {
// 这里可以添加蜂鸣器和LED的控制代码
LED_Flash(10); // LED闪烁
// 添加蜂鸣器控制
}
最新发布
扫一扫
3509
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
