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

sick改can2等

Browse Source
This commit is contained in:
ZHAISHUI04 2025-06-21 15:53:18 +08:00
parent e9710e65a5
commit 6913f1f342
15 changed files with 196 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -463,3 +463,7 @@
[info] Log at : 2025/6/14|15:14:16|GMT+0800
[info] Log at : 2025/6/16|23:35:32|GMT+0800
[info] Log at : 2025/6/18|17:47:04|GMT+0800

90
MDK-ARM/R2.uvoptx
View File

@ -175,6 +175,86 @@
<WinNumber>1</WinNumber>
<ItemText>NUC_send,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>pid,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>pid_param,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_rc,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cbuf,0x10</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>valid_check,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>aeeaa,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>aaaaaaaaaaaa,0x0A</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>valid_check,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>aaaaaaaaaaaaaaaaaaaaaaaaaa,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>aaaaaaaaaaaaaaaaaaa,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -182,7 +262,7 @@
<DebugFlag>
<trace>0</trace>
<periodic>1</periodic>
<aLwin>1</aLwin>
<aLwin>0</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
@ -1181,8 +1261,8 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Algorithm\kalman.c</PathWithFileName>
<FilenameWithoutPath>kalman.c</FilenameWithoutPath>
<PathWithFileName>..\User\Algorithm\user_cmsis_os2.c</PathWithFileName>
<FilenameWithoutPath>user_cmsis_os2.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
@ -1193,8 +1273,8 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Algorithm\user_cmsis_os2.c</PathWithFileName>
<FilenameWithoutPath>user_cmsis_os2.c</FilenameWithoutPath>
<PathWithFileName>..\User\Algorithm\kalman.c</PathWithFileName>
<FilenameWithoutPath>kalman.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>

10
MDK-ARM/R2.uvprojx
View File

@ -1248,16 +1248,16 @@
<FileType>1</FileType>
<FilePath>..\User\Algorithm\ahrs.c</FilePath>
</File>
<File>
<FileName>kalman.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Algorithm\kalman.c</FilePath>
</File>
<File>
<FileName>user_cmsis_os2.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Algorithm\user_cmsis_os2.c</FilePath>
</File>
<File>
<FileName>kalman.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Algorithm\kalman.c</FilePath>
</File>
<File>
<FileName>user_math.c</FileName>
<FileType>1</FileType>

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

Binary file not shown.

56
User/Module/Chassis.c
View File

@ -62,22 +62,24 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
fp32 gyro_kp = 1.0f;
fp32 PIAN = 0;
fp32 pian_yaw;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw; // 左前
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前
Chassis_AngleCompensate(c);
}
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
Chassis_SetCtrl(c, ctrl);
// IMU加滤波修正
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
@ -92,12 +94,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
case RCcontrol:
switch (c->chassis_ctrl.mode) {
case Normal:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
c->move_vec.Vw = ctrl->Vw * 8000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
break;
case Pitch:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break;
case UP_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
@ -159,6 +163,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
}
c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
return CHASSIS_OK;
@ -214,5 +219,36 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
}
pid_type_def pid;
pid_param_t pid_param={
.p = 10.0f,
.i = 0.00f,
.d = 0.00f,
.i_limit = 2000.0f,
.out_limit =5000.0f,
};
fp32 cur_angle,pian_angel;
int8_t Chassis_AngleCompensate(Chassis_t *c)
{
if (c == NULL) return CHASSIS_ERR_NULL;
// int is_in;//是否初始化
// if(is_in==0){
PID_init(&pid,PID_POSITION,&pid_param);
// is_in=1;
// }
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
{
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
}
else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0;
}
pian_yaw = PID_calc(&pid,pian_angel,0);
}

1
User/Module/Chassis.h
View File

@ -189,6 +189,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;
int8_t Chassis_AngleCompensate(Chassis_t *c) ;
#endif

2
User/Module/config.c
View File

@ -110,7 +110,7 @@ static const ConfigParam_t param ={
},
/*投球*/
.PitchCfg = {
.m2006_init =-100, //释放的角度
.m2006_init =-150, //释放的角度
.m2006_trig =0, //可以拉住发射的角度
.go_init = -50, //仅用在go上电，初始位置
.go_pull_pos =-210,

8
User/device/can_use.c
View File

@ -374,9 +374,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index);
break;
case CAN_SICK_ID:
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break;
default:
break;
@ -407,7 +405,9 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break;
case CAN_SICK_ID:
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break;
case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10;

10
User/device/cmd.c
View File

@ -146,18 +146,20 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
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_DOWN) cmd ->CMD_mode =Chassis_Adjust; //左上，右上，手动调整
if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
}
else if(rc->dr16.sw_l==CMD_SW_MID)
{
cmd ->CMD_CtrlType =AUTO;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右中，雷达
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =AUTO_MID360; //左中，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右下，视觉
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)

3
User/device/device.h
View File

@ -28,8 +28,7 @@ extern "C" {
#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
#define SIGNAL_ACTION_RAW_REDY (1u << 10)
#define SIGNAL_OPSTIMER_REDY (1u << 11)
#define SIGNAL_R12DS_BUF1_REDY (1u << 12)
#define SIGNAL_DR16_RAW_REDY (1u <<13)
#define SIGNAL_RC_RAW_REDY (1u <<13)
#define SIGNAL_AI_RAW_REDY (1u << 15)
#define SIGNAL_KEY_REDY (1u << 16)

2
User/device/nuc.c
View File

@ -6,7 +6,7 @@ static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
static uint8_t nucbuf[31];
uint8_t nucbuf[31];
static char SendBuffer[19];

26
User/device/rc.c
View File

@ -12,8 +12,8 @@
#include "error_detect.h"
//#define DR16
#define LD_t
#define DR16
//#define LD_t
#ifdef DR16
#define FRAME_LEN 36
@ -38,14 +38,14 @@ int8_t RC_SBUS_Init(void )
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
RC_SBUS_RxCpltCallback);
RC_RxCpltCallback);
memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包
return DEVICE_OK;
}
static void RC_SBUS_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
static void RC_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_RC_RAW_REDY);
// detect_hook(DR16_TOE);
}
int8_t RC_SBUS_Restart(void) {
@ -53,7 +53,7 @@ int8_t RC_SBUS_Restart(void) {
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
return DEVICE_OK;
}
int8_t RC_SBUS_StartDmaRecv(void) {
int8_t RC_StartDmaRecv(void) {
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
(uint8_t *)cbuf,
@ -61,9 +61,9 @@ int8_t RC_SBUS_StartDmaRecv(void) {
return DEVICE_OK;
return DEVICE_ERR;
}
bool RC_SBUS_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_DR16_RAW_REDY);
bool RC_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_RC_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_RC_RAW_REDY);
}
/*乐迪数据解析 */
int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
@ -197,7 +197,7 @@ int8_t DR16_ParseRaw(DR16_t *dr16)
dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
dr16->data.sw_r = ((cbuf[5] >>4));
dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;
dr16->data.res= (fp32)((cbuf[16]|cbuf[17] << 8));
return 1;
}
int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
@ -220,11 +220,11 @@ int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
rc->dr16.ch_l_x = 2 * ((float)dr16->data.ch_l_x - RC_CH_VALUE_MID) / full_range;
rc->dr16.ch_l_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range;
rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
rc->dr16.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
rc->dr16.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
rc->dr16.key = dr16->data.key;
rc->dr16.res = ((float)dr16->data.res - RC_CH_VALUE_MID) ;
// rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;

12
User/device/rc.h
View File

@ -11,6 +11,10 @@
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef __packed struct
{
uint16_t ch_r_x : 11;
@ -69,10 +73,10 @@ typedef __packed struct
int8_t RC_SBUS_Init(void );
static void RC_SBUS_RxCpltCallback(void) ;
int8_t RC_SBUS_Restart(void) ;
int8_t RC_SBUS_StartDmaRecv(void) ;
bool RC_SBUS_WaitDmaCplt(uint32_t timeout) ;
static void RC_RxCpltCallback(void) ;
int8_t RC_Restart(void) ;
int8_t RC_StartDmaRecv(void) ;
bool RC_WaitDmaCplt(uint32_t timeout) ;
int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD);
static bool DR16_DataCorrupted(const DR16_t *dr16) ;
int8_t DR16_ParseRaw(DR16_t *dr16);

29
User/task/rc_task.c
View File

@ -20,7 +20,9 @@ LD_Data_t LD;
#else
static DR16_t dr16;
static CMD_RC_t cmd_rc;
static RC_ctrl_t cc;
static LD_raw_t LD_raw;
static LD_Data_t LD;
#endif
@ -31,10 +33,12 @@ static CMD_RC_t cmd_rc;
* \brief dr16接收机
*
* \param argument 未使用
*/
*/int aaaaaaaaaaaaaaaaaaa=0;
void Task_rc(void *argument) {
(void)argument; /* 未使用，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
uint32_t tick = osKernelGetTickCount();
RC_SBUS_Init(); /* 初始化 */
while (1) {
@ -43,17 +47,26 @@ void Task_rc(void *argument) {
task_runtime.stack_water_mark.rc = osThreadGetStackSpace(osThreadGetId());
#endif
/* 开启DMA */
RC_SBUS_StartDmaRecv();
if (RC_SBUS_WaitDmaCplt(30)) {
RC_StartDmaRecv();
aaaaaaaaaaaaaaaaaaa++;
if (RC_WaitDmaCplt(30)) {
RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);
} else {
/* 处理dr16遥控器离线 ,乐迪不可用*/
DR16_HandleOffline(&dr16, &cmd_rc);
LD_HandleOffline(&LD,&cmd_rc);
/* 处理遥控器离线*/
DR16_HandleOffline(&dr16, &cmd_rc);
LD_HandleOffline(&LD,&cmd_rc);
}
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
tick += delay_tick;
osDelayUntil(tick);
}
}

1
User/task/user_task.h
View File

@ -20,7 +20,6 @@
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_R12DS (1000u)
#define TASK_FREQ_RC (1000u)
#define TASK_FREQ_ERROR_DTC (3u)