401 lines
14 KiB
C++
401 lines
14 KiB
C++
#include <ti_msp_dl_config.h>
|
||
#include <FreeRTOS.h>
|
||
#include <task.h>
|
||
|
||
#include <Print.h>
|
||
#include <Encoder.h>
|
||
#include <Imu.h>
|
||
#include <Motor.h>
|
||
#include <TrackerMpu.h>
|
||
#include <TrackerGpio.h>
|
||
#include <2025_include_user/Beep.h>
|
||
#include <Host.h>
|
||
#include <Key.h>
|
||
#include <Led.h>
|
||
#include <Servo.h>
|
||
#include <Stepmotor.h>
|
||
#include <Oled.hpp>
|
||
#include <OledMenuDriver.h>
|
||
#include <OledMenuPaint.h>
|
||
#include <TrackerGpio.h>
|
||
#include <TrackerGpio.h>
|
||
|
||
|
||
/************ 外部中断测试部分 ************/
|
||
|
||
extern "C" void GROUP1_IRQHandler() {
|
||
//读取Group1的中断寄存器并清除中断标志位
|
||
switch (DL_Interrupt_getPendingGroup(DL_INTERRUPT_GROUP_1)) {
|
||
//检查是否是KEY的GPIOB端口中断，注意是INT_IIDX，不是PIN_22_IIDX
|
||
case Key_GPIOA_INT_IIDX:
|
||
case GPIO_MULTIPLE_GPIOB_INT_IIDX: {
|
||
const uint32_t statusA = DL_GPIO_getPendingInterrupt(GPIOA);
|
||
const uint32_t statusB = DL_GPIO_getPendingInterrupt(GPIOB);
|
||
// print("%d\n", statusA);
|
||
// print("%d\n", statusB);
|
||
if (statusA == KEY1_PIN_ID + 1) {
|
||
Key::keys[KEY1].state = 1;
|
||
} else if (statusB == KEY2_PIN_ID + 1) {
|
||
Key::keys[KEY2].state = 1;
|
||
} else if (statusB == KEY3_PIN_ID + 1) {
|
||
Key::keys[KEY3].state = 1;
|
||
} else if (statusB == KEY4_PIN_ID + 1) {
|
||
Key::keys[KEY4].state = 1;
|
||
}
|
||
// TrackerGpio::inject(Tracker_Track1_PIN, DL_GPIO_readPins(Tracker_Track1_PORT, Tracker_Track1_PIN) > 0);
|
||
// TrackerGpio::inject(Tracker_Track2_PIN, DL_GPIO_readPins(Tracker_Track2_PORT, Tracker_Track2_PIN) > 0);
|
||
// TrackerGpio::inject(Tracker_Track3_PIN, DL_GPIO_readPins(Tracker_Track3_PORT, Tracker_Track3_PIN) > 0);
|
||
// TrackerGpio::inject(Tracker_Track4_PIN, DL_GPIO_readPins(Tracker_Track4_PORT, Tracker_Track4_PIN) > 0);
|
||
// TrackerGpio::inject(Tracker_Track5_PIN, DL_GPIO_readPins(Tracker_Track5_PORT, Tracker_Track5_PIN) > 0);
|
||
// TrackerGpio::inject(Tracker_Track6_PIN, DL_GPIO_readPins(Tracker_Track6_PORT, Tracker_Track6_PIN) > 0);
|
||
// TrackerGpio::inject(Tracker_Track7_PIN, DL_GPIO_readPins(Tracker_Track7_PORT, Tracker_Track7_PIN) > 0);
|
||
DL_GPIO_clearInterruptStatus(GPIOA, statusA);
|
||
DL_GPIO_clearInterruptStatus(GPIOB, statusB);
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/************ FreeRtos 任务定义区 ************/
|
||
|
||
[[noreturn]] void vLedTask(void *pvParameters) {
|
||
while (true) {
|
||
Led::on(LED0);
|
||
// DL_GPIO_setPins(Lights_PORT, Lights_Light_PIN);
|
||
vTaskDelay(pdMS_TO_TICKS(500)); // 每500毫秒切换一次LED状态
|
||
Led::off(LED0);
|
||
// DL_GPIO_clearPins(Lights_PORT, Lights_Light_PIN);
|
||
vTaskDelay(pdMS_TO_TICKS(500)); // 每500毫秒切换一次LE+D状态
|
||
}
|
||
}
|
||
|
||
[[noreturn]] void vOledTask(void *pvParameters) {
|
||
MenuDriver::init();
|
||
MenuPaint::Init();
|
||
while (true) {
|
||
const int direction = (int) Host::get("Direction", 0);
|
||
if (Key::getState(KEY1 || direction == 2)) {
|
||
MenuPaint::ScrollUp();
|
||
}
|
||
if (Key::getState(KEY2) || direction == 8) {
|
||
MenuPaint::ScrollDown();
|
||
}
|
||
if (Key::getState(KEY3) || direction == 4) {
|
||
MenuPaint::Enter();
|
||
}
|
||
if (Key::getState(KEY4) || direction == 6) {
|
||
MenuPaint::Exit();
|
||
}
|
||
|
||
MenuPaint::Paint();
|
||
vTaskDelay(pdMS_TO_TICKS(10));
|
||
}
|
||
}
|
||
|
||
[[noreturn]] void vTestTask(void *pvParameters) {
|
||
while (true) {
|
||
// print("Ctrl: %f\n", Host::get("Ctrl", 0.0));
|
||
// print("x: %f\n", Host::get("x", 0.0));
|
||
// print("y: %f\n", Host::get("y", 0.0));
|
||
// Motor::setSpeed(3000, 3000);
|
||
// TrackerGpio::Data data = TrackerGpio::getData(); // 每位数据：0 表示压线，1 表示空白
|
||
// const char *d = data.data;
|
||
//
|
||
// print("Data:%d,%d,%d,%d,%d,%d,%d\n", d[0], d[1], d[2], d[3], d[4], d[5], d[6]);
|
||
// vTaskDelay(pdMS_TO_TICKS(100));
|
||
}
|
||
}
|
||
|
||
static uint8_t turn_limit = 0;
|
||
[[noreturn]] void vTrackerTask(void *pvParameters) {
|
||
// 计算偏移量（简单加权）
|
||
// 左侧为正，右侧为负
|
||
constexpr float weights[7] = {-2.1, -1.5, -0.8, 0, 0.8, 1.5, 2.1};
|
||
constexpr int base_speed = 4000; // 基础速度
|
||
constexpr int adjust_speed = 2000; // 差速调整速度
|
||
|
||
while (true) {
|
||
if (turn_limit <= 0) {
|
||
for (uint8_t i = 0; i < 5; i++) {
|
||
if (MenuPaint::GetSwitchState(i) == 1) {
|
||
turn_limit = 4 * (i + 1) + 1;
|
||
}
|
||
}
|
||
|
||
Motor::stop();
|
||
vTaskDelay(pdMS_TO_TICKS(50));
|
||
continue;
|
||
}
|
||
|
||
// 读取循迹传感器数据
|
||
TrackerGpio::Data data = TrackerGpio::getData(); // 每位数据：0 表示压线，1 表示空白
|
||
const char *d = data.data;
|
||
|
||
float error = 0;
|
||
int sum = 0;
|
||
|
||
for (int i = 0; i < 7; ++i) {
|
||
if (d[i] == 0) {
|
||
// 压到线
|
||
error += weights[i];
|
||
sum++;
|
||
}
|
||
}
|
||
|
||
if (sum == 0) {
|
||
turn_limit--;
|
||
if (turn_limit <= 0) {
|
||
Motor::stop();
|
||
vTaskDelay(pdMS_TO_TICKS(50));
|
||
continue;
|
||
}
|
||
|
||
uint8_t is_speed = (MenuPaint::GetSwitchState(5) == 1) ? 1 : 0;
|
||
uint8_t speed = is_speed ? 210 : 160;
|
||
Motor::setLeftSpeed(0);
|
||
Motor::setRightSpeed(speed * 10);
|
||
while (true) {
|
||
data = TrackerGpio::getData(); // 每位数据：0 表示压线，1 表示空白
|
||
const char *inner_d = data.data;
|
||
if (inner_d[3] == 0 || inner_d[2] == 0 || inner_d[1] == 0 || inner_d[0] == 0)
|
||
break;
|
||
Motor::setLeftSpeed(0);
|
||
Motor::setRightSpeed(speed * 10);
|
||
vTaskDelay(pdMS_TO_TICKS(50));
|
||
}
|
||
} else {
|
||
int left_speed = base_speed;
|
||
int right_speed = base_speed;
|
||
|
||
error = error / sum;
|
||
// 差速控制，误差越大，左右轮速度差越大
|
||
left_speed = (int) (base_speed - error * adjust_speed / 4);
|
||
right_speed = (int) (base_speed + error * adjust_speed / 4);
|
||
|
||
// 限制最大最小速度
|
||
if (left_speed > 10000) left_speed = 10000;
|
||
if (left_speed < -10000) left_speed = -10000;
|
||
if (right_speed > 10000) right_speed = 10000;
|
||
if (right_speed < -10000) right_speed = -10000;
|
||
|
||
// 设置电机速度
|
||
Motor::setLeftSpeed(left_speed);
|
||
Motor::setRightSpeed(right_speed);
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(50));
|
||
}
|
||
}
|
||
}
|
||
|
||
[[noreturn]] void vStepMotorTask(void *pvParameters) {
|
||
// // PID参数（根据实际调试再微调）
|
||
//
|
||
// const float integral_limit = 500.0f; // 积分限幅，避免积分快速累积
|
||
// const float DEAD_ZONE = 10.0f; // 误差死区：误差在±5像素内不动作
|
||
//
|
||
// float integral = 0.0f;
|
||
// float last_error = 0.0f;
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
||
while (true) {
|
||
// DL_TimerG_setCaptureCompareValue(Servo_INST, (uint32_t)800, GPIO_Servo_C0_IDX);
|
||
Servo::SetUpAngle(Host::getWithoutClean("angle", 87.0));
|
||
// Servo::SetDownAngle(120);
|
||
// // if (MenuPaint::GetSwitchState(5) == 0 || MenuPaint::GetSwitchState(6) == 0) {
|
||
// // vTaskDelay(pdMS_TO_TICKS(100));
|
||
// // continue;
|
||
// // }
|
||
//
|
||
// const double Kp = Host::getWithoutClean("kp", 0.2);
|
||
// const double Ki = Host::getWithoutClean("ki", 0.0005);
|
||
// const double Kd = Host::getWithoutClean("kd", 0.006);
|
||
// const double t_ = Host::posX; // 获取像素误差，范围0~240
|
||
// const uint8_t direction = (t_ > 0) ? 1 : 0;
|
||
// const double error = t_ > 0 ? t_ : -t_;
|
||
//
|
||
//
|
||
// print("Kp: %f, Ki: %f, Kd: %f\n", Kp, Ki, Kd);
|
||
//
|
||
// // 死区处理
|
||
// if (fabs(error) < DEAD_ZONE) {
|
||
// StepMotor::setPosition(0, 32, 0.0f, 0); // 停止动作
|
||
// integral = 0; // 死区内积分归零，避免积累
|
||
// // while (true) {
|
||
// // // TODO 激光ON
|
||
// // vTaskDelay(pdMS_TO_TICKS(1000));
|
||
// // // TODO 激光OFF
|
||
// // }
|
||
// } else {
|
||
// // 积分项累加 & 限幅
|
||
// integral += error;
|
||
// if (integral > integral_limit) integral = integral_limit;
|
||
// if (integral < -integral_limit) integral = -integral_limit;
|
||
//
|
||
// // 微分项
|
||
// float derivative = error - last_error;
|
||
//
|
||
// // PID输出
|
||
// float output = Kp * error + Ki * integral + Kd * derivative;
|
||
//
|
||
// // 限制输出范围（0.3° ~ 2.0°）
|
||
// if (output < 0.3f) output = 0.3f;
|
||
// if (output > 4.0f) output = 4.0f;
|
||
//
|
||
// // 控制步进电机
|
||
// StepMotor::setPosition(direction, 32, output, -10);
|
||
// // print("Motor: speed=%f, direction=%d\n", output, direction);
|
||
//
|
||
// last_error = error; // 保存上次误差
|
||
// }
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(500));
|
||
}
|
||
}
|
||
|
||
/***************** 主函数 ******************/
|
||
|
||
int main() {
|
||
/*** SysConfig 初始化 ***/
|
||
SYSCFG_DL_init();
|
||
|
||
/*** 模块初始化 ***/
|
||
Print_init();
|
||
Encoder::init();
|
||
// Imu::init();
|
||
Motor::init();
|
||
// TrackerMpu::init();
|
||
// Beep::init();
|
||
// Led::init();
|
||
Host::init();
|
||
Key::init();
|
||
// StepMotor::init();
|
||
|
||
/*** FreeRtos 任务初始化 ***/
|
||
xTaskCreate(vLedTask, "TestLedTask", 32, nullptr, 1, nullptr);
|
||
xTaskCreate(vOledTask, "vOledTask", 128, nullptr, 1, nullptr);
|
||
// xTaskCreate(vTestTask, "vTestTask", 512, nullptr, 1, nullptr);
|
||
xTaskCreate(vTrackerTask, "vTrackerTask", 256, nullptr, 1, nullptr);
|
||
xTaskCreate(vStepMotorTask, "vStepMotorTask", 32, nullptr, 1, nullptr);
|
||
|
||
|
||
/*** 启动 FreeRtos ***/
|
||
vTaskStartScheduler();
|
||
|
||
return 0;
|
||
}
|
||
|
||
/************ FreeRtos 错误处理部分 ************/
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
|
||
/* 栈溢出时的处理代码 */
|
||
/* 例如，打印错误信息并重置系统 */
|
||
|
||
/* 参数:
|
||
* xTask - 发生栈溢出的任务句柄
|
||
* pcTaskName - 发生栈溢出的任务名称
|
||
*/
|
||
|
||
/* 通常在这里添加一些错误处理和系统恢复的代码 */
|
||
while (1) {
|
||
/* 可以在此处添加错误处理代码，如闪烁LED或输出调试信息 */
|
||
}
|
||
}
|
||
|
||
/* 在全局范围内定义存储空间 */
|
||
static StaticTask_t xIdleTaskTCB;
|
||
static StackType_t uxIdleTaskStack[configMINIMAL_STACK_SIZE];
|
||
|
||
/* 实现所需的函数 */
|
||
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
|
||
StackType_t **ppxIdleTaskStackBuffer,
|
||
uint32_t *pulIdleTaskStackSize) {
|
||
/* 传递静态分配的空闲任务数据结构的地址 */
|
||
*ppxIdleTaskTCBBuffer = &xIdleTaskTCB;
|
||
|
||
/* 传递静态分配的空闲任务堆栈的地址 */
|
||
*ppxIdleTaskStackBuffer = uxIdleTaskStack;
|
||
|
||
/* 传递堆栈大小 */
|
||
*pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
|
||
}
|
||
|
||
|
||
/* 为定时器任务定义静态内存 */
|
||
static StaticTask_t xTimerTaskTCB;
|
||
static StackType_t uxTimerTaskStack[configTIMER_TASK_STACK_DEPTH];
|
||
|
||
void vApplicationGetTimerTaskMemory(StaticTask_t **ppxTimerTaskTCBBuffer,
|
||
StackType_t **ppxTimerTaskStackBuffer,
|
||
uint32_t *pulTimerTaskStackSize) {
|
||
/* 传递静态分配的定时器任务数据结构的地址 */
|
||
*ppxTimerTaskTCBBuffer = &xTimerTaskTCB;
|
||
|
||
/* 传递静态分配的定时器任务堆栈的地址 */
|
||
*ppxTimerTaskStackBuffer = uxTimerTaskStack;
|
||
|
||
/* 传递堆栈大小 */
|
||
*pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH;
|
||
}
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
|
||
// [[noreturn]] void vStepMotorTask(void *pvParameters) {
|
||
// while (true) {
|
||
// double speed = Host::posX;
|
||
// uint8_t direction = 1;
|
||
//
|
||
// if (Host::x_count <= 0) {
|
||
// speed = 0.0;
|
||
// } else {
|
||
// Host::x_count--;
|
||
// if (Host::x_count == 0) {
|
||
// Host::posX = 0.0; // 用完后置零
|
||
// }
|
||
// }
|
||
//
|
||
// direction = speed >= 0 ? 1 : 0;
|
||
// speed = speed >= 0 ? speed : -speed;
|
||
// StepMotor::setSpeed(direction, 32, float(speed));
|
||
// print("Motor: speed=%f, direction=%d\n", speed, direction);
|
||
//
|
||
// vTaskDelay(pdMS_TO_TICKS(70));
|
||
// }
|
||
// }
|
||
|
||
// [[noreturn]] void vStepMotorTask(void *pvParameters) {
|
||
// while (true) {
|
||
// int error = (int) Host::posX;
|
||
// uint8_t direction = error > 0;
|
||
// if (error > 5 || error < -5) {
|
||
// StepMotor::setPosition(direction, 32, 0.5, 10);
|
||
// }
|
||
//
|
||
// vTaskDelay(pdMS_TO_TICKS(93));
|
||
// }
|
||
// }
|
||
// print("FunA: %d\n", MenuPaint::GetSwitchState(0));
|
||
// print("FunB: %d\n", MenuPaint::GetSwitchState(1));
|
||
// if (MenuPaint::GetSwitchState(0) == 1) {
|
||
// Beep::start();
|
||
// } else {
|
||
// Beep::stop();
|
||
// }
|
||
|
||
// [[noreturn]] void vTrackerTask(void *pvParameters) {
|
||
// while (true) {
|
||
// print("Tracker!\n");
|
||
// const TrackerGpio::Data data = TrackerGpio::getData();
|
||
// print("Tracker: %d, %d, %d, %d, %d, %d, %d\n", data.data[0], data.data[1], data.data[2], data.data[3], data.data[4], data.data[5], data.data[6]);
|
||
// vTaskDelay(pdMS_TO_TICKS(1000));
|
||
// }
|
||
// }
|