添加云台数据
This commit is contained in:
RB
parent
b6e1393f4d
commit
180b6d728d
@ -407,6 +407,16 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>for_shoot</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>host</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -1738,7 +1748,7 @@
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>100</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\device\ai.c</PathWithFileName>
|
||||
|
||||
Binary file not shown.
18284
MDK-ARM/DevC/DevC.hex
18284
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
@ -14,12 +14,14 @@
|
||||
* @return uint16_t 行为对应的按键
|
||||
*/
|
||||
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
|
||||
CMD_Behavior_t behavior) {
|
||||
CMD_Behavior_t behavior)
|
||||
{
|
||||
return cmd->param->map.key_map[behavior].key;
|
||||
}
|
||||
|
||||
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
|
||||
CMD_Behavior_t behavior) {
|
||||
CMD_Behavior_t behavior)
|
||||
{
|
||||
return cmd->param->map.key_map[behavior].active;
|
||||
}
|
||||
|
||||
@ -32,43 +34,53 @@ static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
|
||||
* @return true 按下
|
||||
* @return false 未按下
|
||||
*/
|
||||
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
|
||||
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key)
|
||||
{
|
||||
/* 按下按键为鼠标左、右键 */
|
||||
if (key == CMD_L_CLICK) {
|
||||
if (key == CMD_L_CLICK)
|
||||
{
|
||||
return rc->mouse.l_click;
|
||||
}
|
||||
if (key == CMD_R_CLICK) {
|
||||
if (key == CMD_R_CLICK)
|
||||
{
|
||||
return rc->mouse.r_click;
|
||||
}
|
||||
return rc->key & (1u << key);
|
||||
}
|
||||
|
||||
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
|
||||
CMD_Behavior_t behavior) {
|
||||
CMD_Behavior_t behavior)
|
||||
{
|
||||
CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
|
||||
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
|
||||
|
||||
bool now_key_pressed, last_key_pressed;
|
||||
|
||||
/* 按下按键为鼠标左、右键 */
|
||||
if (key == CMD_L_CLICK) {
|
||||
if (key == CMD_L_CLICK)
|
||||
{
|
||||
now_key_pressed = rc->mouse.l_click;
|
||||
last_key_pressed = cmd->mouse_last.l_click;
|
||||
} else if (key == CMD_R_CLICK) {
|
||||
}
|
||||
else if (key == CMD_R_CLICK)
|
||||
{
|
||||
now_key_pressed = rc->mouse.r_click;
|
||||
last_key_pressed = cmd->mouse_last.r_click;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
now_key_pressed = rc->key & (1u << key);
|
||||
last_key_pressed = cmd->key_last & (1u << key);
|
||||
}
|
||||
|
||||
switch (active) {
|
||||
case CMD_ACTIVE_PRESSING:
|
||||
return now_key_pressed && !last_key_pressed;
|
||||
case CMD_ACTIVE_RASING:
|
||||
return !now_key_pressed && last_key_pressed;
|
||||
case CMD_ACTIVE_PRESSED:
|
||||
return now_key_pressed;
|
||||
switch (active)
|
||||
{
|
||||
case CMD_ACTIVE_PRESSING:
|
||||
return now_key_pressed && !last_key_pressed;
|
||||
case CMD_ACTIVE_RASING:
|
||||
return !now_key_pressed && last_key_pressed;
|
||||
case CMD_ACTIVE_PRESSED:
|
||||
return now_key_pressed;
|
||||
}
|
||||
}
|
||||
|
||||
@ -79,7 +91,8 @@ static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
|
||||
* @param cmd 主结构体
|
||||
* @param dt_sec 两次解析的间隔
|
||||
*/
|
||||
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
|
||||
/* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
|
||||
@ -91,85 +104,111 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
cmd->shoot.reverse_trig = false;
|
||||
|
||||
/* 按键行为映射相关逻辑 */
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
|
||||
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE))
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
|
||||
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE))
|
||||
{
|
||||
/* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire = true;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 切换至准备模式，停止射击 */
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire = false;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE))
|
||||
{
|
||||
/* 每按一次依次切换开火下一个模式 */
|
||||
cmd->shoot.fire_mode++;
|
||||
cmd->shoot.fire_mode %= FIRE_MODE_NUM;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR))
|
||||
{
|
||||
/* 切换到小陀螺模式 */
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER))
|
||||
{
|
||||
/* 每按一次开、关弹舱盖 */
|
||||
cmd->shoot.cover_open = !cmd->shoot.cover_open;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
|
||||
if (cmd->ai_status == AI_STATUS_HITSWITCH) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF))
|
||||
{
|
||||
if (cmd->ai_status == AI_STATUS_HITSWITCH)
|
||||
{
|
||||
/* 停止ai的打符模式，停用host控制 */
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
|
||||
cmd->host_overwrite = false;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
} else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
|
||||
}
|
||||
else if (cmd->ai_status == AI_STATUS_AUTOAIM)
|
||||
{
|
||||
/* 自瞄模式中切换失败提醒 */
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
/* ai切换至打符模式，启用host控制 */
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
|
||||
cmd->ai_status = AI_STATUS_HITSWITCH;
|
||||
cmd->host_overwrite = true;
|
||||
}
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
|
||||
if (cmd->ai_status == AI_STATUS_AUTOAIM) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM))
|
||||
{
|
||||
if (cmd->ai_status == AI_STATUS_AUTOAIM)
|
||||
{
|
||||
/* 停止ai的自瞄模式，停用host控制 */
|
||||
cmd->host_overwrite = false;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
/* ai切换至自瞄模式，启用host控制 */
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd->host_overwrite = false;
|
||||
// TODO: 修复逻辑
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG))
|
||||
{
|
||||
/* 按下拨弹反转 */
|
||||
cmd->shoot.reverse_trig = true;
|
||||
}
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
|
||||
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35))
|
||||
{
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
}
|
||||
/* 保存当前按下的键位状态 */
|
||||
@ -184,73 +223,69 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
* @param cmd 主结构体
|
||||
* @param dt_sec 两次解析的间隔
|
||||
*/
|
||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
switch (rc->sw_l) {
|
||||
/* 左拨杆相应行为选择和解析 */
|
||||
case CMD_SW_UP:
|
||||
cmd->chassis.mode = CHASSIS_MODE_BREAK;
|
||||
break;
|
||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
switch (rc->sw_l)
|
||||
{
|
||||
/* 左拨杆相应行为选择和解析 */
|
||||
case CMD_SW_UP:
|
||||
cmd->chassis.mode = CHASSIS_MODE_BREAK;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
cmd->host_overwrite = false;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
|
||||
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
|
||||
cmd->ai_status = AI_STATUS_AUTOAIM;
|
||||
cmd->host_overwrite = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_ERR:
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
break;
|
||||
case CMD_SW_ERR:
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
cmd->ai_status = AI_STATUS_STOP;
|
||||
cmd->host_overwrite = false;
|
||||
break;
|
||||
}
|
||||
switch (rc->sw_r) {
|
||||
/* 右拨杆相应行为选择和解析*/
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
switch (rc->sw_r)
|
||||
{
|
||||
/* 右拨杆相应行为选择和解析*/
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
/*
|
||||
case CMD_SW_UP:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
|
||||
case CMD_SW_MID:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.fire = false;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
break;
|
||||
|
||||
case CMD_SW_DOWN:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
|
||||
cmd->shoot.fire = true;
|
||||
break;
|
||||
*/
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
case CMD_SW_ERR:
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
cmd->shoot.mode = SHOOT_MODE_RELAX;
|
||||
}
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
|
||||
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
|
||||
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
|
||||
if (cmd->ai_status != AI_STATUS_STOP || cmd->host_overwrite == false)
|
||||
{
|
||||
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
|
||||
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
|
||||
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
|
||||
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -258,7 +293,8 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
*
|
||||
* @param cmd 主结构体
|
||||
*/
|
||||
static void CMD_RcLostLogic(CMD_t *cmd) {
|
||||
static void CMD_RcLostLogic(CMD_t *cmd)
|
||||
{
|
||||
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
|
||||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||||
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
|
||||
@ -272,10 +308,13 @@ static void CMD_RcLostLogic(CMD_t *cmd) {
|
||||
* @param param 参数
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
|
||||
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param)
|
||||
{
|
||||
/* 指针检测 */
|
||||
if (cmd == NULL) return -1;
|
||||
if (param == NULL) return -1;
|
||||
if (cmd == NULL)
|
||||
return -1;
|
||||
if (param == NULL)
|
||||
return -1;
|
||||
|
||||
/* 设置机器人的命令参数，初始化控制方式为rc控制 */
|
||||
cmd->pc_ctrl = false;
|
||||
@ -301,10 +340,13 @@ inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
|
||||
* @param dt_sec 两次解析的间隔
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
/* 指针检测 */
|
||||
if (rc == NULL) return -1;
|
||||
if (cmd == NULL) return -1;
|
||||
if (rc == NULL)
|
||||
return -1;
|
||||
if (cmd == NULL)
|
||||
return -1;
|
||||
|
||||
/* 在pc控制和rc控制间切换 */
|
||||
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
|
||||
@ -315,12 +357,18 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
|
||||
cmd->pc_ctrl = false;
|
||||
/*c当rc丢控时，恢复机器人至默认状态 */
|
||||
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
|
||||
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR))
|
||||
{
|
||||
CMD_RcLostLogic(cmd);
|
||||
} else {
|
||||
if (cmd->pc_ctrl) {
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cmd->pc_ctrl)
|
||||
{
|
||||
CMD_PcLogic(rc, cmd, dt_sec);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
CMD_RcLogic(rc, cmd, dt_sec);
|
||||
}
|
||||
}
|
||||
@ -335,21 +383,27 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
||||
* @param dt_sec 两次解析的间隔
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
|
||||
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec)
|
||||
{
|
||||
(void)dt_sec; /* 未使用dt_sec，消除警告 */
|
||||
/* 指针检测 */
|
||||
if (host == NULL) return -1;
|
||||
if (cmd == NULL) return -1;
|
||||
if (host == NULL)
|
||||
return -1;
|
||||
if (cmd == NULL)
|
||||
return -1;
|
||||
|
||||
/* 云台欧拉角设置为host相应的变化的欧拉角 */
|
||||
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
|
||||
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
|
||||
|
||||
/* host射击命令，设置不同的射击频率和弹丸初速度 */
|
||||
if (host->fire) {
|
||||
if (host->fire)
|
||||
{
|
||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||
cmd->shoot.fire = true;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd->shoot.mode = SHOOT_MODE_SAFE;
|
||||
}
|
||||
return 0;
|
||||
@ -362,11 +416,14 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
|
||||
* @param cmd 要添加的命令
|
||||
* @return int8_t 0对应没有错误
|
||||
*/
|
||||
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
|
||||
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd)
|
||||
{
|
||||
/* 指针检测 */
|
||||
if (ref == NULL) return -1;
|
||||
if (ref == NULL)
|
||||
return -1;
|
||||
/* 越界检测 */
|
||||
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
|
||||
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0)
|
||||
return -1;
|
||||
|
||||
/* 添加机器人当前行为状态到画图的命令队列中 */
|
||||
ref->cmd[ref->counter] = cmd;
|
||||
|
||||
@ -88,9 +88,10 @@ error:
|
||||
}
|
||||
|
||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
|
||||
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
|
||||
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
|
||||
// cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
|
||||
cmd_host->fire = true;
|
||||
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
|
||||
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
|
||||
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
|
||||
@ -105,10 +106,12 @@ int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
|
||||
ai->to_host.mcu.id = AI_ID_MCU;
|
||||
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
|
||||
sizeof(*quat));
|
||||
memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
|
||||
sizeof(*gimbal_ai));
|
||||
ai->to_host.mcu.package.data.notice = 0;
|
||||
if (ai->status == AI_STATUS_AUTOAIM)
|
||||
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
|
||||
|
||||
@ -57,7 +57,7 @@ int8_t AI_StartReceiving(AI_t *ai);
|
||||
bool AI_WaitDmaCplt(void);
|
||||
int8_t AI_ParseHost(AI_t *ai);
|
||||
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
|
||||
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai);
|
||||
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
|
||||
int8_t AI_StartSend(AI_t *ai, bool option);
|
||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
|
||||
|
||||
@ -72,7 +72,7 @@ typedef enum {
|
||||
|
||||
CAN_M3508_FRIC1_ID = 0x205, /* 5 */
|
||||
CAN_M3508_FRIC2_ID = 0x206, /* 6 */
|
||||
CAN_M2006_TRIG_ID = 0x208, /* 8 */
|
||||
CAN_M2006_TRIG_ID = 0x207, /* 8 */
|
||||
|
||||
CAN_GM6020_YAW_ID = 0x209, /* 5 */
|
||||
CAN_GM6020_PIT_ID = 0x20A, /* 6 */
|
||||
|
||||
@ -511,14 +511,14 @@ static const Config_RobotParam_t param_sentry_left = {
|
||||
.trig = -1.0f,
|
||||
},
|
||||
},
|
||||
.num_trig_tooth = 10.0f,
|
||||
.num_trig_tooth = 8.0f,
|
||||
.trig_gear_ratio = 36.0f,
|
||||
.fric_radius = 0.03f,
|
||||
.cover_open_duty = 0.10f,
|
||||
.cover_close_duty = 0.050f,
|
||||
.model = SHOOT_MODEL_17MM,
|
||||
.bullet_speed = 30.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
|
||||
.bullet_speed = 25.f,
|
||||
.min_shoot_delay = (uint32_t)(1000.0f / 3.0f),
|
||||
}, /* shoot */
|
||||
|
||||
.can = {
|
||||
|
||||
@ -201,6 +201,20 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 打包云台数据给AI
|
||||
*
|
||||
* \param g 包含云台数据的结构体
|
||||
* \param gimbal_for_ai 云台AI数据
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai){
|
||||
memcpy((void *)ai, (const void *)&(g->feedback.eulr.encoder),
|
||||
sizeof(g->feedback.eulr.encoder));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief 复制云台输出值
|
||||
*
|
||||
|
||||
@ -145,6 +145,16 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
|
||||
*/
|
||||
int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
|
||||
|
||||
/**
|
||||
* \brief 打包云台数据给AI
|
||||
*
|
||||
* \param g 包含云台数据的结构体
|
||||
* \param gimbal_for_ai 云台AI数据
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai);
|
||||
|
||||
/**
|
||||
* \brief 复制云台输出值
|
||||
*
|
||||
|
||||
@ -248,6 +248,7 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
s->setpoint.fric_rpm[1] =
|
||||
CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
|
||||
(s->param->model == SHOOT_MODEL_17MM));
|
||||
s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[1];
|
||||
s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
|
||||
|
||||
/* 计算拨弹电机位置的目标值 */
|
||||
@ -255,10 +256,10 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
|
||||
(s_cmd->fire)) {
|
||||
/* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
|
||||
if (s_cmd->reverse_trig) { /* 反转拨弹 */
|
||||
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
|
||||
CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
|
||||
M_2PI);
|
||||
} else {
|
||||
CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
|
||||
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
|
||||
M_2PI);
|
||||
s->fire_ctrl.shooted++;
|
||||
s->fire_ctrl.last_shoot = now;
|
||||
|
||||
@ -17,11 +17,13 @@ AI_t ai;
|
||||
CMD_Host_t cmd_host;
|
||||
AHRS_Quaternion_t quat;
|
||||
Referee_ForAI_t referee_ai;
|
||||
AHRS_Eulr_t gimbal_ai;
|
||||
#else
|
||||
static AI_t ai;
|
||||
static CMD_Host_t cmd_host;
|
||||
static AHRS_Quaternion_t quat;
|
||||
static Referee_ForAI_t referee_ai;
|
||||
static AHRS_Eulr_t gimbal_ai;
|
||||
#endif
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -66,10 +68,11 @@ void Task_Ai(void *argument) {
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), NULL, 0);
|
||||
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
|
||||
&(referee_ai), NULL, 0) == osOK);
|
||||
|
||||
AI_PackMCU(&ai, &quat);
|
||||
AI_PackMCU(&ai, &quat, &gimbal_ai);
|
||||
if (ref_update) AI_PackRef(&ai, &(referee_ai));
|
||||
|
||||
AI_StartSend(&(ai), ref_update);
|
||||
|
||||
@ -68,7 +68,6 @@ void Task_Command(void *argument) {
|
||||
if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
|
||||
osOK)
|
||||
CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
|
||||
|
||||
osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
|
||||
|
||||
/* 将需要与其他任务分享的数据放到消息队列中 */
|
||||
|
||||
@ -22,11 +22,13 @@ CMD_GimbalCmd_t gimbal_cmd;
|
||||
Gimbal_t gimbal;
|
||||
CAN_GimbalOutput_t gimbal_out;
|
||||
Referee_GimbalUI_t gimbal_ui;
|
||||
AHRS_Eulr_t gimbal_for_ai;
|
||||
#else
|
||||
static CMD_GimbalCmd_t gimbal_cmd;
|
||||
static Gimbal_t gimbal;
|
||||
static CAN_GimbalOutput_t gimbal_out;
|
||||
static Referee_GimbalUI_t gimbal_ui;
|
||||
static AHRS_Eulr_t gimbal_for_ai;
|
||||
#endif
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {
|
||||
|
||||
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
|
||||
Gimbal_UpdateFeedback(&gimbal, &can);
|
||||
Gimbal_PackAI(&gimbal, &gimbal_for_ai);
|
||||
Gimbal_Control(&gimbal, &gimbal_cmd, tick);
|
||||
Gimbal_DumpOutput(&gimbal, &gimbal_out);
|
||||
|
||||
osKernelUnlock();
|
||||
osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
|
||||
osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_encoder);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_encoder, &gimbal_for_ai, 0, 0);
|
||||
|
||||
Gimbal_DumpUI(&gimbal, &gimbal_ui);
|
||||
osMessageQueueReset(task_runtime.msgq.ui.gimbal);
|
||||
|
||||
176
User/task/init.c
176
User/task/init.c
@ -30,101 +30,103 @@
|
||||
*
|
||||
* \param argument 未使用
|
||||
*/
|
||||
void Task_Init(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
void Task_Init(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
|
||||
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
|
||||
|
||||
osKernelLock();
|
||||
/* 创建任务 */
|
||||
task_runtime.thread.atti_esti =
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
|
||||
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
|
||||
task_runtime.thread.ctrl_chassis =
|
||||
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal =
|
||||
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.ctrl_shoot =
|
||||
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
|
||||
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
|
||||
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
|
||||
osKernelLock();
|
||||
/* 创建任务 */
|
||||
task_runtime.thread.atti_esti =
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
|
||||
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
|
||||
task_runtime.thread.ctrl_chassis =
|
||||
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal =
|
||||
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.ctrl_shoot =
|
||||
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
|
||||
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
|
||||
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
|
||||
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
|
||||
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
|
||||
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
|
||||
|
||||
/* 创建消息队列 */
|
||||
/* motor */
|
||||
task_runtime.msgq.can.feedback.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.output.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
|
||||
/* 创建消息队列 */
|
||||
/* motor */
|
||||
task_runtime.msgq.can.feedback.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.feedback.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_t), NULL);
|
||||
task_runtime.msgq.can.output.chassis =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.shoot =
|
||||
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
|
||||
task_runtime.msgq.can.output.cap =
|
||||
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
|
||||
|
||||
/* command */
|
||||
task_runtime.msgq.cmd.chassis =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.gimbal =
|
||||
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.shoot =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.ai =
|
||||
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
|
||||
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
|
||||
/* command */
|
||||
task_runtime.msgq.cmd.chassis =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.gimbal =
|
||||
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.shoot =
|
||||
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
|
||||
task_runtime.msgq.cmd.ai =
|
||||
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
|
||||
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
|
||||
|
||||
/* atti_esti */
|
||||
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.host =
|
||||
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
|
||||
/* atti_esti */
|
||||
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.host =
|
||||
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
|
||||
|
||||
task_runtime.msgq.gimbal.accl =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_imu =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.gimbal.gyro =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.gimbal.accl =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_imu =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.gimbal.gyro =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
|
||||
task_runtime.msgq.gimbal.eulr_encoder =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
|
||||
task_runtime.msgq.cap_info =
|
||||
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
|
||||
|
||||
task_runtime.msgq.cap_info =
|
||||
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
|
||||
/* AI */
|
||||
task_runtime.msgq.ai.quat =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
|
||||
/* AI */
|
||||
task_runtime.msgq.ai.quat =
|
||||
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.ai =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
|
||||
task_runtime.msgq.referee.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
task_runtime.msgq.referee.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
|
||||
|
||||
/* 裁判系统 */
|
||||
task_runtime.msgq.referee.ai =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
|
||||
task_runtime.msgq.referee.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
|
||||
task_runtime.msgq.referee.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
|
||||
task_runtime.msgq.referee.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
|
||||
/* UI */
|
||||
task_runtime.msgq.ui.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.ui.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.ui.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.ui.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
|
||||
/* UI */
|
||||
task_runtime.msgq.ui.chassis =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
|
||||
task_runtime.msgq.ui.cap =
|
||||
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
|
||||
task_runtime.msgq.ui.gimbal =
|
||||
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
|
||||
task_runtime.msgq.ui.shoot =
|
||||
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
|
||||
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
|
||||
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
}
|
||||
|
||||
@ -58,6 +58,7 @@ typedef struct {
|
||||
osMessageQueueId_t accl; /* IMU读取 */
|
||||
osMessageQueueId_t gyro; /* IMU读取 */
|
||||
osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
|
||||
osMessageQueueId_t eulr_encoder; /* 编码器读取 */
|
||||
} gimbal;
|
||||
|
||||
/* 控制指令 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user