SkyLiCoder
4d1702b577
- Rewrite DM motor driver based on DM_6220 reference protocol (MIT/PosVel/Velocity modes) - Use velocity control mode: MCU runs angle PID with IMU feedback, motor handles speed loop - Rewrite vision protocol with 3 frame types (0xA5 auto data, 0xA6 manual data, 0xA7 command) - Decouple serial parsing (opencv module) from mode switching (robot_cmd module) - Implement dual-loop architecture: vision PID outer loop → angle ref → motor PID inner loop - Add KF/PID reset on mode switch transitions - Merge motor_reverse_flag and feedback_reverse_flag into single output_reverse flag - Remove DMMotorEnable/DMMotorStop (motor always runs, controlled by angle ref) - Remove unused modules: DJI/LK/HT/step/servo motors, chassis, shoot, referee - Add ExitRun0Mode() for STM32H723 power supply configuration - Clean up CMakeLists: 115 → 76 C source files
103 lines
2.8 KiB
C
103 lines
2.8 KiB
C
#pragma once
|
|
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "main.h"
|
|
#include "cmsis_os.h"
|
|
|
|
#include "robot.h"
|
|
#include "ins_task.h"
|
|
#include "motor_task.h"
|
|
#include "daemon.h"
|
|
#include "buzzer.h"
|
|
|
|
#include "bsp_log.h"
|
|
|
|
osThreadId insTaskHandle;
|
|
osThreadId robotTaskHandle;
|
|
osThreadId motorTaskHandle;
|
|
osThreadId daemonTaskHandle;
|
|
|
|
void StartINSTASK(void const *argument);
|
|
void StartMOTORTASK(void const *argument);
|
|
void StartDAEMONTASK(void const *argument);
|
|
void StartROBOTTASK(void const *argument);
|
|
|
|
void OSTaskInit()
|
|
{
|
|
osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024);
|
|
insTaskHandle = osThreadCreate(osThread(instask), NULL);
|
|
|
|
osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
|
|
motorTaskHandle = osThreadCreate(osThread(motortask), NULL);
|
|
|
|
osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 128);
|
|
daemonTaskHandle = osThreadCreate(osThread(daemontask), NULL);
|
|
|
|
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
|
|
robotTaskHandle = osThreadCreate(osThread(robottask), NULL);
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartINSTASK(void const *argument)
|
|
{
|
|
static float ins_start, ins_dt;
|
|
INS_Init();
|
|
LOGINFO("[freeRTOS] INS Task Start");
|
|
for (;;)
|
|
{
|
|
ins_start = DWT_GetTimeline_ms();
|
|
INS_Task();
|
|
ins_dt = DWT_GetTimeline_ms() - ins_start;
|
|
if (ins_dt > 1)
|
|
LOGERROR("[freeRTOS] INS Task DELAY dt=%.3f", ins_dt);
|
|
osDelay(1);
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartMOTORTASK(void const *argument)
|
|
{
|
|
static float motor_start, motor_dt;
|
|
LOGINFO("[freeRTOS] MOTOR Task Start");
|
|
for (;;)
|
|
{
|
|
motor_start = DWT_GetTimeline_ms();
|
|
MotorControlTask();
|
|
motor_dt = DWT_GetTimeline_ms() - motor_start;
|
|
if (motor_dt > 1)
|
|
LOGERROR("[freeRTOS] MOTOR Task DELAY dt=%.3f", motor_dt);
|
|
osDelay(1);
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartDAEMONTASK(void const *argument)
|
|
{
|
|
static float daemon_start, daemon_dt;
|
|
BuzzerInit();
|
|
LOGINFO("[freeRTOS] Daemon Task Start");
|
|
for (;;)
|
|
{
|
|
daemon_start = DWT_GetTimeline_ms();
|
|
DaemonTask();
|
|
BuzzerTask();
|
|
daemon_dt = DWT_GetTimeline_ms() - daemon_start;
|
|
if (daemon_dt > 10)
|
|
LOGERROR("[freeRTOS] Daemon Task DELAY dt=%.3f", daemon_dt);
|
|
osDelay(10);
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartROBOTTASK(void const *argument)
|
|
{
|
|
static float robot_start, robot_dt;
|
|
LOGINFO("[freeRTOS] ROBOT core Task Start");
|
|
for (;;)
|
|
{
|
|
robot_start = DWT_GetTimeline_ms();
|
|
RobotTask();
|
|
robot_dt = DWT_GetTimeline_ms() - robot_start;
|
|
if (robot_dt > 5)
|
|
LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt);
|
|
osDelay(2);
|
|
}
|
|
}
|