diff --git a/MDK-ARM/.vscode/c_cpp_properties.json b/MDK-ARM/.vscode/c_cpp_properties.json index fb4b89f..1efa7c0 100644 --- a/MDK-ARM/.vscode/c_cpp_properties.json +++ b/MDK-ARM/.vscode/c_cpp_properties.json @@ -415,6 +415,114 @@ "__vfp_status(x,y)=0" ], "intelliSenseMode": "${default}" + }, + { + "name": "R2", + "includePath": [ + "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm", + "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module", + "d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM", + "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App", + "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang", + "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src", + "d:\\Desktop\\r2\\R2_NEW\\R2\\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 fa42cdc..488f11b 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -451,3 +451,23 @@ [info] Log at : 2025/3/28|21:46:16|GMT+0800 +[info] Log at : 2025/6/6|21:51:18|GMT+0800 + +[info] Log at : 2025/6/7|17:45:08|GMT+0800 + +[info] Log at : 2025/6/7|21:03:23|GMT+0800 + +[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 + +[info] Log at : 2025/6/16|23:35:32|GMT+0800 + +[info] Log at : 2025/6/18|17:47:04|GMT+0800 + +[info] Log at : 2025/6/18|19:43:09|GMT+0800 + +[info] Log at : 2025/6/22|14:26:08|GMT+0800 + diff --git a/MDK-ARM/.vscode/settings.json b/MDK-ARM/.vscode/settings.json index e9743cc..681d6c0 100644 --- a/MDK-ARM/.vscode/settings.json +++ b/MDK-ARM/.vscode/settings.json @@ -96,6 +96,7 @@ "limits": "c", "gimbal_task.h": "c", "gimgal.h": "c", - "navi.h": "c" + "navi.h": "c", + "up.h": "c" } } \ No newline at end of file diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index a71f241..42438c7 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -103,7 +103,7 @@ 1 0 0 - 6 + 3 @@ -114,7 +114,7 @@ - STLink\ST-LINKIII-KEIL_SWO.dll + BIN\CMSIS_AGDI.dll @@ -140,7 +140,7 @@ 0 DLGUARM - (105=-1,-1,-1,-1,0) + 0 @@ -158,157 +158,72 @@ 0 1 - rc_ctrl,0x0A + can,0x0A 1 1 - LD_raw,0x0A + chassis,0x0A 2 1 - can,0x0A + UP,0x0A 3 1 - UP,0x0A + NUC_send,0x0A 4 1 - cmd_rc,0x0A + pid,0x0A 5 1 - chassis + pid_param,0x0A 6 1 - nuc_raw,0x0A + cmd_rc,0x0A 7 1 - nucbuf + cbuf,0x10 8 1 - cmd_fromnuc + cmd 9 1 - up_cmd,0x0A + rc_ctrl,0x0A 10 1 - can_out + nuc_raw,0x0A 11 1 - Nor_Vx + cmd_fromnuc,0x0A 12 1 - Nor_Vy + nucbuf,0x0A 13 1 - b - - - 14 - 1 - count,0x0A - - - 15 - 1 - count - - - 16 - 1 - raw_rx1,0x0A - - - 17 - 1 - gyro_kp,0x0A - - - 18 - 1 - nucbuf,0x0A - - - 19 - 1 - vofa_send,0x0A - - - 20 - 1 - SendBuffer,0x10 - - - 21 - 1 - NUC_StartSending,0x0A - - - 22 - 1 - PIAN - - - 23 - 1 - pid - - - 24 - 1 - bmi088 - - - 25 - 1 - ist8310 - - - 26 - 1 - imu_eulr,0x0A - - - 27 - 1 - imu_temp,0x0A - - - 28 - 1 - htim10,0x0A - - - 29 - 1 - pulse - - - 30 - 1 - imu_temp + LD_raw,0x0A @@ -1316,8 +1231,8 @@ 0 0 0 - ..\User\Algorithm\kalman.c - kalman.c + ..\User\Algorithm\user_cmsis_os2.c + user_cmsis_os2.c 0 0 @@ -1328,8 +1243,8 @@ 0 0 0 - ..\User\Algorithm\user_cmsis_os2.c - user_cmsis_os2.c + ..\User\Algorithm\kalman.c + kalman.c 0 0 diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx index 2e3293a..42309e5 100644 --- a/MDK-ARM/R2.uvprojx +++ b/MDK-ARM/R2.uvprojx @@ -1248,16 +1248,16 @@ 1 ..\User\Algorithm\ahrs.c - - kalman.c - 1 - ..\User\Algorithm\kalman.c - user_cmsis_os2.c 1 ..\User\Algorithm\user_cmsis_os2.c + + kalman.c + 1 + ..\User\Algorithm\kalman.c + user_math.c 1 diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index ccce2e5..b0eb3ad 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -216,7 +216,21 @@ void abs_limit_fp(fp32 *num, fp32 Limit) *num = -Limit; } } - +//改原始值，限制最大最小 +fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max) +{ + if (*num > Limit_max) + { + *num=Limit_max; + return Limit_max; + } + else if (*num < Limit_min) + { + *num=Limit_min; + return Limit_min; + + } +} /* 移动向量 */ MoveVector_t *mv; diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index dd4d8ab..0dad6dd 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -164,5 +164,6 @@ int read_flag_state(uint8_t flag) ; void normalize_vector(double x, double y, double *out_x, double *out_y) ; bool is_reached(float current, float target, float mistake) ; bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ; +fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max); #endif diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index f2ce673..b5a18dd 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -4,197 +4,148 @@ #include "bsp_buzzer.h" #include "bsp_delay.h" - - - - - -/*机器人坐标系，向前x，右y，上yaw - 不同于nuc (前x，左y，上yaw) */ -/* - x - | - --y - - +/* +机器人坐标系，向前x，右y，上yaw +不同于nuc (前x，左y，上yaw) + x + | + --y */ -static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){ - if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */ - if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/ - c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ; - c->chassis_ctrl.mode =ctrl->CMD_mode ; - - return CHASSIS_OK; -} //设置控制模式 - - -/*该函数用来更新can任务获得的电机反馈值*/ +static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) { + if (c == NULL) return CHASSIS_ERR_NULL; + if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) + return CHASSIS_OK; + c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType; + c->chassis_ctrl.mode = ctrl->CMD_mode; + return CHASSIS_OK; +} int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) { - if (c == NULL) return CHASSIS_ERR_NULL; - if (can == NULL) return CHASSIS_ERR_NULL; - for (uint8_t i = 0; i < 4; i++) - { - 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; - } - - - - return CHASSIS_OK; -} - - -int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq) -{ - if (c == NULL) return CHASSIS_ERR_NULL; - - c->param = param; /*初始化参数 */ - - for(int i =0 ; i < 4 ; i++) - { - PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param)); //带D项滤波 - } - - 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)); - - LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f); //给角加速度做滤波 - - - LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波 - - LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给y做滤波 - - LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波 - - // - - - return CHASSIS_OK; -} - -fp32 gyro_kp=1.0f; -fp32 PIAN=0; -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] = -Vx+Vy+Vw;//右前 -// c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后 -// c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后 -// c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前 -// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前 -// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后 -// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后 -// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左前 - 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 ;//左前 - -// if(!Vw){ -// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw ); -// } -// c->hopemotorout.OmniSpeedOut[0] += PIAN ;//右前 -// c->hopemotorout.OmniSpeedOut[1] += PIAN ;//右后 -// c->hopemotorout.OmniSpeedOut[2] += PIAN ;//左后 -// c->hopemotorout.OmniSpeedOut[3] += PIAN ;//左前 - -} - - -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); - - switch (c->chassis_ctrl .ctrl) - { - case RCcontrol: //手动控制 + if (c == NULL) return CHASSIS_ERR_NULL; + if (can == NULL) return CHASSIS_ERR_NULL; + for (uint8_t i = 0; i < 4; i++) { + 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++) { -/* - 在cmd里对数据进行处理 包括方向和油门 - 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/ - if(c->chassis_ctrl .mode == Pitch){ - c->move_vec.Vw = 0; - c->move_vec.Vx = 0; - c->move_vec.Vy = 0; - } - else if(c->chassis_ctrl .mode == UP_Adjust) - { - c->move_vec.Vw = ctrl->Vw*6000; - c->move_vec.Vx = 0; - c->move_vec.Vy = 0; - } - else { - c->move_vec.Vw = ctrl->Vw*6000; - c->move_vec.Vx = ctrl->Vy*6000; - c->move_vec.Vy = ctrl->Vx*6000; - } + c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值 + } + return CHASSIS_OK; +} + +int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) { + if (c == NULL) return CHASSIS_ERR_NULL; + c->param = param; + + for (int i = 0; i < 4; i++) { + PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param)); + } + 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.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滤波 + LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波 + LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波 + + c->sick_cali .sickparam=c->param ->sickparam ; + + return CHASSIS_OK; +} + +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+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); - break; +} +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; - case AUTO: // 自动模式 + Chassis_SetCtrl(c, ctrl); + // 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: switch (c->chassis_ctrl.mode) { + case Normal: + 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; + + break; + case Chassis_Adjust: + + sick_calibration(c, ctrl, out); + c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0; + break; + default: + + break; + } + break; - case AUTO_MID360: // 自动雷达 - // 全向轮方向, 注意xy方向 + 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]=0; + c->NUC_send.send[0] = 1; 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; + 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; } @@ -203,65 +154,102 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) abs_limit_fp(&c->move_vec.Vy, 6000.0f); abs_limit_fp(&c->move_vec.Vw, 6000.0f); - Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); - - - for (uint8_t i = 0 ; i <4 ; i++) - { - c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]); - - out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i]; - } - + Chassis_speed_calculate(c, c->move_vec.Vx, c->move_vec.Vy, c->move_vec.Vw); + for (uint8_t i = 0; i < 4; i++) { + c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]), + c->motorfeedback.rotor_rpm3508[i], + c->hopemotorout.OmniSpeedOut[i]); + out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i]; + } -// c->vofa_send[0]=c->pos088.bmi088.gyro.z; - -// c->vofa_send[1]=c->motorfeedback .rotor_rpm3508 [0]; -// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1]; -// c->vofa_send[3]=c->motorfeedback .rotor_rpm3508 [2]; -// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [3]; -// -// c->vofa_send[0]=c->hopemotorout .OmniSpeedOut [0]; -// c->vofa_send[1]=c->hopemotorout .OmniSpeedOut [0]; -//// c->vofa_send[2]=c->hopemotorout .OmniSpeedOut [2]; -// c->vofa_send[3]=c->hopemotorout .OmniSpeedOut [3]; - -//// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [0]; -// c->vofa_send[0]=c->motorfeedback .rotor_rpm3508 [0]; -//// c->vofa_send[6]=c->motorfeedback .rotor_rpm3508 [2]; -// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [3]; -// -// c->vofa_send[5]=c->pos088 .imu_eulr .yaw ; - - return CHASSIS_OK; - + c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2]; + + return CHASSIS_OK; + + + } - pid_type_def pid; - pid_param_t pid_param={ - .p = 0.50f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 50.0f, - .out_limit = 100.0f - }; -fp32 jiuzheng(fp32 yaw) + + +/* +sick0,下 +sick1,左2 +sick2,左1 + + +*/ + +int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { - static fp32 pian_yaw=0; - static fp32 shang_yaw=0; - static int is=0; + if (c == NULL) return CHASSIS_ERR_NULL; + if (ctrl == NULL) return CHASSIS_ERR_NULL; - - if(is==0) - { - PID_init (&pid,PID_POSITION ,&pid_param); - is=1; - } - - pian_yaw+= (yaw - shang_yaw); - shang_yaw=yaw ; - - return PID_calc(&pid,pian_yaw,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_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); + + + 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; } + +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); +} + + diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index acef449..e2e2e4a 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -58,7 +58,12 @@ typedef struct { AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制） }ChassisImu_t; - +typedef struct { + int32_t w_error;//角度纠正时的允许误差量 + int32_t xy_error;//xy允许误差量 + int32_t x_set; + int32_t y_set; +}sickparam_t; /* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */ typedef struct @@ -74,10 +79,13 @@ typedef struct pid_param_t NaviVx_param; pid_param_t NaviVy_param; pid_param_t NaviVw_param; + + pid_param_t Sick_CaliWparam; pid_param_t Sick_CaliYparam; pid_param_t Sick_CaliXparam; + sickparam_t sickparam; }Chassis_Param_t; @@ -90,12 +98,12 @@ typedef struct fp32 Vw; fp32 mul;//油门倍率 }ChassisMove_Vec; - - typedef struct +typedef struct { fp32 send[4]; }NUC_send_t; + /** * @brief * @@ -148,12 +156,10 @@ typedef struct{ - pid_type_def chassis_NaviWzPID; - pid_type_def chassis_NaviVxPID; - pid_type_def chassis_NaviVyPID; + pid_type_def SickCaliWzPID; + pid_type_def SickCaliVxPID; + pid_type_def SickCaliVyPID; - pid_type_def sick_CaliforYPID; - pid_type_def sick_CaliforXPID; }pid; @@ -161,43 +167,29 @@ typedef struct{ LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ - + struct { int32_t sick_dis[4]; //获取到的sick激光值 - NUC_send_t NUC_send; + sickparam_t sickparam; + int is_reach; + }sick_cali; + + NUC_send_t NUC_send; + + }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) ; + +int8_t Chassis_AngleCompensate(Chassis_t *c) ; - - -/// @brief -/// @param c -void vesc_current_detection(Chassis_t *c); #endif diff --git a/User/Module/config.c b/User/Module/config.c index 65a65cd..ef628ab 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -95,13 +95,13 @@ static const ConfigParam_t param ={ /*上层其他参数*/ /*运球*/ - .DribbleConfig_Config = { + .DribbleCfg = { .dribble_flag=0, - .m3508_init_angle = -5, - .m3508_translate_angle = -930, - .m3508_dribble_Reverse_speed=-3500, - . m3508_dribble_speed= 4000, // 转动速度 - .m3508_dribble_init_speed=0, + .m3508_init_ang = -5, + .m3508_translate_angle = 1000, + .rev_spd=-2000, + .spd= 4200, // 转动速度 + .init_spd=0, .light_3508_flag=0,//3508平移处的光电，0初始，1到位置 .light_ball_flag=0,//检测球的flag @@ -109,21 +109,21 @@ static const ConfigParam_t param ={ }, /*投球*/ - .PitchConfig_Config = { - .m2006_init_angle =-90, - .m2006_trigger_angle =0, - .go1_init_position = -50, - .go1_Receive_ball = -5, //偏下 - .go1_release_threshold =-210, - .m2006_Screw_init=0, - .Pitch_angle =56, + .PitchCfg = { + .m2006_init =-150, //释放的角度 + .m2006_trig =0, //可以拉住发射的角度 + .go_init = -50, //仅用在go上电，初始位置 + .go_pull_pos =-214, + .Pitch_angle =58, + + .pull_speed =5, + }, - - + }, .chassis = {/**/ - + .M3508_param = { .p = 15.1f, .i = 0.02f, @@ -132,21 +132,49 @@ static const ConfigParam_t param ={ .out_limit =6000.0f, }, /*视觉*/ - .chassis_PICKWzPID_HIGN_param ={ //高响应 + .chassis_PICKWzPID_HIGN_param ={ .p = 1.0f, .i = 0.03f, .d = 0.03f, .i_limit = 100.0f, .out_limit =2000.0f, }, - .chassis_PICKWzPID_LOW_param ={ //高精度 + .chassis_PICKWzPID_LOW_param ={ .p = 0.5f, //1.0 0.5 .i = 0.5f, //0.01 0.04 .d = 0.0f, //0.02 0.02 .i_limit = 50.0f, .out_limit =1000.0f, }, - + .Sick_CaliWparam ={ + .p = 4.5f, + .i = 0.005f, + .d = 0.0f, + .i_limit = 500.0f, + .out_limit =1000.0f, + }, + .Sick_CaliYparam ={ + .p = 2.9f, + .i = 0.0051f, + .d = 0.0f, + .i_limit = 500.0f, + .out_limit =3000.0f, + }, + .Sick_CaliXparam ={ + .p = 2.9f, + .i = 0.0065f, + .d = 0.0f, + .i_limit = 500.0f, + .out_limit =3000.0f, + }, + .sickparam={ + .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 ad0209e..f9d6b73 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -53,17 +53,17 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) // 初始化上层状态机 - if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球 - u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 - u->DribbleContext .DribbleState = DRIBBLE_PREPARE; - u->DribbleContext .is_initialized = 1; + if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球 + u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值 + u->DribbleCont .DribbleState = DRIBBLE_PREPARE; + u->DribbleCont .is_init = 1; } - if (!u->PitchContext .is_initialized) { - u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值 - u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 - u->PitchContext .is_initialized = 1; + if (!u->Pitch_Cfg .is_init) { + u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值 + u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 + u->Pitch_Cfg .is_init = 1; } BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层 @@ -96,8 +96,8 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { u->cmd =c; /*光电状态更新*/ - u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); - u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); + u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); + u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); return 0; @@ -168,17 +168,8 @@ int8_t GO_SendData(UP_t *u, float pos, float limit) static int is_calibration=0; static fp32 error=0; //误差量 -// GO_MotorData_t *GO_calibration;//校准前，原始数据 u->motorfeedback .go_data = get_GO_measure_point() ; -// if(is_calibration==0) -// { -// is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 ); -// u->go_cmd.Pos = -50; //上电之后跑 -// error= GO_calibration->Pos ; -// } -// u->motorfeedback .go_data = GO_calibration; -// u->motorfeedback .go_data ->Pos= error ; -// u->go_cmd.Pos = pos; + // 读取参数 float tff = u->go_cmd.T; // 前馈力矩 @@ -214,7 +205,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]); DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]); DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]); - DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] , + DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] , &u->pid .Dribble_translate_M3508_angle , &u->pid .Dribble_translate_M3508_speed , u->motor_target .Dribble_translate_M3508_angle ); @@ -230,8 +221,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) (PID_calc (&(u->pid .Pitch_M2006_angle ), u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle)) ); - - GO_SendData(u,u->motor_target .go_shoot,5 ); + + GO_SendData(u, + u->motor_target .go_shoot, + u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度 for(int i=0;i<4;i++){ @@ -249,75 +242,78 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) } -int a=0; -int b=0; - int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) { if(u ==NULL) return 0; + if(out ==NULL) return 0; + if(c ==NULL) return 0; + + /*指针简化*/ + PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; + DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig; + + up_motor_target_t *target=&u->motor_target ; static int is_pitch=1; - + /*config值限位*/ + abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f); + switch (c->CMD_CtrlType ) { case RCcontrol: //在手动模式下 - + switch (c-> CMD_mode ) { case Normal : /*投篮*/ if(is_pitch){ - u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; - u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; + target->go_shoot =pitch_cfg->go_init ; + target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle; is_pitch=0; } //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删 - u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; - u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 + target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ; + u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 /*运球*/ RELAY1_TOGGLE(0);//关闭气缸 for(int i=0;i<3;i++){ - u->motor_target.Dribble_M3508_speed[i]=0; + target->Dribble_M3508_speed[i]=0; } - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle; - u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态 + target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle; + u->DribbleCont .DribbleState = DRIBBLE_PREPARE; //重置最初状态 break; case Pitch : - if (u->PitchContext .PitchState ==PITCH_PREPARE) + if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE) { - u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 + u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 } Pitch_Process(u,out); -// if (u->PitchContext .PitchState ==PITCH_PREPARE) -// { -// u->CoContext .CoState =CoTRANSLATE_OUT; -// } -// Co_Process(u,out); + break ; case UP_Adjust: //测试用 - - u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ; - u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100; + pitch_cfg ->pull_speed=5; + pitch_cfg ->go_release_pos += c->Vx ; + pitch_cfg->Pitch_angle += c->Vy/100; - u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball; - u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; + target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos; + target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; break ; case Dribble: { - if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){ - u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE; + if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){ + u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN; } //光电状态更新 Dribble_Process(u,out); @@ -332,23 +328,28 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) switch(c-> CMD_mode) { case AUTO_MID360_Pitch: - u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos); + pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos); - if (u->PitchContext .PitchState ==PITCH_PREPARE) + if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE) { - u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 + u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 } /*根据距离实时计算所需pos*/ -// u->PitchContext .PitchConfig .go1_release_threshold=c->pos; + pitch_cfg ->go_release_pos=c->pos; +// if(u->CoContext .CoState == CoPITCH){ +// u->CoContext.CoState=CoPITCH_MID360; +// } +// Co_Process(u,out); + Pitch_Process(u,out); break ; case AUTO_MID360: - u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; - - u->PitchContext .PitchState = PITCH_PREPARE; + target->Shoot_M2006_angle =pitch_cfg ->m2006_init ; + u->Pitch_Cfg .PitchState = PITCH_PREPARE; + u->DribbleCont .DribbleState = DRIBBLE_PREPARE; break ; @@ -372,22 +373,24 @@ return 0; - - int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) { - - switch(u->PitchContext .PitchState){ + + PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig; + PitchState_t *state =&u->Pitch_Cfg .PitchState; + up_motor_target_t *target=&u->motor_target ; + + switch(*state){ case PITCH_START: -// u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 - - u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold; +// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 + cfg->pull_speed=10; + target->go_shoot = cfg->go_pull_pos; if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 - u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0 - - u->PitchContext .PitchState=PITCH_PULL_TRIGGER; + target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0 + u->Pitch_Cfg .PitchConfig .pull_speed=6; + *state=PITCH_PULL_TRIGGER; }//更改标志位 break ; @@ -396,16 +399,16 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 { - u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball; - if(is_reached (u->motorfeedback .go_data ->Pos ,u->motor_target .go_shoot ,1.0f)) + target->go_shoot=cfg->go_release_pos; + if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f)) { - u->PitchContext .PitchState=PITCH_LAUNCHING; + *state=PITCH_LAUNCHING; } } break ; case PITCH_LAUNCHING: //等待发射 - u->PitchContext .PitchState=PITCH_COMPLETE; +// *state=PITCH_COMPLETE; break ; case PITCH_COMPLETE: //发射完成 @@ -432,41 +435,43 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) static int common_speed_flag=0;//是否共速 static int common_speed_reverse_flag=0;//是否共速 + DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig; + up_motor_target_t *target=&u->motor_target ; + DribbleState_t *DribbleState=&u->DribbleCont.DribbleState; - switch (u->DribbleContext.DribbleState) { + switch (u->DribbleCont.DribbleState) { case DRIBBLE_TRANSLATE: - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动 if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 { - u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态 + u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态 } break; case DRIBBLE_PROCESS_DOWN: - u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed; - u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed; - u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed; + target->Dribble_M3508_speed[0]=DribbleCfg->spd; + target->Dribble_M3508_speed[1]=DribbleCfg->spd; + target->Dribble_M3508_speed[2]=DribbleCfg->spd; if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm, - u->motorfeedback .DJmotor_feedback [1].rpm, - u->motorfeedback .DJmotor_feedback [2].rpm, - u->motor_target.Dribble_M3508_speed [0], - u->motor_target.Dribble_M3508_speed [1], - u->motor_target.Dribble_M3508_speed[2], - 50.0f,50) + u->motorfeedback .DJmotor_feedback [1].rpm, + u->motorfeedback .DJmotor_feedback [2].rpm, + DribbleCfg->spd, + DribbleCfg->spd, + DribbleCfg->spd, 50.0f,50) ) { RELAY1_TOGGLE(1); //速度达到后打开气缸 common_speed_flag =1; } if(common_speed_flag){ - if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转 - u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; - u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; - u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; + if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转 + target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd; + target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd; + target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd; - u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP; + *DribbleState=DRIBBLE_PROCESS_UP; } @@ -485,21 +490,21 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) } if(common_speed_reverse_flag){ - if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){ - u->DribbleContext .DribbleState=DRIBBLE_DONE; - RELAY1_TOGGLE(0); //关闭气缸 + if(DribbleCfg->light_ball_flag == 1){ + *DribbleState=DRIBBLE_DONE; + RELAY1_TOGGLE(0); //关闭气缸 } } break ; case DRIBBLE_DONE: common_speed_reverse_flag=0; for(int i=0;i<3;i++){ - u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0 + target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0 } /*标志位清零*/ - u->DribbleContext.DribbleConfig.light_ball_flag=0; - u->DribbleContext.DribbleConfig.light_3508_flag=0; + DribbleCfg->light_ball_flag=0; + DribbleCfg->light_3508_flag=0; break; @@ -515,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 ) @@ -524,12 +532,12 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out) break ; case CoTRANSLATE_OUT: - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 - if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 + if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主 { - u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态 - u->PitchContext .PitchState = PITCH_START; + u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态 + u->Pitch_Cfg .PitchState = PITCH_START; u->CoContext .CoState =CoDRIBBLE; } @@ -537,17 +545,17 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out) case CoDRIBBLE: Dribble_Process(u,out); //状态停在DRIBBLE_DONE - Pitch_Process(u,out); //状态停在PITCH_PULL_TRIGGER + Pitch_Process(u,out); //状态停在PITCH_LAUNCHING //状态停在对应位置时,平移去给发射送球 - if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING) + if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING) { - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle; + u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang; } - if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,1.0f)) + if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f)) { u->motor_target.Dribble_M3508_speed[0]=1000; - u->motor_target.Dribble_M3508_speed[1]=-1000; - u->motor_target.Dribble_M3508_speed[2]=-1000; + u->motor_target.Dribble_M3508_speed[1]=1000; + u->motor_target.Dribble_M3508_speed[2]=1000; u->CoContext .CoState =CoTRANSLATE_IN; } @@ -565,26 +573,32 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out) ) { RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射 } - if(u->DribbleContext .DribbleConfig.light_ball_flag == 0) + if(u->DribbleCont .DribbleConfig.light_ball_flag == 0) { u->motor_target.Dribble_M3508_speed[0]=0; u->motor_target.Dribble_M3508_speed[1]=0; u->motor_target.Dribble_M3508_speed[2]=0; - u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 } - if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端 + if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端 { + u->CoContext .CoState =CoPITCH; + } break ; case CoPITCH: - - u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; + + + case CoPITCH_MID360: + u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init; + + break ; break ; } diff --git a/User/Module/up.h b/User/Module/up.h index d915ebf..b3265cc 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -27,13 +27,14 @@ */ -/*配合过程状态,co是合作的意思*/ +/*配合过程状态*/ typedef enum { CoPREPARE, // 准备阶段 CoTRANSLATE_OUT,//上方平移,去运球 CoDRIBBLE, //运球和蓄力阶段,再平移回去 CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 CoPITCH, //发射 + CoPITCH_MID360, //雷达发射 CoDONE, @@ -42,8 +43,8 @@ typedef enum { typedef struct { CoState_t CoState; - uint8_t is_initialized ; -} CoContext_t; + uint8_t is_init ; +} CoCon_t; @@ -61,30 +62,32 @@ typedef enum { /* 投球参数配置 */ typedef struct { - fp32 m2006_init_angle; // M2006初始角度-140 - fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机 - fp32 go1_init_position; // GO电机触发位置,0,初始位置 - fp32 go1_release_threshold; // go上升去合并扳机，扳机位置 - fp32 go1_Receive_ball; //用在配合过程，用来接平移后的球 - fp32 m2006_Screw_init; + fp32 m2006_init; // M2006初始角度 + fp32 m2006_trig; // M2006触发角度0,用来合并扳机 + fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置 + fp32 go_pull_pos; // go上升去合并扳机，扳机位置 fp32 Pitch_angle; -} PitchConfig_t; - + fp32 pull_speed;//go速度 + fp32 go_release_pos;//go松开，发射位置 + +} PitchCfg_t; +//fp32 go_pull_pos; +// fp32 m2006_init; // M2006初始角度 +// fp32 m2006_trig; // M2006触发角度 +// fp32 go_init; // GO初始位置 +// fp32 go_release; // GO释放阈值 +// fp32 go_recv; // GO接球位置 +// fp32 screw_init; // 螺杆初始值 +// fp32 pitch_ang; // 投球角度 +// fp32 pull_spd; // 拉动速度 /* 投球控制上下文 */ typedef struct { PitchState_t PitchState; - PitchConfig_t PitchConfig; - - uint8_t is_initialized ; -} PitchContext_t; - - - - - - + PitchCfg_t PitchConfig; + uint8_t is_init ; +} Pitch_Cfg_t; /*运球*/ typedef enum { @@ -100,27 +103,27 @@ typedef enum { typedef struct { int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到 - fp32 m3508_init_angle; // 平移前位置 + fp32 m3508_init_ang; // 3508平移前位置 fp32 m3508_translate_angle; // 平移后位置 - fp32 m3508_dribble_init_speed; - fp32 m3508_dribble_speed; // 转动速度 - fp32 m3508_dribble_Reverse_speed; + fp32 init_spd; + fp32 spd; // 转动速度 + fp32 rev_spd; /*光电标志位，初始值均为0，触发为1*/ int light_3508_flag;//3508平移处的光电，0初始，1到位置 int light_ball_flag;//检测球的flag -} DribbleConfig_t; +} DribbleCfg_t; /* 状态机上下文 */ typedef struct { DribbleState_t DribbleState; - DribbleConfig_t DribbleConfig; + DribbleCfg_t DribbleConfig; - uint8_t is_initialized; + uint8_t is_init; -} DribbleContext_t; +} DribbleCont_t; typedef struct { @@ -151,57 +154,12 @@ typedef struct pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环 - DribbleConfig_t DribbleConfig_Config; - PitchConfig_t PitchConfig_Config; + DribbleCfg_t DribbleCfg; + PitchCfg_t PitchCfg; GO_MotorCmd_t go_cmd; }UP_Param_t; - -typedef struct -{ - fp32 ecd; - fp32 rpm; - uint8_t id; - fp32 orig_angle; - fp32 last_angle; - int32_t round_cnt; - int init_cnt; - fp32 total_angle; -}DJmotor_feedback_t; - - typedef struct{ - - uint8_t up_task_run; - const UP_Param_t *param; - CoContext_t CoContext; - /*运球过程*/ - DribbleContext_t DribbleContext; - /*投篮过程*/ - PitchContext_t PitchContext; - - CMD_t *cmd; - - struct{ - - struct{ - fp32 VESC_5065_M1_rpm; - fp32 VESC_5065_M2_rpm; - }VESC; - - GO_MotorData_t *go_data;//存放go电机数据 - - DJmotor_feedback_t DJmotor_feedback[6]; - - struct { - uint32_t ecd; - float angle; - }Encoder; - }motorfeedback; - - - - struct{ fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M2_rpm; @@ -215,9 +173,9 @@ typedef struct{ fp32 Shoot_Pitch_angle; - }motor_target; - - struct{ + }up_motor_target_t; + +typedef struct{ pid_type_def VESC_5065_M1; @@ -237,11 +195,55 @@ typedef struct{ pid_type_def Dribble_translate_M3508_angle;//平移用的角度环 - }pid; - GO_MotorCmd_t go_cmd; + }up_pid_t; +typedef struct +{ + fp32 ecd; + fp32 rpm; + uint8_t id; + fp32 orig_angle; + fp32 last_angle; + int32_t round_cnt; + int init_cnt; + fp32 total_angle; +}DJmotor_feedback_t; + +typedef struct{ + + uint8_t up_task_run; + const UP_Param_t *param; + CoCon_t CoContext; + /*运球过程*/ + DribbleCont_t DribbleCont; + /*投篮过程*/ + Pitch_Cfg_t Pitch_Cfg; + + CMD_t *cmd; + + struct{ + struct{ + fp32 VESC_5065_M1_rpm; + fp32 VESC_5065_M2_rpm; + }VESC; + GO_MotorData_t *go_data;//存放go电机数据 + DJmotor_feedback_t DJmotor_feedback[6]; + + struct { + uint32_t ecd; + float angle; + }Encoder; + }motorfeedback; + + + + + up_pid_t pid; + up_motor_target_t motor_target; + + GO_MotorCmd_t go_cmd; /*经PID计算后的实际发送给电机的实时输出值*/ struct @@ -257,8 +259,6 @@ typedef struct{ fp32 vofa_send[8]; - - } UP_t; diff --git a/User/device/can_use.c b/User/device/can_use.c index 4010af2..eb86332 100644 --- a/User/device/can_use.c +++ b/User/device/can_use.c @@ -1,3 +1,4 @@ + /* CAN总线上的设备1到7 将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制 @@ -32,7 +33,7 @@ #define CAN_M3508_MAX_ABS_CUR (20) #define CAN_M2006_MAX_ABS_CUR (10) -#define CAN_ENCODER_RESOLUTION (32768) //欧艾迪编码器分辨率 +#define CAN_ENCODER_RESOLUTION (4096) //欧艾迪编码器分辨率 CAN_RawRx_t raw_rx1;//原始的can数据 CAN_RawRx_t raw_rx2; @@ -56,31 +57,20 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback, feedback->torque_current = raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; } -static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id, +static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, const uint8_t *raw) { if (feedback == NULL || raw == NULL) return; - switch (id) - { - case CAN_SICK_ID_1: - feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]); + static uint16_t sick_rec[4]; - break ; - - case CAN_SICK_ID_2: - feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]); - - break ; - - case CAN_SICK_ID_3: - feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]); - - break ; - - case CAN_SICK_ID_4: - feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]); - - break ; - } + sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]); + sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]); + sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]); + sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]); + + feedback ->raw_dis [0]= sick_rec[0]; + feedback ->raw_dis [1]= sick_rec[1]; + feedback ->raw_dis [2]= sick_rec[2]; + feedback ->raw_dis [3]= sick_rec[3]; } void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, const uint8_t *raw) { @@ -93,7 +83,8 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, { case 0x01: feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24; - feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f; + feedback->angle=(fp32)(2048-(fp32)feedback->ecd)/(2048-1797)*(70.0f-48.0f)+48.0f; + break; } } @@ -383,19 +374,8 @@ 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_1: - CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data); - break; - case CAN_SICK_ID_2: - CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data); - break; - case CAN_SICK_ID_3: - CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data); - break; - case CAN_SICK_ID_4: - CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data); - - break; + + default: break; } @@ -425,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; diff --git a/User/device/can_use.h b/User/device/can_use.h index 066ded2..9339e47 100644 --- a/User/device/can_use.h +++ b/User/device/can_use.h @@ -18,10 +18,8 @@ typedef enum { CAN_UP_M3508_M5_ID = 0x205, /* 1 */ CAN_UP_M3508_M6_ID = 0x206, /* 2 */ - CAN_SICK_ID_1=0x301, - CAN_SICK_ID_2=0x302, - CAN_SICK_ID_3=0x303, - CAN_SICK_ID_4=0x304, + CAN_SICK_ID=0x305, + CAN_Encoder_ID=0x01, @@ -200,6 +198,6 @@ void CAN_Encoder_Control(CAN_t *can); void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, const uint8_t *raw) ; -static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id, +static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, const uint8_t *raw) ; #endif diff --git a/User/device/cmd.c b/User/device/cmd.c index 2d914c1..c7b9a10 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -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 =Dribble; //左上，右上，手动调整 + 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_Pitch; //左中，右中，雷达发射 + 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 =AUTO_MID360; //左中，右中，雷达 + if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =AUTO_MID360; //左中，右中，无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) 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) @@ -205,8 +207,8 @@ 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/device.h b/User/device/device.h index 86a547a..897ba4f 100644 --- a/User/device/device.h +++ b/User/device/device.h @@ -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) diff --git a/User/device/nuc.c b/User/device/nuc.c index 49435ba..a1c723f 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -6,7 +6,8 @@ static volatile uint32_t drop_message = 0; static osThreadId_t thread_alert; -uint8_t nucbuf[31]; + uint8_t nucbuf[31]; +static char SendBuffer[19]; static void NUC_IdleCallback(void) { @@ -28,11 +29,8 @@ int8_t NUC_StartReceiving() { return DEVICE_OK; return DEVICE_ERR; } - char SendBuffer[19]; - int8_t NUC_StartSending(fp32 *data) { - union { float x[4]; @@ -64,7 +62,7 @@ int8_t NUC_Restart(void) { return DEVICE_OK; } bool_t NUC_WaitDmaCplt(void) { - return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) == + return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) == SIGNAL_NUC_RAW_REDY); } diff --git a/User/device/rc.c b/User/device/rc.c index fd23ae8..84c5658 100644 --- a/User/device/rc.c +++ b/User/device/rc.c @@ -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) { @@ -224,7 +224,7 @@ int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) { 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; diff --git a/User/device/rc.h b/User/device/rc.h index e21aaa5..86adf4b 100644 --- a/User/device/rc.h +++ b/User/device/rc.h @@ -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); 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 diff --git a/User/task/can_task.c b/User/task/can_task.c index fcfc4ca..dabf5a4 100644 --- a/User/task/can_task.c +++ b/User/task/can_task.c @@ -47,7 +47,6 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN; //一问一答sick数据指令 -// CAN_Sick_Control(&can); CAN_Encoder_Control(&can); /*can设备数据存入队列*/ diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c index 28549a0..58d3938 100644 --- a/User/task/chassis_task.c +++ b/User/task/chassis_task.c @@ -56,6 +56,8 @@ void Task_Chassis(void *argument) { #ifdef DEBUG task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId()); + task_runtime.freq.chassis=TASK_FREQ_CHASSIS; + task_runtime.last_up_time.chassis=tick; #endif /*imu数据获取*/ @@ -85,13 +87,14 @@ 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); //发送数据 vofa_send[0] = chassis.vofa_send[0]; - vofa_send[1] = chassis.vofa_send[1]; + vofa_send[1] = chassis.vofa_send[1]; vofa_send[2] = chassis.vofa_send[2]; vofa_send[3] = chassis.vofa_send[3]; vofa_send[4] = chassis.vofa_send[4]; - vofa_send[5] = chassis.vofa_send[5]; + vofa_send[5] = chassis.vofa_send[5]; vofa_send[6] = chassis.vofa_send[6]; vofa_send[7] = chassis.vofa_send[7]; diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c index a1f64de..8bf0203 100644 --- a/User/task/cmd_task.c +++ b/User/task/cmd_task.c @@ -30,8 +30,9 @@ void Task_cmd(void *argument){ while(1){ #ifdef DEBUG /*记录任务所需要的栈空间*/ - task_runtime.stack_water_mark.cmd = - osThreadGetStackSpace(osThreadGetId()); + task_runtime.stack_water_mark.cmd =osThreadGetStackSpace(osThreadGetId()); + task_runtime.last_up_time.cmd=tick; + task_runtime.freq.cmd=TASK_FREQ_CTRL_CMD; #endif osKernelLock(); /*锁住RTOS内核调度*/ diff --git a/User/task/init.c b/User/task/init.c index 4f20970..9b3e7af 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -85,7 +85,9 @@ void Task_Init(void *argument) { task_runtime.msgq.cmd.raw.nuc = osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL); - + task_runtime.msgq.cmd.raw.nuc_send = + osMessageQueueNew(3u,sizeof(NUC_send_t), NULL); + osKernelUnlock(); osThreadTerminate(osThreadGetId()); /* 结束自身 */ } diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 8befda0..94de5f9 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -5,32 +5,38 @@ NUC_t nuc_raw; CMD_NUC_t cmd_fromnuc; + +NUC_send_t NUC_send; #else static NUC_t nuc_raw; static CMD_NUC_t cmd_fromnuc; - +NUC_send_t NUC_send; #endif fp32 send[4]={11.0f,45.0,1.f,4.0f}; + void Task_nuc(void *argument){ (void)argument; /**/ // osDelay(TASK_INIT_DELAY_NUC); - const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC; NUC_Init(&nuc_raw); uint32_t tick = osKernelGetTickCount(); - + while (1) { #ifdef DEBUG task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); + task_runtime .freq.nuc = TASK_FREQ_NUC; + task_runtime.last_up_time.nuc= tick; + #endif NUC_StartReceiving(); -// NUC_StartSending(NUC_send.send); - if (NUC_WaitDmaCplt()){ + NUC_StartSending(NUC_send.send); +// NUC_StartSending (send); + if (NUC_WaitDmaCplt()){ NUC_RawParse(&cmd_fromnuc); } else{ @@ -38,7 +44,8 @@ void Task_nuc(void *argument){ } osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0); - + osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); + tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ osDelayUntil(tick); diff --git a/User/task/rc_task.c b/User/task/rc_task.c index b34b109..15586ef 100644 --- a/User/task/rc_task.c +++ b/User/task/rc_task.c @@ -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); + } } diff --git a/User/task/user_task.h b/User/task/user_task.h index 928f2ef..81c836e 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -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) @@ -58,17 +57,15 @@ typedef struct { struct { struct { osMessageQueueId_t rc; - osMessageQueueId_t nuc; + osMessageQueueId_t nuc; // osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/ - + osMessageQueueId_t nuc_send; //给nuc发 }raw; /*控制分离*/ osMessageQueueId_t UP_cmd_ctrl_t; osMessageQueueId_t CHASSIS_cmd_ctrl_t; - - osMessageQueueId_t status; } cmd;