#include "ti_msp_dl_config.h"
#include "main.h"
char str[16];
int16_t AR_Speed, AR_Location;
int16_t AL_Speed, AL_Location;
float relative_yaw, best_yaw, yaw_offset;
// 转向控制变量
enum { MODE_STRAIGHT, MODE_TURN_LEFT, MODE_TURN_RIGHT } drive_mode = MODE_STRAIGHT;
uint32_t turn_start_time = 0;
const uint32_t TURN_DURATION = 800; // 转向持续时间 (ms)
const float TURN_ANGLE = 90.0f; // 转向角度 (度)
// 状态跟踪变量
static uint8_t action_index = 0; // 当前动作索引
static uint32_t straight_start_time = 0; // 直行开始时间
static uint8_t left_turn_count = 0; // 已执行左转次数
static uint8_t right_turn_count = 0; // 已执行右转次数
// 定义任意顺序的动作序列
#define ACTION_COUNT 10 // 8个动作:4次转向 + 4次直行
typedef enum { ACTION_STRAIGHT, ACTION_LEFT_TURN, ACTION_RIGHT_TURN } ActionType;
// 自定义序列: 左-直-右-直-右-直-左-直
const ActionType custom_sequence[ACTION_COUNT] = {
ACTION_LEFT_TURN, // 左转
ACTION_STRAIGHT, // 直行
ACTION_RIGHT_TURN, // 右转
ACTION_STRAIGHT, // 直行
ACTION_RIGHT_TURN, // 右转
ACTION_STRAIGHT, // 直行
ACTION_LEFT_TURN, // 左转
ACTION_STRAIGHT, // 直行
ACTION_LEFT_TURN, // 左转
ACTION_STRAIGHT // 直行
};
// 每个动作的等待时间 (ms)
const uint32_t WAIT_TIMES[ACTION_COUNT] = {
1200, // 左转1前等待
0, // 直行1持续时间
800, // 右转1前等待
0, // 直行2持续时间
800, // 右转2前等待
0, // 直行3持续时间
800, // 左转2前等待
0, // 直行4持续时间
800, // 左转2前等待
0 // 直行4持续时间
};
// 动作描述(用于显示)
const char* ACTION_DESCRIPTIONS[3] = {
"STRAIGHT",
"LEFT TURN",
"RIGHT TURN"
};
PID_t AR_Inner = {
.Kp = 0.85,
.Ki = 0.1,
.Kd = 0,
.OutMax = 100,
.OutMin = -100,
};
PID_t AL_Inner = {
.Kp = 0.85,
.Ki = 0.1,
.Kd = 0,
.OutMax = 100,
.OutMin = -100,
};
PID_t angle = {
.Kp = 0.15,
.Ki = 0,
.Kd = 0,
.OutMax = 60,
.OutMin = -60,
};
// 启动左转
void start_left_turn(void) {
drive_mode = MODE_TURN_LEFT;
turn_start_time = tick_ms;
left_turn_count++;
}
// 启动右转
void start_right_turn(void) {
drive_mode = MODE_TURN_RIGHT;
turn_start_time = tick_ms;
right_turn_count++;
}
// 进入直行模式
void enter_straight_mode(void) {
drive_mode = MODE_STRAIGHT;
straight_start_time = tick_ms;
angle.Target = 0; // 重置角度目标
}
// 执行当前序列动作
void execute_current_action(void) {
ActionType current_action = custom_sequence[action_index];
switch (current_action) {
case ACTION_LEFT_TURN:
start_left_turn();
break;
case ACTION_RIGHT_TURN:
start_right_turn();
break;
case ACTION_STRAIGHT:
enter_straight_mode();
break;
}
// 更新到下一个动作
action_index = (action_index + 1) % ACTION_COUNT;
}
int main(void) {
SYSCFG_DL_init();
SysTick_Init();
WIT_Init();
lcd_init();
PID_Init();
encoder_Init();
// 初始状态为直行
enter_straight_mode();
while (1) {
// 检查是否应该执行下一个动作
if (drive_mode == MODE_STRAIGHT) {
// 检查是否达到当前动作的等待时间
if (tick_ms - straight_start_time > WAIT_TIMES[action_index]) {
execute_current_action();
}
}
// 显示当前模式
const char* mode_str;
switch (drive_mode) {
case MODE_STRAIGHT: mode_str = "STRAIGHT"; break;
case MODE_TURN_LEFT: mode_str = "LEFT TURN"; break;
case MODE_TURN_RIGHT: mode_str = "RIGHT TURN"; break;
default: mode_str = "UNKNOWN"; break;
}
sprintf(str, "Mode: %s", mode_str);
LCD_ShowString(0, 48, (unsigned char *)str, RED, WHITE, 16, 0);
// 显示当前动作序列
ActionType current_action = custom_sequence[action_index];
sprintf(str, "Action: %s", ACTION_DESCRIPTIONS[current_action]);
LCD_ShowString(0, 64, (unsigned char *)str, RED, WHITE, 16, 0);
// 显示当前状态持续时间
uint32_t elapsed = 0;
uint32_t total_duration = 0;
const char* time_label = "";
if (drive_mode == MODE_STRAIGHT) {
elapsed = tick_ms - straight_start_time;
total_duration = WAIT_TIMES[action_index];
time_label = "Straight:";
} else {
elapsed = tick_ms - turn_start_time;
total_duration = TURN_DURATION;
time_label = "Turn:";
}
sprintf(str, "%s %d/%d", time_label, elapsed, total_duration);
LCD_ShowString(0, 80, (unsigned char *)str, RED, WHITE, 16, 0);
// 显示动作序列索引
sprintf(str, "Index: %u/%d", action_index, ACTION_COUNT-1);
LCD_ShowString(0, 96, (unsigned char *)str, RED, WHITE, 16, 0);
// 显示转向计数
sprintf(str, "Turns: L:%u R:%u", left_turn_count, right_turn_count);
LCD_ShowString(0, 112, (unsigned char *)str, RED, WHITE, 16, 0);
// 显示tick_ms
sprintf(str, "tick: %lu", tick_ms);
LCD_ShowString(0, 128, (unsigned char *)str, RED, WHITE, 16, 0);
// 原始显示代码保持不变
sprintf(str, "AR_Target:%+04.0f", AR_Inner.Target);
LCD_ShowString(200, 0, (unsigned char *)str, RED, WHITE, 16, 0);
sprintf(str, "AR_Out:%+04.0f", AR_Inner.Out);
LCD_ShowString(200, 16, (unsigned char *)str, RED, WHITE, 16, 0);
sprintf(str, "AR_Actual:%+04.0f", AR_Inner.Actual);
LCD_ShowString(200, 32, (unsigned char *)str, RED, WHITE, 16, 0);
sprintf(str, "AL_Target:%+04.0f", AL_Inner.Target);
LCD_ShowString(0, 0, (unsigned char *)str, RED, WHITE, 16, 0);
sprintf(str, "AL_Out:%+04.0f", AL_Inner.Out);
LCD_ShowString(0, 16, (unsigned char *)str, RED, WHITE, 16, 0);
sprintf(str, "AL_Actual:%+04.0f", AL_Inner.Actual);
LCD_ShowString(0, 32, (unsigned char *)str, RED, WHITE, 16, 0);
sprintf(str, "yaw:%-6.1f", relative_yaw);
LCD_ShowString(0, 144, (unsigned char *)str, RED, WHITE, 16, 0);
}
}
void TIMER_0_INST_IRQHandler(void) {
switch (DL_TimerG_getPendingInterrupt(TIMER_0_INST)) {
case DL_TIMER_IIDX_ZERO:
encoder_value_up();
// 转向控制逻辑
float turn_control = 0;
if (drive_mode == MODE_TURN_LEFT || drive_mode == MODE_TURN_RIGHT) {
// 检查转向是否完成
if (tick_ms - turn_start_time > TURN_DURATION) {
// 转向完成,进入下一个动作
execute_current_action();
} else {
// 设置转向控制量(左转为正,右转为负)
turn_control = (drive_mode == MODE_TURN_LEFT) ? TURN_ANGLE : -TURN_ANGLE;
}
}
// 角度控制
angle.Target = turn_control;
angle.Actual = relative_yaw;
PID_Update(&angle);
// 速度控制
float forward_speed = 40;
AR_Inner.Target = forward_speed + angle.Out;
AL_Inner.Target = forward_speed - angle.Out;
AR_Speed = AR_encoder();
AL_Speed = AL_encoder();
AR_Location += AR_Speed;
AL_Location += AL_Speed;
AR_Inner.Actual = AR_Speed;
AL_Inner.Actual = AL_Speed;
PID_Update(&AR_Inner);
PID_Update(&AL_Inner);
AR_MOTOR(AR_Inner.Out);
AL_MOTOR(AL_Inner.Out);
best_yaw = InfiniteYaw(wit_data.yaw);
static int first_time = 1;
if (first_time) {
yaw_offset = best_yaw;
first_time = 0;
}
relative_yaw = best_yaw - yaw_offset;
break;
default:
break;
}
}为什么我的代码无法实现第三次左转