diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log
index 2caa556..4555934 100644
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -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
+
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 403d151..b1b816f 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -158,102 +158,22 @@
0
1
- Nor_Vx
+ can,0x0A
1
1
- Nor_Vy
+ chassis,0x0A
2
1
- count,0x0A
+ UP,0x0A
3
1
- count
-
-
- 4
- 1
- pid
-
-
- 5
- 1
- BMI088_t
-
-
- 6
- 1
- up,0x0A
-
-
- 7
- 1
- low,0x0A
-
-
- 8
- 1
- UP,0x0A
-
-
- 9
- 1
- send,0x0A
-
-
- 10
- 1
- cmd,0x0A
-
-
- 11
- 1
- rc_ctrl,0x0A
-
-
- 12
- 1
- a,0x0A
-
-
- 13
- 1
- delay_aa,0x0A
-
-
- 14
- 1
- aaaaaaaaaaa,0x0A
-
-
- 15
- 1
- task_runtime,0x0A
-
-
- 16
- 1
- last_tick,0x0A
-
-
- 17
- 1
- chassis,0x0A
-
-
- 18
- 1
- can,0x0A
-
-
- 19
- 1
- can_rx,0x0A
+ NUC_send,0x0A
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 07eaeff..45fd09e 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 75040b9..1924905 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -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);
+ /*初始数据*/
+ c->move_vec.Vw = 0;
+ c->move_vec.Vx = 0;
+ c->move_vec.Vy = 0;
+ c->NUC_send.send[0] = 0;
+ c->sick_cali.is_reach = 0;
+
switch (c->chassis_ctrl.ctrl) {
- case RCcontrol: // 手动控制
- /*
- 在cmd里对数据进行处理 包括方向和油门
- 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同
- */
- c->NUC_send .send [0]=0;
- c->NUC_send .send [1]=0;
- c->NUC_send .send [2]=10;
- c->NUC_send .send [3]=100;
-
+ 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;
+ case AUTO_MID360_Pitch:
+
break;
- case AUTO_MID360_Adjust:
+ 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;
+ 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;
+
+
+
}
@@ -203,33 +181,35 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
if (ctrl == NULL) return CHASSIS_ERR_NULL;
- fp32 sick0 = c->sick_cali.sick_dis[0];
+ fp32 sick0 = c->sick_cali.sick_dis[0] ;
fp32 sick1 = c->sick_cali.sick_dis[1];
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;
}
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index 8e153bb..dd09723 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -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);
+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
diff --git a/User/Module/config.c b/User/Module/config.c
index 0de4e36..134e59f 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -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,
},
diff --git a/User/Module/up.c b/User/Module/up.c
index 54d8964..95d17c1 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -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 )
diff --git a/User/device/can_use.c b/User/device/can_use.c
index bb02319..0f2730c 100644
--- a/User/device/can_use.c
+++ b/User/device/can_use.c
@@ -1,3 +1,4 @@
+
/*
CAN总线上的设备1到7
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
diff --git a/User/device/cmd.c b/User/device/cmd.c
index 027e402..f5fa1bf 100644
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@@ -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;
}
}
diff --git a/User/device/cmd.h b/User/device/cmd.h
index 6f77a9a..6e7df12 100644
--- a/User/device/cmd.h
+++ b/User/device/cmd.h
@@ -28,6 +28,7 @@ typedef enum{
AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整
+ Chassis_Adjust,
Dribble , //运球
Pitch, //投篮,底盘锁定
diff --git a/User/device/vofa.c b/User/device/vofa.c
index d1f8b7f..b897999 100644
--- a/User/device/vofa.c
+++ b/User/device/vofa.c
@@ -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);
+ float fdata[8] = {0};
+ 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);
+
+}
\ No newline at end of file