156 lines
5.9 KiB
C
156 lines
5.9 KiB
C
/* =============================================================================
|
|
* chassis_task.c - 底盘运动控制(2ms)
|
|
* 状态机:
|
|
* F_WAIT 等待"底盘姿态就绪+四轮在线+云台自检结果下发(0x1A9)"
|
|
* F_RUN 正常模式控制(跟随/小陀螺/停车;符号用参数回退值直至自检完成)
|
|
* F_DETECT 跟随符号自检——**仅由操作手手势显式触发,绝不自动执行**:
|
|
* 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
|
|
* 检测给四轮约 5% 电流脉冲 80ms,落地时整车轻转一下,操作手
|
|
* 应确认车周围无人后再做手势。失败重试 3 次后回退参数符号。
|
|
* 功率限制:缓冲能量不足时按比例缩放输出(裁判系统 0x0202)
|
|
* SHIFT:临时速度增益(超电驱动 TODO,接入后替换)
|
|
* ========================================================================== */
|
|
#include "app_tasks.h"
|
|
#include "cmsis_os.h"
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "robot_typedef.h"
|
|
#include "chassis.h"
|
|
#include "attitude.h"
|
|
#include "bmi088.h"
|
|
#include "referee.h"
|
|
#include "c_comm.h"
|
|
#include "robot_param.h"
|
|
|
|
/* 校准手势:左摇杆推最左 + 右摇杆推最右(与云台侧手势一致,两板同时自检) */
|
|
#define CALIB_GESTURE_L_RL 400 // < 400 即最左
|
|
#define CALIB_GESTURE_R_RL 1600 // > 1600 即最右
|
|
#define CALIB_GESTURE_MS 500 // 保持时长
|
|
#define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务
|
|
|
|
typedef enum
|
|
{
|
|
F_WAIT = 0,
|
|
F_RUN,
|
|
F_DETECT,
|
|
} follow_state_e;
|
|
|
|
void chassis_task(void const * argument)
|
|
{
|
|
TickType_t last_wake = xTaskGetTickCount();
|
|
follow_state_e state = F_WAIT;
|
|
uint8_t detect_retry = 0;
|
|
uint16_t gesture_cnt = 0;
|
|
|
|
while (!app_ready)
|
|
osDelay(2);
|
|
|
|
for (;;)
|
|
{
|
|
/* 校准手势检测(遥控在线=ctoc_alive 时才可能成立;摇杆值来自 0x149 转发) */
|
|
if (cmd_robot.online &&
|
|
ctoc_remote.rc_l_rl < CALIB_GESTURE_L_RL && ctoc_remote.rc_r_rl > CALIB_GESTURE_R_RL)
|
|
{
|
|
if (gesture_cnt < CALIB_GESTURE_TICKS)
|
|
gesture_cnt++;
|
|
}
|
|
else
|
|
gesture_cnt = 0;
|
|
const uint8_t gesture = (gesture_cnt >= CALIB_GESTURE_TICKS);
|
|
if (gesture)
|
|
gesture_cnt = 0;
|
|
|
|
switch (state)
|
|
{
|
|
case F_WAIT:
|
|
chassis_output_zero();
|
|
/* 自身就绪即可进入运行(符号先用参数回退值) */
|
|
if (!chassis_motor_offline() && att_ready)
|
|
state = F_RUN;
|
|
break;
|
|
|
|
case F_DETECT:
|
|
if (chassis_detect_follow_sign())
|
|
{
|
|
chassis_follow_reset(); // 检测期间车被动转过,重锁基准
|
|
state = F_RUN;
|
|
}
|
|
else if (++detect_retry >= 3)
|
|
{
|
|
detect_retry = 0;
|
|
chassis_follow_reset();
|
|
state = F_RUN; // 回退参数符号
|
|
}
|
|
else
|
|
chassis_output_zero();
|
|
break;
|
|
|
|
case F_RUN:
|
|
default:
|
|
if (gesture && !chassis_motor_offline() && att_ready && ctoc_encoder_ccw != 0)
|
|
{
|
|
detect_retry = 0;
|
|
chassis_output_zero();
|
|
chassis_follow_reset();
|
|
state = F_DETECT; // 操作手主动请求自检
|
|
}
|
|
else if (cmd_robot.online && !chassis_motor_offline())
|
|
{
|
|
float w = 0;
|
|
|
|
switch (cmd_robot.chassis_mode)
|
|
{
|
|
case CHASSIS_MODE_SPIN_SLOW:
|
|
w = MECANUM_SPIN_SLOW_W;
|
|
break;
|
|
|
|
case CHASSIS_MODE_SPIN_FAST:
|
|
w = MECANUM_SPIN_FAST_W;
|
|
break;
|
|
|
|
case CHASSIS_MODE_FOLLOW:
|
|
default:
|
|
w = chassis_follow_w(); // 编码器相对角,零漂移
|
|
break;
|
|
}
|
|
|
|
/* SHIFT 加速(超电接入前的临时方案,受功率限制约束) */
|
|
float boost = 1.0f;
|
|
if (ctoc_remote.shift_push)
|
|
boost = MECANUM_SHIFT_BOOST_RATE;
|
|
float vx = cmd_robot.vx * boost;
|
|
float vy = cmd_robot.vy * boost;
|
|
|
|
/* 坐标系旋转投影:将以云台枪口为准的平移速度转换到底盘坐标系，并叠加 kw 前馈 */
|
|
float chassis_vx = vx;
|
|
float chassis_vy = vy;
|
|
/* 跟随模式下无持续自旋，前馈量为0；小陀螺模式下代入实际自旋角速度 w */
|
|
float w_feedforward = (cmd_robot.chassis_mode == CHASSIS_MODE_FOLLOW) ? 0.0f : bmi088_gyro[2];
|
|
chassis_vector_to_body(vx, vy, w_feedforward, &chassis_vx, &chassis_vy);
|
|
|
|
|
|
/* 简易功率限制:缓冲能量线性映射为缩放系数 */
|
|
float scale = 1.0f;
|
|
if (referee.buffer_energy < POWER_LIMIT_BUFFER_LOW)
|
|
scale = (referee.buffer_energy - POWER_LIMIT_BUFFER_MIN) /
|
|
(POWER_LIMIT_BUFFER_LOW - POWER_LIMIT_BUFFER_MIN);
|
|
if (scale < 0.0f)
|
|
scale = 0.0f;
|
|
else if (scale > 1.0f)
|
|
scale = 1.0f;
|
|
|
|
chassis_control(chassis_vx, chassis_vy, w, scale);
|
|
}
|
|
else
|
|
{
|
|
chassis_follow_reset();
|
|
chassis_output_zero();
|
|
}
|
|
break;
|
|
}
|
|
|
|
monitor_heartbeat(HB_CHASSIS);
|
|
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
|
|
}
|
|
}
|