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

sick定位

Browse Source
This commit is contained in:
ZHAISHUI04 2025-06-15 16:29:12 +08:00
parent afdb0edbc3
commit e9710e65a5
11 changed files with 152 additions and 240 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

@ -459,3 +459,7 @@
[info] Log at : 2025/6/12|16:54:50|GMT+0800
[info] Log at : 2025/6/13|16:30:54|GMT+0800
[info] Log at : 2025/6/14|15:14:16|GMT+0800

88
MDK-ARM/R2.uvoptx
View File

@ -158,102 +158,22 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vx</ItemText>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vy</ItemText>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>count,0x0A</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>pid</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>BMI088_t</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>up,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>low,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>send,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>a,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>delay_aa,0x0A</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>aaaaaaaaaaa,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>task_runtime,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>last_tick,0x0A</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>can_rx,0x0A</ItemText>
<ItemText>NUC_send,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>

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

Binary file not shown.

124
User/Module/Chassis.c
View File

@ -28,6 +28,11 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
}
for (uint8_t i = 0; i < 4; i++) {
c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
}
return CHASSIS_OK;
}
@ -40,8 +45,10 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
}
PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
PID_init(&(c->pid.sick_CaliforYPID), PID_POSITION, &(c->param->Sick_CaliYparam));
PID_init(&(c->pid.sick_CaliforXPID), PID_POSITION, &(c->param->Sick_CaliXparam));
PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam));
PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
@ -65,7 +72,6 @@ void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw; // 左前
}
float aaaaa=0;
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;
@ -75,98 +81,66 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
// IMU加滤波修正
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
switch (c->chassis_ctrl.ctrl) {
case RCcontrol: // 手动控制
/*
在cmd里对数据进行处理 包括方向和油门
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同
*/
/*初始数据*/
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->NUC_send.send[0] = 0;
c->NUC_send .send [1]=0;
c->NUC_send .send [2]=10;
c->NUC_send .send [3]=100;
c->sick_cali.is_reach = 0;
switch (c->chassis_ctrl.ctrl) {
case RCcontrol:
switch (c->chassis_ctrl.mode) {
case Normal:
if(ctrl->Vw){
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
}
else
{
c->move_vec.Vw = ctrl->Vw * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa;
c->move_vec.Vx = ctrl->Vy * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ;
c->move_vec.Vy = ctrl->Vx * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ;
}
break;
case Pitch:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
case UP_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
case Chassis_Adjust:
sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
}
break;
case AUTO: // 自动模式
case AUTO:
switch (c->chassis_ctrl.mode) {
case AUTO_MID360: // 自动雷达
// 全向轮方向, 注意xy方向
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]=0;
break;
case AUTO_MID360_Pitch:
c->move_vec.Vw = 0;
c->move_vec.Vy = 0;
c->move_vec.Vx = 0;
c->NUC_send .send [0]=0;
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;
c->NUC_send.send[0] = 1;
break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->NUC_send .send [0]=0;
break;
}
break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
}
@ -183,8 +157,12 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->hopemotorout.OmniSpeedOut[i]);
out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
}
c->NUC_send .send [1]=1;
c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
return CHASSIS_OK;
}
@ -208,26 +186,28 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam;
fp32 diff = fabsf(sick0 - sick1);
fp32 diff_yaw = -(fp32)(sick1 - sick2);
fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
// 1. yaw修正
if (diff > param->w_error) {
c->move_vec.Vw = PID_calc(&(c->pid.chassis_SickVxPID), diff, 0);
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
}
// 2. xy修正
else if (diff > param->xy_error) {
c->move_vec.Vw = 0;
c->move_vec.Vx = PID_calc(&(c->pid.chassis_SickVxPID), sick0, param->x_set);
c->move_vec.Vy = PID_calc(&(c->pid.chassis_SickVyPID), sick2, param->y_set);
}
// 3. 修正完成
else {
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
c->move_vec.Vx = (fabsf(diff_x) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
c->move_vec.Vy = (fabsf(diff_y) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0;
static uint16_t reach_cnt = 0;
if (fabsf(diff_yaw) <= param->w_error &&
fabsf(diff_x) <= param->xy_error &&
fabsf(diff_y) <= param->xy_error) {
reach_cnt++;
if (reach_cnt >= 50) {
c->sick_cali.is_reach = 1;
}
} else {
reach_cnt = 0;
c->sick_cali.is_reach = 0;
}
return CHASSIS_OK;

34
User/Module/Chassis.h
View File

@ -156,12 +156,10 @@ typedef struct{
pid_type_def chassis_SickWzPID;
pid_type_def chassis_SickVxPID;
pid_type_def chassis_SickVyPID;
pid_type_def SickCaliWzPID;
pid_type_def SickCaliVxPID;
pid_type_def SickCaliVyPID;
pid_type_def sick_CaliforYPID;
pid_type_def sick_CaliforXPID;
}pid;
@ -170,8 +168,9 @@ typedef struct{
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
struct {
int32_t sick_dis[3]; //获取到的sick激光值
int32_t sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;
NUC_send_t NUC_send;
@ -179,38 +178,17 @@ typedef struct{
}Chassis_t;
/**
* @brief
*
* @param c
* @param param
* @param mech_zero
* @param wheelPolar
* @return
*/
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
/**
* \brief
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
/**
* \brief
*/
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
fp32 jiuzheng(fp32 yaw);
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;
/// @brief
/// @param c
void vesc_current_detection(Chassis_t *c);
#endif

36
User/Module/config.c
View File

@ -99,8 +99,8 @@ static const ConfigParam_t param ={
.dribble_flag=0,
.m3508_init_ang = -5,
.m3508_translate_angle = 1000,
.rev_spd=-1500,
.spd= 4000, // 转动速度
.rev_spd=-2000,
.spd= 4200, // 转动速度
.init_spd=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
@ -147,31 +147,31 @@ static const ConfigParam_t param ={
.out_limit =1000.0f,
},
.Sick_CaliWparam ={
.p = 0.0f,
.i = 0.0f,
.p = 4.5f,
.i = 0.005f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =0.0f,
.i_limit = 500.0f,
.out_limit =1000.0f,
},
.Sick_CaliYparam ={
.p = 0.0f,
.i = 0.0f,
.p = 2.9f,
.i = 0.0051f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.Sick_CaliXparam ={
.p = 0.0f,
.i = 0.0f,
.p = 2.9f,
.i = 0.0065f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.sickparam={
.w_error=5000,
.xy_error=5000,
.x_set=1000,
.y_set=1000,
.w_error=5,
.xy_error=5,
.x_set=927,
.y_set=1255,
},

5
User/Module/up.c
View File

@ -520,7 +520,10 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
// CoPITCH, //发射
// CoDONE,
//int8_t Trans_Process (UP_t *u, CAN_Output_t *out)
//{
//
//}
int8_t Co_Process(UP_t *u, CAN_Output_t *out)
{
switch(u->CoContext .CoState )

1
User/device/can_use.c
View File

@ -1,3 +1,4 @@
/*
CAN总线上的设备1到7
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制

1
User/device/cmd.c
View File

@ -205,6 +205,7 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust;
else cmd ->CMD_mode =Normal;
}
}

1
User/device/cmd.h
View File

@ -28,6 +28,7 @@ typedef enum{
AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整
Chassis_Adjust,
Dribble , //运球
Pitch, //投篮,底盘锁定

72
User/device/vofa.c
View File

@ -5,37 +5,61 @@
//用来对需要的数据进行串口绘图 （未添加接收函数）
//float vofa_send[8];
////发送函数
//void vofa_tx_main(float *data)
//{
// float fdata[8]={0};
// uint8_t tail[4];
// tail[0]=0x00;
// tail[1]=0x00;
// tail[2]=0x80;
// tail[3]=0x7f;
// /*在下面添加发送的数据*/
// fdata[0] = data[0];
// fdata[1] = data[1];
// fdata[2] = data[2];
// fdata[3] = data[3];
// fdata[4] = data[4];
// fdata[5] = data[5];
// fdata[6] = data[6];
// fdata[7] = data[7];
//
//
// /*在下面使用对应的串口发送函数*/
//// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
//// osDelay(1);
//// CDC_Transmit_FS( tail, 4);
//// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
//// osDelay(1);
// HAL_UART_Transmit_DMA(&huart1, tail, 4);
// osDelay(1);
//
//}
float vofa_send[8];
//发送函数
void vofa_tx_main(float *data)
{
float fdata[8] = {0};
uint8_t tail[4];
tail[0]=0x00;
tail[1]=0x00;
tail[2]=0x80;
tail[3]=0x7f;
/*在下面添加发送的数据*/
fdata[0] = data[0];
fdata[1] = data[1];
fdata[2] = data[2];
fdata[3] = data[3];
fdata[4] = data[4];
fdata[5] = data[5];
fdata[6] = data[6];
fdata[7] = data[7];
/*在下面使用对应的串口发送函数*/
// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// CDC_Transmit_FS( tail, 4);
// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// HAL_UART_Transmit_DMA(&huart1, tail, 4);
// osDelay(1);
uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};
for (int i = 0; i < 8; i++) {
fdata[i] = data[i];
}
uint8_t packet[8 * sizeof(float) + 4];
memcpy(packet, fdata, 8 * sizeof(float));
memcpy(packet + 8 * sizeof(float), tail, 4);
/*在下面使用对应的串口发送函数*/
// HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
// CDC_Transmit_FS( packet, sizeof(packet));
// osDelay(1);
}