LYL/25RC_ShootBallGame
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加重定位，已测试

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-06 03:09:43 +08:00
parent 053e2cdea3
commit 117bdf6d12
12 changed files with 180 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
MDK-ARM/R2.uvoptx
View File

@ -180,6 +180,31 @@
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>SendBuffer</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>NUC_send</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>a,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

81
User/Module/Chassis.c
View File

@ -3,6 +3,8 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#include "nuc.h"
/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
@ -101,8 +103,8 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
@ -113,6 +115,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
switch (c->chassis_ctrl.ctrl) {
case ShootGame_Mode: //投球赛模式
c->to_nuc.send=0;
switch(c->chassis_ctrl.mode){
case ShootRst:
c->move_vec.Vw = ctrl->Vw * 8000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
break;
default:
if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) {
// 有遥控器输出，设置标志位1
osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG);
@ -128,66 +141,22 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
}else {
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
}
break;
case AUTO:
switch (c->chassis_ctrl.mode) {
case AUTO_MID360:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
// c->NUC_send.send[0] = 1;
break;
case AUTO_MID360_Pitch:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break;
default:
break;
}
case PASS_BALL:
switch (c->chassis_ctrl.mode)
{
case PB_UP:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break ;
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break ;
}
break;
}
break;
case REPOSITION:
c->to_nuc.send=1;
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;

11
User/Module/Chassis.h
View File

@ -32,6 +32,7 @@
#include "cmd.h"
#include "filter.h"
#include "kalman.h"
#include "nuc.h"
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
@ -103,12 +104,6 @@ typedef struct
fp32 Vw;
fp32 mul;//油门倍率
}ChassisMove_Vec;
typedef struct
{
fp32 send[4];
}NUC_send_t;
/**
* @brief
@ -194,7 +189,9 @@ typedef struct{
int is_reach;
}sick_cali;
NUC_send_t NUC_send;
struct {
fp32 send;
}to_nuc;
}Chassis_t;

38
User/Module/up.c
View File

@ -192,6 +192,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
switch (c->CMD_CtrlType )
{
case ShootGame_Mode:
UP_AUTO_Control(u,out,c);
@ -364,36 +365,45 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25f,3.2,4.3,&u->MID360Context.Curve);
/* 函数，角度更新*/
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
target->Pitch_angle = 66;
}
/* 上下速度更改*/ }
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
/*检测重置*/
if(MID360Context->IsLaunch==0){
switch(c-> CMD_mode)
{
case AUTO_MID360_Pitch:
if(MID360Context->IsLaunch==0){
MID360Context->IsLaunch=1;
LaunchContext->LaunchState = Launch_PREPARE;
}
/*运行*/
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
break ;
switch(c-> CMD_mode)
{
case Shooting://发射
case AUTO_MID360:
target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
break;
case ShootRst://重置发射
MID360Context->IsLaunch=0;
break ;
default:
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;
break ;
default:
break;
}
return 1;
}

4
User/bsp/protocol.h
View File

@ -14,6 +14,8 @@ extern "C" {
#define HEAD (0xFF)
#define TAIL (0xFE)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
@ -83,7 +85,7 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed))
{
uint8_t status;
float status;
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/

103
User/device/cmd.c
View File

@ -136,41 +136,74 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
cmd->key_ctrl_l = rc->dr16.sw_l;
cmd->key_ctrl_r = rc->dr16.sw_r ;
if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
cmd->CMD_CtrlType =RELAXED;
}
else if(rc->dr16.sw_l==CMD_SW_UP)
{
cmd ->CMD_CtrlType =ShootGame_Mode;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左上，右上，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，无模式
if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst;
if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting;
}
else if(rc->dr16.sw_l==CMD_SW_MID)
{
cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)
{
cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
}
// if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
// cmd->CMD_CtrlType =RELAXED;
// }
// else if(rc->dr16.sw_l==CMD_SW_UP)
// {
// cmd ->CMD_CtrlType =ShootGame_Mode;
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Shooting; //左上，右上，无模式
//
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
//
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =ShootRst; //左上，右上，无模式
//
//// if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst;
//// if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting;
// }
//
// else if(rc->dr16.sw_l==CMD_SW_MID)
// {
//
// cmd ->CMD_CtrlType =RCcontrol;
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达
//
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
//
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
//
// }
// else if(rc->dr16.sw_l==CMD_SW_DOWN)
// {
// cmd ->CMD_CtrlType =RCcontrol;
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
// }
if ((rc->dr16.sw_r == CMD_SW_ERR) || (rc->dr16.sw_l == CMD_SW_ERR)) {
cmd->CMD_CtrlType = RELAXED;
}
// 右拨杆控制 CtrlType，左拨杆控制 Mode
else if (rc->dr16.sw_r == CMD_SW_UP)
{
cmd->CMD_CtrlType = ShootGame_Mode;
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右上，左上，
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = ShootRst; // 右上，左中，
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右上，左下，无模式
// 可选保留遥控旋钮控制
// if (rc->dr16.res > 3000) cmd->CMD_mode = ShootRst;
// if (rc->dr16.res == 660) cmd->CMD_mode = Shooting;
}
else if (rc->dr16.sw_r == CMD_SW_MID)
{
cmd->CMD_CtrlType = REPOSITION;
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右中，左上，
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右中，左中，无模式
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右中，左下，
}
else if (rc->dr16.sw_r == CMD_SW_DOWN)
{
cmd->CMD_CtrlType = RCcontrol;
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右下，左上，无模式
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右下，左中，无模式
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右下，左下，无模式
}
return 0;
}

3
User/device/cmd.h
View File

@ -13,7 +13,10 @@
typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层
REPOSITION, //重定位
AUTO,
PASS_BALL,//传球模式
ShootGame_Mode,//投球赛模式

28
User/device/nuc.c
View File

@ -170,28 +170,34 @@ int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){
nuc->to_nuc.cmd.head = HEAD;
nuc->to_nuc.cmd.id = IMU_ID;
nuc->to_nuc.cmd.type = TYPE;
nuc->to_nuc.cmd.end = TAIL;
// memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
/*在这里添加你们的控制命令*/
return DEVICE_OK;
}
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){
if (cmd_update) {
// if (HAL_UART_Transmit_DMA(
// BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
// sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
/*仅发送cmd数据*/
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
sizeof(nuc->to_nuc.cmd) ) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)&(nuc->to_nuc.imu),
sizeof(nuc->to_nuc.imu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}
// else {
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
// (uint8_t *)&(nuc->to_nuc.imu),
// sizeof(nuc->to_nuc.imu)) == HAL_OK)
// return DEVICE_OK;
// else
// return DEVICE_ERR;
// }
}

6
User/device/nuc.h
View File

@ -9,6 +9,11 @@
#include "Algorithm/ahrs.h"
#include "Module/Chassis.h"
typedef struct
{
fp32 send;
}NUC_send_t;
typedef struct {
uint8_t id;
@ -25,7 +30,6 @@ typedef struct {
typedef struct {
uint8_t head;
uint8_t type; // 0x01 控制帧
uint8_t id;
Protocol_UpDataCMD_t package;
uint8_t end;

2
User/task/chassis_task.c
View File

@ -88,7 +88,7 @@ void Task_Chassis(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.to_nuc, 0, 0); //发送数据
vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2];

10
User/task/nuc_task.c
View File

@ -57,19 +57,19 @@ void Task_nuc(void *argument){
osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
// osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
//osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
&(NUC_send), NULL, 0) == osOK);
NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
// NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
if (cmd_update) {
NUC_PackCMD(&nuc_raw,&NUC_send);
NUC_StartSend(&nuc_raw,cmd_update);
}
// NUC_StartSend(&nuc_raw, cmd_update);
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
osDelayUntil(tick);
}