diff --git a/MDK-ARM/.vscode/c_cpp_properties.json b/MDK-ARM/.vscode/c_cpp_properties.json index 114c2a6..fb4b89f 100644 --- a/MDK-ARM/.vscode/c_cpp_properties.json +++ b/MDK-ARM/.vscode/c_cpp_properties.json @@ -307,6 +307,114 @@ "__vfp_status(x,y)=0" ], "intelliSenseMode": "${default}" + }, + { + "name": "R2_CHASSIS", + "includePath": [ + "d:\\Desktop\\r2\\R2_CHASSIS\\Core\\Inc", + "d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\STM32F4xx_HAL_Driver\\Inc", + "d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", + "d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", + "d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\CMSIS\\Include", + "d:\\Desktop\\r2\\R2_CHASSIS\\User", + "d:\\Desktop\\r2\\R2_CHASSIS\\User\\bsp", + "d:\\Desktop\\r2\\R2_CHASSIS\\User\\device", + "d:\\Desktop\\r2\\R2_CHASSIS\\User\\task", + "d:\\Desktop\\r2\\R2_CHASSIS\\User\\Algorithm", + "d:\\Desktop\\r2\\R2_CHASSIS\\User\\Module", + "d:\\Desktop\\r2\\R2_CHASSIS\\MDK-ARM", + "d:\\Desktop\\r2\\R2_CHASSIS\\USB_DEVICE\\App", + "d:\\Desktop\\r2\\R2_CHASSIS\\USB_DEVICE\\Target", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc", + "d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\CMSIS\\Lib\\ARM", + "d:\\Desktop\\r2\\R2_CHASSIS\\Core\\Src", + "d:\\Desktop\\r2\\R2_CHASSIS\\Drivers\\STM32F4xx_HAL_Driver\\Src", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src", + "d:\\Desktop\\r2\\R2_CHASSIS\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src" + ], + "defines": [ + "USE_HAL_DRIVER", + "STM32F407xx", + "__CC_ARM", + "__arm__", + "__align(x)=", + "__ALIGNOF__(x)=", + "__alignof__(x)=", + "__asm(x)=", + "__forceinline=", + "__restrict=", + "__global_reg(n)=", + "__inline=", + "__int64=long long", + "__INTADDR__(expr)=0", + "__irq=", + "__packed=", + "__pure=", + "__smc(n)=", + "__svc(n)=", + "__svc_indirect(n)=", + "__svc_indirect_r7(n)=", + "__value_in_regs=", + "__weak=", + "__writeonly=", + "__declspec(x)=", + "__attribute__(x)=", + "__nonnull__(x)=", + "__register=", + "__breakpoint(x)=", + "__cdp(x,y,z)=", + "__clrex()=", + "__clz(x)=0U", + "__current_pc()=0U", + "__current_sp()=0U", + "__disable_fiq()=", + "__disable_irq()=", + "__dmb(x)=", + "__dsb(x)=", + "__enable_fiq()=", + "__enable_irq()=", + "__fabs(x)=0.0", + "__fabsf(x)=0.0f", + "__force_loads()=", + "__force_stores()=", + "__isb(x)=", + "__ldrex(x)=0U", + "__ldrexd(x)=0U", + "__ldrt(x)=0U", + "__memory_changed()=", + "__nop()=", + "__pld(...)=", + "__pli(...)=", + "__qadd(x,y)=0", + "__qdbl(x)=0", + "__qsub(x,y)=0", + "__rbit(x)=0U", + "__rev(x)=0U", + "__return_address()=0U", + "__ror(x,y)=0U", + "__schedule_barrier()=", + "__semihost(x,y)=0", + "__sev()=", + "__sqrt(x)=0.0", + "__sqrtf(x)=0.0f", + "__ssat(x,y)=0", + "__strex(x,y)=0U", + "__strexd(x,y)=0", + "__strt(x,y)=", + "__swp(x,y)=0U", + "__usat(x,y)=0U", + "__wfe()=", + "__wfi()=", + "__yield()=", + "__vfp_status(x,y)=0" + ], + "intelliSenseMode": "${default}" } ], "version": 4 diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index cef60ea..fa42cdc 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -449,3 +449,5 @@ [info] Log at : 2025/1/13|22:53:23|GMT+0800 +[info] Log at : 2025/3/28|21:46:16|GMT+0800 + diff --git a/MDK-ARM/R2_CHASSIS.uvoptx b/MDK-ARM/R2_CHASSIS.uvoptx index f3e593f..d3de66b 100644 --- a/MDK-ARM/R2_CHASSIS.uvoptx +++ b/MDK-ARM/R2_CHASSIS.uvoptx @@ -135,7 +135,7 @@ 0 ST-LINKIII-KEIL_SWO - -U-O206 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + -U00260035480000034E575152 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8000 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) 0 @@ -160,13 +160,63 @@ 1 dr16,0x0A + + 2 + 1 + chassis,0x0A + + + 3 + 1 + nuc_raw,0x0A + + + 4 + 1 + nucbuf + + + 5 + 1 + dr16,0x0A + + + 6 + 1 + cmd_fromnuc,0x0A + + + 7 + 1 + ctrl,0x0A + + + 8 + 1 + can,0x0A + + + 9 + 1 + out,0x0A + + + 10 + 1 + can_out,0x0A + + + 11 + 1 + cmd_fromnuc,0x0A + 0 0 - 0 + 1 1 0 0 @@ -247,7 +297,7 @@ Application/User/Core - 0 + 1 0 0 0 @@ -659,7 +709,7 @@ Drivers/CMSIS - 0 + 1 0 0 0 @@ -679,7 +729,7 @@ Middlewares/FreeRTOS - 0 + 1 0 0 0 @@ -839,7 +889,7 @@ User/bsp - 0 + 1 0 0 0 @@ -1131,7 +1181,7 @@ User/Algorithm - 0 + 1 0 0 0 @@ -1211,7 +1261,7 @@ User/Device - 0 + 1 0 0 0 @@ -1303,7 +1353,7 @@ Application/User/USB_DEVICE/App - 0 + 1 0 0 0 @@ -1347,7 +1397,7 @@ Application/User/USB_DEVICE/Target - 0 + 1 0 0 0 @@ -1367,7 +1417,7 @@ Middlewares/USB_Device_Library - 0 + 1 0 0 0 diff --git a/MDK-ARM/R2_CHASSIS.uvprojx b/MDK-ARM/R2_CHASSIS.uvprojx index 35261e3..d639b59 100644 --- a/MDK-ARM/R2_CHASSIS.uvprojx +++ b/MDK-ARM/R2_CHASSIS.uvprojx @@ -10,6 +10,7 @@ R2_CHASSIS 0x4 ARM-ADS + 5060960::V5.06 update 7 (build 960)::.\ARMCC 5060960::V5.06 update 7 (build 960)::.\ARMCC 0 @@ -81,7 +82,7 @@ 0 - 1 + 0 0 diff --git a/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf b/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf new file mode 100644 index 0000000..3aea999 Binary files /dev/null and b/MDK-ARM/R2_CHASSIS/R2_CHASSIS.axf differ diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index 6d118c6..3d163b4 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -309,3 +309,12 @@ uint8_t average(uint8_t arr[], uint8_t n) { } return (float) sum / n; } + + +int abs_value(int num) { + if (num >= 0) { + return num; // 如果数是非负的，直接返回该数 + } else { + return -num; // 如果数是负的，返回它的相反数 + } +} \ No newline at end of file diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index 21cf578..766dc9a 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -157,4 +157,7 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2); /// @param n 元素数 /// @return 平均值 uint8_t average(uint8_t arr[], uint8_t n); + +int abs_value(int num); + #endif diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index d2f5a2f..051c4e8 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -71,23 +71,11 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq) PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param)); //带D项滤波 } - PID_init((&c->pid.chassis_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度 - PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_param)); + PID_init(&(c->pid.chassis_PICKWzPID_IN),PID_POSITION,&(c->param->chassis_PICKWzPIDIN_param)); - PID_init((&c->pid.chassis_gimbal_yawAnglePID),PID_POSITION,&(c->param->Gimbal_yawAngle_param));//尝试位置控制角度 - - PID_init((&c->pid.chassis_gimbal_yawOmegaPID),PID_POSITION,&(c->param->Gimbal_yawOmega_param)); + PID_init(&(c->pid.chassis_PICKWzPID_OUT),PID_POSITION,&(c->param->chassis_PICKWzPIDOUT_param)); - PID_init((&c->pid.chassis_gimbal_pitchAnglePID),PID_POSITION,&(c->param->Gimbal_pitchAngle_param));//尝试位置控制角度 - - PID_init((&c->pid.chassis_gimbal_pitchOmegaPID),PID_POSITION,&(c->param->Gimbal_pitchOmega_param)); - - PID_init(&(c->pid.chassis_NaviVxPID),PID_POSITION,&(c->param->NaviVx_param)); - - PID_init(&(c->pid.chassis_NaviVyPID),PID_POSITION,&(c->param->NaviVy_param)); - - PID_init(&(c->pid.chassis_NaviWzPID),PID_POSITION,&(c->param->NaviVw_param)); PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam)); @@ -180,11 +168,25 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) break; - case PICK: - c->move_vec.Vw =ctrl->C_navi.wz ; - c->move_vec.Vx =ctrl->C_navi.vy ; - c->move_vec.Vy =ctrl->C_navi.vx ; + case PICK_t: + + c->move_vec.Vx =ctrl->Vx*6000 ; + c->move_vec.Vy =ctrl->Vy *6000; + c->move_vec .Vw = ctrl->C_pick .posx ; +// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw); + if(abs_value(ctrl ->C_pick .posx )>10) + { + c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_IN),-(ctrl ->C_pick .posx) ,0); + + } + else if(abs_value(ctrl ->C_pick .posx )<0.2) + {c->move_vec.Vw =0; + } + else + c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_OUT),-(ctrl ->C_pick .posx) ,0); + + break ; diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index e906470..7f4f6c8 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -82,16 +82,10 @@ typedef struct /*该部分决定PID的参数整定在config中修改*/ pid_param_t M3508_param; - pid_param_t AngleCor_param; - pid_param_t OmegaCor_param; - pid_param_t DisCamera_param; - pid_param_t ImuCor_param; - pid_param_t C6020pitAngle_param; - pid_param_t C6020pitOmega_param; - pid_param_t Gimbal_yawAngle_param; - pid_param_t Gimbal_yawOmega_param; - pid_param_t Gimbal_pitchAngle_param; - pid_param_t Gimbal_pitchOmega_param; + + pid_param_t chassis_PICKWzPIDIN_param; + pid_param_t chassis_PICKWzPIDOUT_param; + pid_param_t NaviVx_param; pid_param_t NaviVy_param; pid_param_t NaviVw_param; @@ -166,21 +160,19 @@ typedef struct{ struct{ pid_type_def chassis_3508VecPID[4]; - pid_type_def chassis_pitAngle6020; - pid_type_def chassis_pitOmega6020; - pid_type_def chassis_gimbal_yawAnglePID; - pid_type_def chassis_gimbal_yawOmegaPID; - pid_type_def chassis_gimbal_pitchAnglePID; - pid_type_def chassis_gimbal_pitchOmegaPID; - pid_type_def chassis_NaviVxPID; - pid_type_def chassis_NaviVyPID; - pid_type_def chassis_NaviWzPID; + pid_type_def chassis_PICKWzPID_IN; + pid_type_def chassis_PICKWzPID_OUT; + + pid_type_def chassis_NaviWzPID; + pid_type_def chassis_NaviVxPID; + pid_type_def chassis_NaviVyPID; + pid_type_def sick_CaliforYPID; pid_type_def sick_CaliforXPID; - pid_type_def Action_VxPID; - pid_type_def Action_VyPID; - pid_type_def Action_WzPID; +// pid_type_def Action_VxPID; +// pid_type_def Action_VyPID; +// pid_type_def Action_WzPID; }pid; fp32 vofa_send[8]; diff --git a/User/Module/config.c b/User/Module/config.c index 909b2ab..8c82fd0 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -8,21 +8,6 @@ #define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度 -/*导航地图*/ -#define point_num 4 - -point_t map_point[point_num] ={ - - {0.0, 0, DEG_TO_RAD(90.0), 5.0}, - - {0, 0, DEG_TO_RAD(180.0), 5.0}, -// - {0.0, 0.0, DEG_TO_RAD(270.0), 5.0}, - - {0.0, 0.0, DEG_TO_RAD(360.0), 5.0}, - - -}; #ifdef DEBUG @@ -36,83 +21,7 @@ static const ConfigParam_t param_chassis ={ #endif .chassis = {/**/ - .C6020pitAngle_param = { - .p = 15.0f, - .i = 0.3f, - .d =0.0f, - .i_limit = 200.0f, - .out_limit = 3000.0f, - }, - .C6020pitOmega_param = { - .p =30.0f, - .i =0.3f, - .d =0.0f, - .i_limit = 200.0f, - .out_limit = 3000.0f - }, - .Gimbal_yawAngle_param = { - .p =8.0f, - .i =0.0f, - .d =0.0f, - .i_limit = 200.0f, - .out_limit = 3000.0f - }, - - .Gimbal_yawOmega_param = { - .p =18.0f, - .i =0.0f, - .d =0.0f, - .i_limit = 200.0f, - .out_limit = 3000.0f - }, - - .Gimbal_pitchAngle_param = { - .p =8.0f, - .i =0.0f, - .d =0.0f, - .i_limit = 200.0f, - .out_limit = 3000.0f - }, - - .Gimbal_pitchOmega_param = { - .p =18.0f, - .i =0.0f, - .d =0.0f, - .i_limit = 200.0f, - .out_limit = 3000.0f - }, - .AngleCor_param = { - .p =0.8f, - .i =0.0f, - .d =1.0f, - .i_limit = 0.0f, - .out_limit =5000.0f, - }, - - .OmegaCor_param = { - .p =23.5f, - .i =0.0f, - .d =0.05f, - .i_limit = 0.0f, - .out_limit =5000.0f, - }, - - .ImuCor_param = { - .p =95.0f, - .i =0.0f, - .d =0.0f, - .i_limit = 0.0f, - .out_limit =200.0f, - }, - - .DisCamera_param = { - .p =80.0f, - .i =0.1f, - .d =0.0f, - .i_limit = 0.0f, - .out_limit =5000.0f, - }, .M3508_param = { .p = 15.1f, @@ -121,6 +30,20 @@ static const ConfigParam_t param_chassis ={ .i_limit = 200.0f, .out_limit =6000.0f, }, + .chassis_PICKWzPIDIN_param ={ + .p = 5.0f, + .i = 0.0f, + .d = 0.15f, + .i_limit = 0.0f, + .out_limit =5000.0f, + }, + .chassis_PICKWzPIDOUT_param ={ + .p = 10.0f, + .i = 0.02f, + .d = 0.00f, + .i_limit = 0.0f, + .out_limit =5000.0f, + }, // .M3508_param = { // .p = 10.0f, // .i = 0.0f, diff --git a/User/device/cmd.c b/User/device/cmd.c index db1937d..76b1653 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -187,7 +187,7 @@ int8_t CMD_CtrlSet(CMD_t *cmd) { { if(cmd->key_ctrl_r==CMD_SW_UP) { - cmd->C_cmd.type = RC; + cmd->C_cmd.type = PICK_t; cmd->C_cmd.mode = RC_MODE3; } if(cmd->key_ctrl_r==CMD_SW_MID) diff --git a/User/device/nuc.c b/User/device/nuc.c index d05906f..05bc64a 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -6,7 +6,7 @@ static volatile uint32_t drop_message = 0; static osThreadId_t thread_alert; -uint8_t nucbuf[32]; +uint8_t nucbuf[31]; static void NUC_IdleCallback(void) { osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY); @@ -54,7 +54,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){ case MID://控制帧0x09 /* 协议格式 0xFF HEAD - 0x0X 控制帧 + 0x09 控制帧 0x01 相机帧 vx fp32 vy fp32