diff --git a/Core/Src/main.c b/Core/Src/main.c index 244ccf3..39a953b 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -32,7 +32,6 @@ /* USER CODE BEGIN Includes */ #include "bsp_delay.h" #include "bsp_can.h" -#include "Action.h" //#include "bsp_dwt.h" /* USER CODE END Includes */ diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index de52ba5..429293f 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -475,3 +475,5 @@ Log at : 2025/2/23|14:39:36|GMT+0800 [info] Log at : 2025/3/11|21:09:29|GMT+0800 +[info] Log at : 2025/3/28|21:47:12|GMT+0800 + diff --git a/MDK-ARM/AUTO_CHASSIS.uvoptx b/MDK-ARM/AUTO_CHASSIS.uvoptx index a88a95b..5fb0e6c 100644 --- a/MDK-ARM/AUTO_CHASSIS.uvoptx +++ b/MDK-ARM/AUTO_CHASSIS.uvoptx @@ -145,7 +145,7 @@ 0 DLGUARM - + (105=-1,-1,-1,-1,0) 0 @@ -195,6 +195,46 @@ 1 GO_motor_info + + 8 + 1 + cbuf[36] + + + 9 + 1 + dr16,0x0A + + + 10 + 1 + cmd,0x0A + + + 11 + 1 + rc_ctrl,0x0A + + + 12 + 1 + up_cmd,0x0A + + + 13 + 1 + angle + + + 14 + 1 + can_ZZ + + + 15 + 1 + flaggg,0x0A + @@ -1116,18 +1156,6 @@ 0 0 0 - ..\User\task\nuc_task.c - nuc_task.c - 0 - 0 - - - 8 - 68 - 1 - 0 - 0 - 0 ..\User\task\error_detect_task.c error_detect_task.c 0 @@ -1135,7 +1163,7 @@ 8 - 69 + 68 1 0 0 @@ -1147,7 +1175,7 @@ 8 - 70 + 69 1 0 0 @@ -1157,6 +1185,30 @@ 0 0 + + 8 + 70 + 1 + 0 + 0 + 0 + ..\User\task\cmd_task.c + cmd_task.c + 0 + 0 + + + 8 + 71 + 1 + 0 + 0 + 0 + ..\User\task\nuc_task.c + nuc_task.c + 0 + 0 + @@ -1167,7 +1219,7 @@ 0 9 - 71 + 72 1 0 0 @@ -1177,18 +1229,6 @@ 0 0 - - 9 - 72 - 1 - 0 - 0 - 0 - ..\User\task\cmd_task.c - cmd_task.c - 0 - 0 - 9 73 @@ -1249,18 +1289,6 @@ 0 0 - - 9 - 78 - 1 - 0 - 0 - 0 - ..\User\Algorithm\navi.c - navi.c - 0 - 0 - @@ -1271,7 +1299,7 @@ 0 10 - 79 + 78 1 0 0 @@ -1283,7 +1311,7 @@ 10 - 80 + 79 1 0 0 @@ -1295,7 +1323,7 @@ 10 - 81 + 80 1 0 0 @@ -1307,7 +1335,7 @@ 10 - 82 + 81 1 0 0 @@ -1319,7 +1347,7 @@ 10 - 83 + 82 1 0 0 @@ -1331,7 +1359,7 @@ 10 - 84 + 83 1 0 0 @@ -1343,7 +1371,7 @@ 10 - 85 + 84 1 0 0 @@ -1363,7 +1391,7 @@ 0 11 - 86 + 85 1 0 0 @@ -1375,7 +1403,7 @@ 11 - 87 + 86 1 0 0 @@ -1387,7 +1415,7 @@ 11 - 88 + 87 1 0 0 @@ -1407,7 +1435,7 @@ 0 12 - 89 + 88 1 0 0 @@ -1427,7 +1455,7 @@ 0 13 - 90 + 89 1 0 0 @@ -1439,7 +1467,7 @@ 13 - 91 + 90 1 0 0 @@ -1451,7 +1479,7 @@ 13 - 92 + 91 1 0 0 @@ -1463,7 +1491,7 @@ 13 - 93 + 92 1 0 0 diff --git a/MDK-ARM/AUTO_CHASSIS.uvprojx b/MDK-ARM/AUTO_CHASSIS.uvprojx index eee0eb5..8e40444 100644 --- a/MDK-ARM/AUTO_CHASSIS.uvprojx +++ b/MDK-ARM/AUTO_CHASSIS.uvprojx @@ -1208,11 +1208,6 @@ 1 ..\User\task\can_task.c - - nuc_task.c - 1 - ..\User\task\nuc_task.c - error_detect_task.c 1 @@ -1228,6 +1223,67 @@ 1 ..\User\task\up_task.c + + cmd_task.c + 1 + ..\User\task\cmd_task.c + + + 2 + 0 + 0 + 0 + 0 + 1 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + nuc_task.c + 1 + ..\User\task\nuc_task.c + @@ -1238,11 +1294,6 @@ 1 ..\User\Algorithm\filter.c - - cmd_task.c - 1 - ..\User\task\cmd_task.c - ahrs.c 1 @@ -1268,11 +1319,6 @@ 1 ..\User\Algorithm\pid.c - - navi.c - 1 - ..\User\Algorithm\navi.c - diff --git a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf index 5253c10..669a09d 100644 Binary files a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf and b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf differ diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c deleted file mode 100644 index 5f738e6..0000000 --- a/User/Module/Chassis.c +++ /dev/null @@ -1,231 +0,0 @@ -#include "Chassis.h" -#include "gpio.h" -#include "Action.h" -#include "user_math.h" -#include "bsp_buzzer.h" -#include "bsp_delay.h" - - - - - - -/*机器人坐标系，向前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->C_cmd.type== c->ctrl && ctrl->C_cmd.mode== c->mode) return CHASSIS_OK; /*模式未改变直接返回*/ - //此处源代码处做了pid的reset 待添加 - c->ctrl =ctrl->C_cmd.type; - c->mode =ctrl->C_cmd.mode; - - return CHASSIS_OK; -} //设置控制模式 - - -/*该函数用来更新can任务获得的电机反馈值*/ - -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.motor3508.as_array[i].rotor_speed; - c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current; - } - - - c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_ecd; - c->motorfeedback.rotor_pit6020rpm = can->motor.pit6020.as_array[0].rotor_speed; - - c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_ecd; - c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed; - - c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_ecd; - c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed; - - c->sick_dis[0] = can->sickfed.raw_dis[0]; - c->sick_dis[1] = can->sickfed.raw_dis[1]; - c->sick_dis[2] = can->sickfed.raw_dis[2]; - c->sick_dis[3] = can->sickfed.raw_dis[3]; - - 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_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度 - - PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_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_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)); - - 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; -} - - -void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算 -{ - c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前 - c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后 - c->hopemotorout.OmniSpeedOut[1] = Vx-Vy+Vw;//左后 - c->hopemotorout.OmniSpeedOut[2] = Vx+Vy+Vw;//左前 -} - - -//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake) -//{ -// uint16_t xArrive = 0, yArrive = 0; -// xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0; -// yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0; -// if(xArrive && yArrive) return true; -// else return false; -//} - - -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->ctrl) - { - case RC: - -/* - 在cmd里对数据进行处理 包括方向和油门 - 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/ - c->move_vec.Vw = ctrl->Vw*6000; - c->move_vec.Vx = -ctrl->Vy*6000; - c->move_vec.Vy = ctrl->Vx*6000; - if(c->mode == RC_MODE1 ){ - } - - - break; - - case MID_NAVI: -// //这套是全向轮的方向,一定要注意这里的xy方向 - c->move_vec.Vw =ctrl->C_navi.wz ; - c->move_vec.Vx =ctrl->C_navi.vy ; - c->move_vec.Vy =ctrl->C_navi.vx ; - - c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw); - c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy); - c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx); - - c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw); - c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx); - c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy); - - if(ctrl->status[5] ==1) - { - c->move_vec.Vw = c->move_vec.Vw * 0.8f; - c->move_vec.Vx = c->move_vec.Vx * 0.5f; - c->move_vec.Vy = c->move_vec.Vy * 0.5f; - } - - break; - - } -/*怎么用*/ - switch (c->mode) - { - case RELAXED: - c->move_vec.Vx =0; - c->move_vec.Vy =0; - c->move_vec.Vw =0; - break; - case NORMAL: - break; - - case GYRO_STAY://陀螺仪yaw修正 - c->move_vec.Vw = c->move_vec.Vw +c->pos088.bmi088.gyro.z *2000; - break; - -} - - //电机速度限幅 - -// abs_limit_fp(&c->move_vec.Vx,2000.0f); - -// abs_limit_fp(&c->move_vec.Vy,2000.0f); - -// abs_limit_fp(&c->move_vec.Vw,2000.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->motor3508.as_array[i] = c->final_out.final_3508out[i]; - } - - -// c->vofa_send[0]=c->pos088.bmi088.gyro.x; -// c->vofa_send[1]=c->pos088.bmi088.gyro.y; -// c->vofa_send[2]=c->pos088.bmi088.gyro.z; -// c->vofa_send[3]=c->pos088.bmi088.accl.x; -// c->vofa_send[4]=c->pos088.bmi088.accl.y; -// c->vofa_send[5]=c->pos088.bmi088.accl.z; - - return CHASSIS_OK; - -} - - - - diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h deleted file mode 100644 index d3f15ad..0000000 --- a/User/Module/Chassis.h +++ /dev/null @@ -1,232 +0,0 @@ -#ifndef CHASSIS_H - -/** - * @brief 底盘控制类头文件 - * - * 本头文件定义了底盘控制类的接口和数据结构，用于控制车辆的底盘。 - * - * @param ChassisImu_t 底盘的 IMU 数据结构体 - * @param ops_t 操作数据结构体 - * @param Chassis_t 底盘数据结构体 - * - * @brief 底盘控制类的功能包括： - * 速度控制 - * 方向控制 - * PID 控制 - * 速度和方向的综合控制 - * - * @attention PID 控制器的配置需要在 config 文件中设置 - * - */ - - -#define CHASSIS_H - -#include "struct_typedef.h" -#include "pid.h" -#include "bmi088.h" -#include "map.h" -#include "user_math.h" -#include "ahrs.h" -#include "can_use.h" -#include "cmd.h" -#include "filter.h" -#include "Action.h" - -#define CHASSIS_OK (0) -#define CHASSIS_ERR (-1) -#define CHASSIS_ERR_NULL (-2) -#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */ -#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */ - -//m3508的电机转速转换为底盘的实际速度 -#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f - - -#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f -#define PI 3.1415926535f - -typedef struct { - - BMI088_t bmi088; - - /*可通过该枚举类型来决定Imu的数据量纲*/ - enum { - IMU_DEGREE,//角度值（0-360） - IMU_RADIAN//弧度制（0-2pi) - }angle_mode; - - AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制） -}ChassisImu_t; - -/*底盘的类型*/ -typedef enum { - CHASSIS_TYPE_MECANUM, /* 麦轮 */ - CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/ - CHASSIS_TYPE_AGV, /* AGV舵轮 */ -} Chassis_Type_e; - -/*底盘的电机轮组*/ -typedef enum { - DJI_M3508, - DJI_G6020, - AGV_Group, -}Chassis_Motortype_e; - - -/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */ -typedef struct -{ - Chassis_Type_e chassis_type; /* */ - Chassis_Motortype_e motor_type; /**/ - - - /*该部分决定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 NaviVx_param; - pid_param_t NaviVy_param; - pid_param_t NaviVw_param; - pid_param_t Sick_CaliYparam; - pid_param_t Sick_CaliXparam; - - - -}Chassis_Param_t; - - -/*该结构体用于底盘的期望运动向量*/ -typedef struct -{ - fp32 Vx; - fp32 Vy; - fp32 Vw; - fp32 mul;//油门倍率 -}ChassisMove_Vec; - - -/** - * @brief - * - */ -typedef struct{ - - uint8_t chassis_task_run; //线程的运行 - - const Chassis_Param_t *param; //一些固定的参数 - - ChassisImu_t pos088; //088的实时姿态 - Action_POS_t Action_pos; - CMD_Chassis_CtrlType_e ctrl; - CMD_Chassis_mode_e mode; - - ChassisMove_Vec move_vec; //由控制任务决定 - - struct{ - - fp32 rotor_rpm3508[4]; - fp32 rotor_current3508[4]; - - fp32 rotor_pit6020angle; - fp32 rotor_pit6020rpm; - - fp32 rotor_gimbal_yawangle; - fp32 rotor_gimbal_yawrpm; - - fp32 rotor_gimbal_pitchangle; - fp32 rotor_gimbal_pitchrpm; - - }motorfeedback; - - - /*期望的底盘输出值*/ - struct{ - fp32 OmniSpeedOut[4]; - - }hopemotorout; - - - /*经PID计算后的实际发送给电机的实时输出值*/ - struct - { - fp32 final_3508out[4]; - fp32 final_pitchout; - fp32 final_gimbal_yawout; - fp32 final_gimbal_pitchout; - - }final_out; - - 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 sick_CaliforYPID; - pid_type_def sick_CaliforXPID; - - pid_type_def Action_VxPID; - pid_type_def Action_VyPID; - pid_type_def Action_WzPID; - }pid; - - fp32 vofa_send[8]; - - LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ - - - int32_t sick_dis[4]; //获取到的sick激光值 - - - -}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); - - - - - - -/// @brief -/// @param c -void vesc_current_detection(Chassis_t *c); -#endif diff --git a/User/Module/config.h b/User/Module/config.h index dd2b1aa..324fd11 100644 --- a/User/Module/config.h +++ b/User/Module/config.h @@ -1,14 +1,12 @@ #ifndef _CONFIG_H #define _CONFIG_H -#include "Chassis.h" #include "can_use.h" #include "ahrs.h" #include "map.h" #include "up.h" typedef struct{ -Chassis_Param_t chassis; /**/ UP_Param_t up; CAN_Params_t can; AHRS_Eulr_t mech_zero[4]; diff --git a/User/Module/up.c b/User/Module/up.c index 70c51ce..9c35bbd 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -4,102 +4,158 @@ #include "bsp_buzzer.h" #include "bsp_delay.h" -#define GEAR_RATIO (36) // 2006减速比 +#define GEAR_RATIO_2006 (36) // 2006减速比 +#define GEAR_RATIO_3508 (19) + #define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率 -#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO)) //2006编码值转轴角度 +#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度 +#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度 + + + + + int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) { - u->param = param; /*初始化参数 */ - + u->param = param; /*初始化参数 */ + /*go电机初始化*/ GO_M8010_init(); + /*pid初始化*/ PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param )); PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param )); PID_init (&u->pid .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param )); PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param )); + PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param )); + PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param )); + PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param )); PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param )); - - u->M2006 .motor =M2006 ; - u->M3508 .motor =M3508 ; - - for(int i=0;i<3;i++){ - PID_init (&u->pid .M3508_speed[i] ,PID_POSITION ,&(u->param ->M3508_speed_param )); - } - - for(int i=0;i<2;i++){ //go初始位置设置为0 - u->GO_motor_info[i] = getGoPoint(i); + + + for(int i=0;i<2;i++){ //go初始位置设置为0 GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W ); } + + /**/ + + u->state .Dribble_flag =Not_started_dri; + u->state. Pitch_flag=Not_started_Pit; + u->state .last_state = Not_started_dri; + } - -int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) { - - u->motorfeedback .M2006_angle=can ->motor .motor3508 .as_array [0].rotor_ecd ; - u->motorfeedback .M2006_rpm =can ->motor .motor3508 . as_array [0].rotor_speed ; +/*can，上层状态更新*/ +int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { + for(int i=0;i<2;i++){ //go初始位置设置为0 + u->motorfeedback .GO_motor_info [i] = getGoPoint(i); + } + u->motorfeedback .M2006 .motor =M2006; + u->motorfeedback .M3508 .motor =M3508; + u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ; u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ; u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ; u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed ; - for(int i=0;i<3;i++){ - u->motorfeedback .M3508_speed[i] =can ->motor .motor3508 .as_array [i+1].rotor_speed ; + + for(int i=0;i<4;i++){ + u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ; + u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ; } + + u->cmd =c; + + return 0; } - int8_t cnt=0; -/*上层电机控制*/ -int8_t UP_angle(UP_t *u, fp32 target_angle) { + +/*上层电机控制,使用can1的id1和2*/ +int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) { // 获取当前编码器角度 - -// switch (u->) - float angle = u->motorfeedback.M2006_angle; - // 初始化阶段：前50次循环记录初始值 - if (u->M2006.init_cnt < 50) { - u->M2006.orig_angle = angle; // 记录初始编码器值 - u->M2006.last_angle = angle; + int8_t cnt=0; + fp32 angle ,delta; + + switch(motor) + { + case M2006 : + angle = u->motorfeedback.M3508_angle[0]; + if (u->motorfeedback .M2006 .init_cnt < 50) { + u->motorfeedback .M2006.orig_angle = angle; // 记录初始编码器值 + u->motorfeedback .M2006.last_angle = angle; - u->M2006.init_cnt++; // 初始化计数器递增 + u->motorfeedback .M2006.init_cnt++; // 初始化计数器递增 return 0; } - - - float delta = angle - u->M2006.last_angle; + + delta = angle - u->motorfeedback .M2006.last_angle; + delta = angle - u->motorfeedback .M2006.last_angle; if (delta > 4096) { - u->M2006.round_cnt--; // 逆时针跨圈 + u->motorfeedback .M2006.round_cnt--; // 逆时针跨圈 } else if (delta < -4096) { - u->M2006.round_cnt++; // 顺时针跨圈 + u->motorfeedback .M2006.round_cnt++; // 顺时针跨圈 } - - u->M2006.last_angle = angle; - - // 计算总角度 - float total_angle = (u->M2006.round_cnt * 8191 + (angle - u->M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE; - u->M2006 .total_angle =total_angle; + u->motorfeedback.M2006.last_angle = angle; + // 计算总角度 + float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE; + u->motorfeedback.M2006.total_angle =total_angle; float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle); - float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M2006_rpm, delta_angle); + float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle); u->motor_target.M2006_angle = target_angle; - u->final_out .final_2006out =delta_speed; + u->final_out .final_3508out [0] =delta_speed; + + break ; + + case M3508 : + + angle = u->motorfeedback.M3508_angle[1]; + if (u->motorfeedback .M3508 .init_cnt < 50) { + u->motorfeedback .M3508.orig_angle = angle; // 记录初始编码器值 + u->motorfeedback .M3508.last_angle = angle; + + u->motorfeedback .M3508.init_cnt++; // 初始化计数器递增 + return 0; + } + + delta = angle - u->motorfeedback .M3508.last_angle; + delta = angle - u->motorfeedback .M3508.last_angle; + if (delta > 4096) { + u->motorfeedback .M3508.round_cnt--; // 逆时针跨圈 + } else if (delta < -4096) { + u->motorfeedback .M3508.round_cnt++; // 顺时针跨圈 + } + u->motorfeedback.M3508.last_angle = angle; + // 计算总角度 + total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE; + u->motorfeedback.M3508.total_angle =total_angle; + delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle); + delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle); + + u->motor_target.M3508_angle = target_angle; + u->final_out .final_3508out[1] =delta_speed; + + break ; + + } return 0; } -int8_t UP_M3508_speed(UP_t *u,fp32 speed) -{ - u->motor_target .M3508_speed =speed; - for(int i=0;i<3;i++){ - u->final_out .final_3508out [i] = - PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed ); - } - -} +//int8_t UP_M3508_speed(UP_t *u,fp32 speed) +//{ +// u->motor_target .M3508_speed [] =speed; +// for(int i=0;i<3;i++){ +// u->final_out .final_3508out [i] = +// PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed ); +// } +// +//} int8_t VESC_M5065_Control(UP_t *u,fp32 speed) @@ -107,16 +163,17 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed) u->motor_target .VESC_5065_M1_rpm =speed; u->motor_target .VESC_5065_M2_rpm =speed; - u->final_out .final_VESC_5065_M1out =u->motor_target .VESC_5065_M1_rpm; - u->final_out .final_VESC_5065_M2out =-u->motor_target .VESC_5065_M2_rpm; + u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm; + u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm; } + int8_t GM6020_control(UP_t *u,fp32 angle) { fp32 delat_speed; - Clip(&angle,90,270); +// Clip(&angle,90,270); delat_speed = PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191)); @@ -124,6 +181,7 @@ int8_t GM6020_control(UP_t *u,fp32 angle) PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed); u->motor_target .rotor_pit6020angle =angle ; } + /*go电机控制*/ int8_t GO_SendData(UP_t *u,int id,float pos) { @@ -133,35 +191,148 @@ int8_t GO_SendData(UP_t *u,int id,float pos) } + int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) { //电机控制 ，传进can - out ->motor3508 .as_array [0]=u->final_out .final_2006out ; - for(int i=1;i<4;i++){ - out ->motor3508 .as_array[i]=u->final_out.final_3508out [i-1] ; + UP_angle_control(u,u->motor_target .M2006_angle ,M2006); + UP_angle_control(u,u->motor_target .M3508_angle ,M3508 ); + + + + GO_SendData(u,0 ,u->motor_target .go_shoot ); + GO_SendData(u,1 ,u->motor_target .go_spin); + + + for(int i=0;i<4;i++){ + out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ; } - out ->chassis5065 .erpm [0]=u->final_out .final_VESC_5065_M1out ; - out ->chassis5065 .erpm [1]=u->final_out .final_VESC_5065_M2out ; - + out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ; + out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ; out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ; + + } -int8_t UP_control(UP_t *u,CAN_Output_t *out) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) { -// if(u ==NULL) return 0; -// if(u ==NULL) return 0; -// UP_M2006_angle(u,2500); - switch (u->ctrl) + + if(u ==NULL) return 0; + if(u ==NULL) return 0; + + switch (c->CMD_CtrlType ) { - case STATE_IDLE : break ; - case STATE_PRE_DRIBBLE : break ; - case STATE_POST_DRIBBLE : break ; - case STATE_PRE_LAUNCH : break ; - case STATE_POST_LAUNCH : break ; + case UP_RCcontrol: //在手动模式下 + + switch (c->CMD_UP_mode ) + { + + case Normal : + + u->state .Pitch_flag =Not_started_Pit; + u->state .last_state = Not_started_Pit; + + u->motor_target .go_shoot =0; + u->motor_target .M2006_angle =-140; + + break; + + case Pitch_pull : + if(u->state .last_state == Not_started_Pit) + { + + u->motor_target .go_shoot =-300; + u->motor_target .M2006_angle =-140; + + if(u->motorfeedback .GO_motor_info[0]->Pos < (-4.8)) //到达位置后再扣扳机 + { + u->motor_target .M2006_angle =0; + if(u->motorfeedback .M2006.total_angle>-5) + {//避免没勾上就拉 + u->motor_target .go_shoot =0; + u->state .Pitch_flag = Launch_Ready ; + u->state .last_state = Launch_Ready; + + } + + } + } + + break ; + case Pitch_shoot : + if (u->state .last_state == Launch_Ready) + { + + u->motor_target .M2006_angle =-140; + u->state .Pitch_flag = Done_Pit ; + u->state .last_state = Done_Pit; + + } + { + + } + break; + + + + } + break; + + case MID_NAVI : + { + } + break ; + + case PICK_t : + { + } + + + break; + + default: + break; } + + + + + + } + + diff --git a/User/Module/up.h b/User/Module/up.h index afc21a6..6630a7b 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -5,26 +5,47 @@ #include "struct_typedef.h" #include "pid.h" #include "bmi088.h" -#include "map.h" #include "user_math.h" #include "ahrs.h" #include "can_use.h" #include "cmd.h" #include "filter.h" -#include "Action.h" -#include "chassis.h" #include "vofa.h" #include "GO_M8010_6_Driver.h" - -typedef enum { - STATE_IDLE, // 空闲状态 - STATE_PRE_DRIBBLE, // 运球前 - STATE_POST_DRIBBLE, // 运球后 - STATE_PRE_LAUNCH, // 发射前 - STATE_POST_LAUNCH // 发射后 -} OperationState; +#include "bsp_usart.h" + +typedef enum +{ + Not_started_Pit=0,//未开始 + Launch_Ready=1, //准备发射 + Launch_complete=2,//发射完成 + Done_Pit=3 //已完成 + +}Pitch_flag_t; + +typedef enum{ + + Not_started_dri=0,//未开始 + No_ball =1, //抓上无球 + Have_ball_F=2, //刚开始有球 + Have_ball_S=3, //中途有球 + Done_dri =4 //已完成 + +}Dribble_flag_t; + +/*运行控制中的控制*/ +typedef struct{ + + /*投球过程*/ + Pitch_flag_t Pitch_flag; + /*运球过程*/ + Dribble_flag_t Dribble_flag; + + int last_state; + +} Oper_control_state_t; typedef struct { @@ -52,17 +73,18 @@ typedef struct { float K_W; }GO_param_t; - +/*角度环控制电机类型*/ typedef enum { M2006 = 1, M3508 + } MotorType_t; typedef struct { - /*该部分决定PID的参数整定在config中修改*/ + pid_param_t VESC_5065_M1_param; pid_param_t VESC_5065_M2_param; @@ -73,6 +95,8 @@ typedef struct pid_param_t M2006_angle_param; pid_param_t M3508_speed_param; + pid_param_t M3508_angle_param; + GO_param_t go_param; }UP_Param_t; @@ -80,6 +104,7 @@ typedef struct typedef struct { MotorType_t motor; + float orig_angle; float last_angle; int32_t round_cnt; @@ -93,26 +118,30 @@ typedef struct{ uint8_t up_task_run; const UP_Param_t *param; - UP_Imu_t pos088; - OperationState ctrl; - motor_angle_data M2006; - motor_angle_data M3508; - GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据 - - struct{ - + UP_Imu_t pos088; + + /*控制及状态*/ + CMD_t *cmd; + Oper_control_state_t state;//上层机构的运行状态 + + struct{ fp32 rotor_pit6020ecd; fp32 rotor_pit6020rpm; fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M2_rpm; - fp32 M2006_rpm; - fp32 M2006_angle; + motor_angle_data M2006; + motor_angle_data M3508; - fp32 M3508_speed[3]; + GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据 + fp32 M3508_angle[4]; + fp32 M3508_rpm [4]; + + int flag; + }motorfeedback; @@ -125,7 +154,10 @@ typedef struct{ fp32 VESC_5065_M2_rpm; fp32 M2006_angle; - fp32 M3508_speed; + fp32 M3508_angle; + + fp32 go_shoot; + fp32 go_spin; }motor_target; @@ -133,33 +165,35 @@ typedef struct{ pid_type_def GM6020_speed; pid_type_def GM6020_angle; + pid_type_def VESC_5065_M1; pid_type_def VESC_5065_M2; + pid_type_def M2006_angle; pid_type_def M2006_speed; - pid_type_def M3508_speed[3]; + + pid_type_def M3508_angle; + pid_type_def M3508_speed; }pid; - - - /*经PID计算后的实际发送给电机的实时输出值*/ struct { - fp32 final_2006out; - fp32 final_3508out[3]; + fp32 final_3508out[4]; fp32 final_pitchout; fp32 final_VESC_5065_M1out; fp32 final_VESC_5065_M2out; + }final_out; LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ fp32 vofa_send[8]; + } UP_t; @@ -167,12 +201,12 @@ typedef struct{ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq); -int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) ; +int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ; int8_t GM6020_control(UP_t *u,fp32 angle); int8_t VESC_M5065_Control(UP_t *u,fp32 speed); int8_t GO_SendData(UP_t *u,int id,float pos); -int8_t UP_angle(UP_t *u, fp32 target_angle) ; -int8_t UP_control(UP_t *u,CAN_Output_t *out); +int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor); +int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c); int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out); int8_t UP_M2006_angle(UP_t *u,fp32 target_angle); int8_t UP_M3508_speed(UP_t *u,fp32 speed); diff --git a/User/device/Action.c b/User/device/Action.c deleted file mode 100644 index 0277d8a..0000000 --- a/User/device/Action.c +++ /dev/null @@ -1,203 +0,0 @@ - -/** - * 东北大学ACTION码盘驱动 - * - * 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。 - * 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。 - * - * 使用手册： - * 请参阅相关文档了解如何使用本驱动。 - * - * 注意： - * 本驱动仅适用于东北大学ACTION码盘。 - - *删去Action_HandleOffline错误处理函数中对结构体清0的函数 - - */ - -/* Includes ----------------------------------------------------------------- */ -#include "tim.h" -#include "Action.h" -#include - -static osThreadId_t thread_alert; - - -uint8_t rxbuf[RS232_FRAME_LENGTH]; - -static void Ops10msTimerCallback(void *arg){ - (void)arg; - osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY); -} - -/* Private function -------------------------------------------------------- */ -static void ACTION_IdleCallback(void) { - osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY); -} - -/* Exported functions ------------------------------------------------------- */ -int8_t ACTIONRECV_Init(Action_POS_t *pos){ - if(pos == NULL) return DEVICE_ERR_NULL; - - pos->Action_ready =0;//码盘校准标志位初始化 - - if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL; - pos->action_timer_Id = - osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL); - - osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数 - - BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB, - ACTION_IdleCallback); - - return DEVICE_OK; -} - -int8_t ACTION_StartReceiving() { - if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION), - (uint8_t *)rxbuf, - sizeof(rxbuf)) == HAL_OK) - return DEVICE_OK; - return DEVICE_ERR; -} - -bool_t ACTION_WaitDmaCplt(void) { - return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) == - SIGNAL_ACTION_RAW_REDY); -} - - -/* * - - -*/ -int8_t ACTION_Parse(Action_POS_t *pos) -{ - if (pos == NULL) return DEVICE_ERR_NULL; - - static union - { - fp32 pos_data[3]; - char rxbuff[12]; - } pos_rxbuf; - - - if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A) - { - if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D) - { - - pos_rxbuf.rxbuff[0]=rxbuf[2]; - pos_rxbuf.rxbuff[1]=rxbuf[3]; - pos_rxbuf.rxbuff[2]=rxbuf[4]; - pos_rxbuf.rxbuff[3]=rxbuf[5]; - - - pos_rxbuf.rxbuff[4]=rxbuf[14]; - pos_rxbuf.rxbuff[5]=rxbuf[15]; - pos_rxbuf.rxbuff[6]=rxbuf[16]; - pos_rxbuf.rxbuff[7]=rxbuf[17]; - - - pos_rxbuf.rxbuff[8]=rxbuf[18]; - pos_rxbuf.rxbuff[9]=rxbuf[19]; - pos_rxbuf.rxbuff[10]=rxbuf[20]; - pos_rxbuf.rxbuff[11]=rxbuf[21]; - - - - // 数据解析 - pos->pos_yaw = pos_rxbuf.pos_data[0]; - // 按照安装方向决定正负号 - pos->pos_x = pos_rxbuf.pos_data[1]; - pos->pos_y = pos_rxbuf.pos_data[2]; - - pos ->Action_ready =1;//码盘校准完成 - } - } - else - { - return -1; - } - return DEVICE_OK; -} - -//该函数用来计算速度(利用获取的位置来计算相应的速度) -//此处获取的是真实的位置(mm)后每10ms做一次微分处理 -int8_t ACTION_DataRefresh(Action_POS_t *pos){ - if (pos == NULL) return DEVICE_ERR_NULL; - - if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){ - osThreadFlagsClear(SIGNAL_OPSTIMER_REDY); - //计算每毫秒的速度 该函数每10ms调用 - pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10; - pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10; - - pos->pos_lastX = pos->pos_x; - pos->pos_lastY = pos->pos_y; - } - return DEVICE_OK; -} - - - -int8_t Action_HandleOffline(Action_POS_t *pos) { - if (pos == NULL) return DEVICE_ERR_NULL; - - (void)pos; - -// memset(pos, 0, sizeof(*pos)); - return 0; -} - -/* 将字符串拼接 */ -void Strcat(char str1[], char str2[], uint8_t num) -{ - int i = 0, j = 0; - - while (str1[i] != '\0') - i++; - - for (j = 0; j < num; j++) - { - str1[i++] = str2[j]; - } -} - -/* 该部分函数用于码盘的重定位系统 */ -/* ----------------------------- */ - -/* 手动标定，用于校正 */ -void ACT_Calibration(void) -{ - HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100); -} - -/* 清零 */ -/* 将当前位置设置为 (0, 0) */ -void ACT_ZeroClear(void) -{ - HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100); -} - -/* 更新 XY 坐标 */ -/* 将当前位置更新为指定的 X 和 Y */ -void ACT_UpdateXY(float pos_x, float pos_y) -{ - char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD" - static union - { - float XY[2]; // 两个浮点数表示 X 和 Y 坐标 - char data[8]; // 将浮点数视为字节数据 - } set; - - set.XY[0] = pos_x; - set.XY[1] = pos_y; - - Strcat(update_xy, set.data, 8); - - HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100); -} - - - diff --git a/User/device/Action.h b/User/device/Action.h deleted file mode 100644 index c0b8186..0000000 --- a/User/device/Action.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef _ACTION_H_ -#define _ACTION_H_ - -#define RS232_FRAME_LENGTH 28u -#define REF_LEN_RX_BUFF 0xFF - -#include -#include "device.h" -#include "bsp_usart.h" -#include "cmd.h" - - -typedef struct -{ - osTimerId_t action_timer_Id; - /*原始数据*/ - fp32 pos_x; - fp32 pos_y; - fp32 pos_yaw; // 航向角速度 - /*处理数据*/ - fp32 pos_Vx; - fp32 pos_Vy; - fp32 pos_lastX; - fp32 pos_lastY; - - int Action_ready;//码盘校准标志位 - - -}Action_POS_t; - - - - - -int8_t ACTIONRECV_Init(Action_POS_t*pos); - -int8_t ACTION_StartReceiving(void); - -bool_t ACTION_WaitDmaCplt(void); - -int8_t ACTION_Parse(Action_POS_t*pos); - -int8_t ACTION_DataRefresh(Action_POS_t*pos); - -int8_t Action_HandleOffline(Action_POS_t *pos); - -void ACT_UpdateXY(float pos_x,float pos_y); - -void ACT_ZeroClear(void); - - - - - - - - -#endif - diff --git a/User/device/GO_M8010_6_Driver.h b/User/device/GO_M8010_6_Driver.h index 265e862..b4bab93 100644 --- a/User/device/GO_M8010_6_Driver.h +++ b/User/device/GO_M8010_6_Driver.h @@ -8,6 +8,7 @@ extern "C"{ #include "usart.h" #include "string.h" #include +#include "user_math.h" #define GO_NUM 2 /** * @brief @@ -62,7 +63,8 @@ typedef struct { float tar_w; float T; float W; - float Pos; + float Pos; + int footForce; uint8_t buffer[17]; uint8_t Rec_buffer[16]; diff --git a/User/device/cmd.c b/User/device/cmd.c index 997135f..3e0fbad 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -8,79 +8,32 @@ /* Includes ----------------------------------------------------------------- */ #include "cmd.h" #include "gpio.h" - + #include /* Private function -------------------------------------------------------- */ /*Export function --------------------------------------------------------------*/ -int8_t CMD_Init(CMD_t *cmd){ - /*若主结构体为空 自动返回错误 */ - if(cmd == NULL) return-1; - /**/ - cmd->C_cmd.type =RC; - cmd->C_cmd.mode =NORMAL; - -return 0; -} - - -static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) { - - /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ - - cmd->Vx = rc->ch_r_x; - cmd->Vy = -rc->ch_r_y; - cmd->Vw = rc->ch_l_x; - - cmd->poscamear = rc->ch_l_y; - - cmd->key_ctrl_l = rc->sw_l; - cmd->key_ctrl_r = rc->sw_r ; - -} - - - -/** - * @brief rc失控时机器人恢复放松模式 - * - * @param cmd 主结构体 - */ -static void CMD_RcLostLogic(CMD_t *cmd){ - /* 机器人底盘运行模式恢复至放松模式 */ - cmd->C_cmd.mode = RELAXED; -} -int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){ - if (cmd == NULL) return -1; - if (rc == NULL) return -1; - /*c当rc丢控时，恢复机器人至默认状态 */ - if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { - CMD_RcLostLogic(cmd); - } else { - CMD_RcLogic(rc, cmd); - } -return 0; -} - +/*nuc数据统一到cmd*/ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ if(cmd == NULL) return -1; if(n == NULL) return -1; - cmd->cmd_status = n->status_fromnuc; - cmd->raw_status = n->ctrl_status; + cmd->NAVI_t .cmd_status = n->status_fromnuc; + cmd->NAVI_t .raw_status = n->ctrl_status; for (int i = 0; i < 7; ++i) { // 从最低位到最高位遍历 - cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0; + cmd->NAVI_t .status[i] = ((cmd->NAVI_t .raw_status) & (1 << i)) ? 1 : 0; } - switch(cmd->cmd_status){ + switch(cmd->NAVI_t .cmd_status){ + + case PICK : + cmd ->NAVI_t .pick .posx =n->pick .posx ; + cmd ->NAVI_t .pick .posy =n->pick .posy ; + cmd ->NAVI_t .pick .posw =n->pick .posw ; - case MID: - cmd->C_navi.vx = n->navi.vx; - cmd->C_navi.vy = n->navi.vy; - cmd->C_navi.wz = n->navi.wz; break; @@ -88,108 +41,40 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){ return 0; } - - -/* - 遥控器模式重新分配 - - 这一部分没有设置具体的模式名，后期根据需要修改 - 遥控器模式(RC): - 左按键 --- 右按键 - mode1 - up no_mode - mode2 - - - mode3 - down no_mode - mode4 - - mid auto_navi(0x09)雷达导航 - -*/ -int8_t CMD_CtrlSet(CMD_t *cmd) { +/*遥控器，上下层通用，按键控制，统一到cmd*/ +int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { if(cmd == NULL) return -1; - if(cmd->key_ctrl_l == CMD_SW_UP)//当左拨杆打到最上面时 强制使用遥控器控制 - { - /*遥控器模式下，右按键三种状态分配*/ - if(cmd->key_ctrl_r==CMD_SW_UP) - { - cmd->C_cmd.type = RC; - cmd->C_cmd.mode = RC_MODE1; - } - if(cmd->key_ctrl_r==CMD_SW_MID) - { - cmd->C_cmd.type = RC; - cmd->C_cmd.mode = RC_NO_MODE; - } - if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动 - { - cmd->C_cmd.type = RC; - cmd->C_cmd.mode =RC_MODE2; - } - - } - else if(cmd->key_ctrl_l ==CMD_SW_DOWN) + if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { + cmd->CMD_CtrlType =RELAXED; + } + else if(rc->sw_l==CMD_SW_UP) { - if(cmd->key_ctrl_r==CMD_SW_UP) - { - cmd->C_cmd.type = RC; - cmd->C_cmd.mode = RC_MODE3; - } - if(cmd->key_ctrl_r==CMD_SW_MID) - { - cmd->C_cmd.type = RC; - cmd->C_cmd.mode = RC_NO_MODE; - } - if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动 - { - cmd->C_cmd.type = RC; - cmd->C_cmd.mode = RC_MODE4; - } - + cmd ->CMD_CtrlType =UP_RCcontrol; + if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左上，右上，投篮 + if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Pitch_pull; //左上，右中，无模式 + if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_shoot; //左上，右上，投篮 } - else //左按键打到中间，自动模式 - { - if( cmd->key_ctrl_l==CMD_SW_MID ){ - cmd->C_cmd.type = MID_NAVI; - cmd->C_cmd.mode = RC_NO_MODE; - - switch(cmd->cmd_status) -{ - case MID://雷达，视觉导航 - cmd->C_cmd.type = MID_NAVI; - break; - } - } - - } -return 0; -} -//接收码盘导航的返回数据，传入cmd_t结构体 -int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act) -{ - if(cmd == NULL) return -1; - if(act == NULL) return -1; - - if(cmd->key_ctrl_l ==CMD_SW_MID ) + + else if(rc->sw_l==CMD_SW_MID) { - if(cmd->key_ctrl_r == CMD_SW_UP) - act->flag =1; - - if(cmd->key_ctrl_r == CMD_SW_DOWN ) - act->flag =-1; - } - - cmd->C_navi .vx =act ->out .Vx ; - cmd ->C_navi .vy =act ->out .Vy ; - cmd ->C_navi .wz =act ->out .Vw ; + if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达 + if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左中，右中，无模式 + if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左中，右上，运球 - - return 0; + } + else if(rc->sw_l==CMD_SW_DOWN) + { + cmd ->CMD_CtrlType =PICK_t; + if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮 + if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式 + if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮 + } + + return 0; } + diff --git a/User/device/cmd.h b/User/device/cmd.h index a412f6e..8f064b1 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -1,19 +1,3 @@ -/* - -该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块 - -*/ - - -/* - 按键控制逻辑 - RC模式，左按键打到最上，右按键启用，中间无状态，上下各代表模式1、模式2 - 雷达导航，左按键打到中间，右按键禁用 - 左按键打到下面，保留，未启用 - -*/ - - #ifndef _CMD_H @@ -23,38 +7,45 @@ #define MID (0x09) +#define PICK (0x06) typedef enum{ - RC,//遥控器控制，左按键上 + UP_RCcontrol,//遥控器控制，左按键上，控制上层 MID_NAVI,//雷达导航 - }CMD_Chassis_CtrlType_e; + PICK_t, + RELAXED,//异常模式 + + }CMD_CtrlType_t; typedef enum{ - RELAXED,//异常模式 - NORMAL, - GYRO_STAY, - - RC_MODE1, - RC_NO_MODE, - RC_MODE2, - - RC_MODE3, - RC_MODE4, - - AUTO_NAVI - }CMD_Chassis_mode_e; -/*该结构体负责接收和解析地盘模块所需要的控制指令*/ -typedef struct{ - - CMD_Chassis_CtrlType_e type; - - CMD_Chassis_mode_e mode; - - -}CMD_Chassis_Ctrl_t; - + + Normal, //无模式 + Dribble , //运球 + Pitch_pull , //投篮 + Pitch_shoot , //投篮 + }CMD_UP_mode_t; +typedef struct { + uint8_t status_fromnuc; + uint8_t ctrl_status; //取其中每一个二进制位用作通信 + struct + { + fp32 vx; + fp32 vy; + fp32 wz; + }navi; + struct + { + fp32 posx; + fp32 posy; + fp32 posw; + }pick; + struct + { + fp32 angle; + }sick_cali; +} CMD_NUC_t; /* 拨杆位置 */ typedef enum { CMD_SW_ERR = 0, @@ -82,102 +73,38 @@ typedef struct { } __attribute__((packed))CMD_RC_t; -typedef struct - { - fp32 Vx; - fp32 Vy; - fp32 Vw; - }OpsHopeVector_t; - - -typedef struct - { - fp32 x; - fp32 y; - fp32 z; - int cnt_point;//计数点 - }ops_point; - - typedef struct { - - OpsHopeVector_t out; - - int flag;//传递flag触发标志位 - -} CMD_ACTION_t; - -typedef struct { - uint8_t status_fromnuc; - uint8_t ctrl_status; //取其中每一个二进制位用作通信 - struct - { - fp32 vx; - fp32 vy; - fp32 wz; - }navi; - struct - { - fp32 posx; - fp32 posy; - fp32 posw; - }pick; - struct - { - fp32 angle; - }sick_cali; -} CMD_NUC_t; - -typedef struct{ - fp32 posy; - fp32 posx; - fp32 posw; -}CMD_FOR_PICK; -typedef struct { - fp32 vx; - fp32 vy; - fp32 wz; - - -}CMD_FOR_NAVI; - -typedef struct { - uint8_t cmd_status; - uint8_t raw_status; - - uint8_t status[7]; - - fp32 Vx; - fp32 Vy; - fp32 Vw; - - fp32 poscamear; - fp32 key_ctrl_l; fp32 key_ctrl_r; - - fp32 forsick_wz; - CMD_Chassis_Ctrl_t C_cmd; - CMD_FOR_NAVI C_navi; + /*视觉*/ + struct { + uint8_t cmd_status; + uint8_t raw_status; + uint8_t status[7]; + struct + { + fp32 posx; + fp32 posy; + fp32 posw; + }pick; + + }NAVI_t; + + + CMD_UP_mode_t CMD_UP_mode; + CMD_CtrlType_t CMD_CtrlType; + } CMD_t; -int8_t CMD_Init(CMD_t *cmd); - -int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc); - -int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act); - +/*nuc数据统一到cmd*/ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n); - -int8_t CMD_SwitchStatus(CMD_t *cmd); - -int8_t CMD_CtrlSet(CMD_t *cmd); +int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ; #endif diff --git a/User/task/action_task.c b/User/task/action_task.c deleted file mode 100644 index cae95a2..0000000 --- a/User/task/action_task.c +++ /dev/null @@ -1,75 +0,0 @@ -/* - 码盘解析任务 - - 处理码盘测量的实际距离 - - 将需要的数据共享给其他的线程 - -*/ -#include "action_task.h" -#include "Action.h" -#include "user_task.h" - - -#ifdef DEBUG - -Action_POS_t pos; -#else - -static Action_POS pos; - -#endif - -void Task_action(void *argument) -{ - (void)argument; -// osDelay(TASK_INIT_DELAY_ACTION); - - const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ACTION; - - - ACTIONRECV_Init(&pos); - - - uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/ - - while (1) - { -#ifdef DEBUG - /* 记录任务使用的的栈空闄*/ - task_runtime.stack_water_mark.action = - osThreadGetStackSpace(osThreadGetId()); -#endif - ACTION_StartReceiving(); - ACTION_DataRefresh(&pos); - - - - /* 使用下面的错误处理函数遇到一些问题 - 每10ms置标志位， ACTION_WaitDmaCplt 判断此标志位，通过后会执行速度计算 - 根据计算，任务运行频率大于中断产生频率，每一次运行判断标志位不通过，会导致结构体内数据清0的操作 - 目前的处理方法：注释掉Action_HandleOffline函数中，对结构体数据清零的函数memset - - - */ - - if(ACTION_WaitDmaCplt()){ - - ACTION_Parse(&pos); - - } - else{ - Action_HandleOffline(&pos); - } - - - //将解算后的码盘位置值放入消息队列供其他任务使用 - osMessageQueueReset(task_runtime.msgq.cmd.raw.Action); - osMessageQueuePut(task_runtime.msgq.cmd.raw.Action,(&pos),0,0); - - - tick += delay_tick; /* 计算下一个唤醒时刻*/ - osDelayUntil(tick); - } -} - diff --git a/User/task/action_task.h b/User/task/action_task.h deleted file mode 100644 index 8b13789..0000000 --- a/User/task/action_task.h +++ /dev/null @@ -1 +0,0 @@ - diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c deleted file mode 100644 index e5ee151..0000000 --- a/User/task/chassis_task.c +++ /dev/null @@ -1,94 +0,0 @@ -/* - 底盘控制任务 -*/ -/* Includes ----------------------------------------------------------------- */ - -#include "Chassis.h" -#include "user_task.h" -#include "can_use.h" -#include -#include "up.h" -#include "vofa.h" - -static CAN_t can; - - - -#ifdef DEBUG - -CAN_Output_t out; - -Chassis_t chassis ; - -CMD_t ctrl; - -UP_t upp; -#else - -static CAN_Output_t out; - -static Chassis_t chassis; - -static Chassis_Ctrl_t ctrl; - -static UP_t upp; - - -#endif - - - -/** - * \brief 底盘任务 - * - * \param argument - */ -void Task_Chassis(void *argument) -{ - - (void)argument; /* 未使用argument，消除警告*/ - const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP; - - - uint32_t tick = osKernelGetTickCount(); -// -// -//// HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除 -// - while(1) - { -#ifdef DEBUG - task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId()); -#endif - -// /*imu数据获取*/ -// osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0); -// -// osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro,NULL, 0); -// -// osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0); -// /*can上设备数据获取*/ -// osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0); -// -// osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0); - - /*底盘控制信息获取,目前dji遥控器*/ - osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0); - - /*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/ -// osKernelLock(); - - /*更新电机反馈 */ - Chassis_UpdateFeedback(&chassis, &can); - - /*底盘控制*/ - Chassis_Control(&chassis,&ctrl,&out); - - /*解锁*/ -// osKernelUnlock(); - - tick += delay_tick; - osDelayUntil(tick); - } - -} diff --git a/User/task/chassis_task.h b/User/task/chassis_task.h deleted file mode 100644 index 98ed1de..0000000 --- a/User/task/chassis_task.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef CHASSIS_TASK_H - - -#define CHASSIS_TASK_H - - - - - - - - - -extern void Task_Chassis(void *argument); - - - - - - - -#endif - diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c index 83bd681..94730ad 100644 --- a/User/task/cmd_task.c +++ b/User/task/cmd_task.c @@ -4,16 +4,14 @@ #ifdef DEBUG CMD_t cmd; -CMD_RC_t rc_ctrl; CMD_NUC_t Nuc; -CMD_ACTION_t cmd_ops_out; +CMD_RC_t rc_ctrl; #else static CMD_t cmd; -static CMD_RC_t rc_ctrl; static CMD_NUC_t Nuc; -static CMD_ACTION_t cmd_ops_out; +static CMD_RC_t rc_ctrl; #endif @@ -26,7 +24,6 @@ void Task_cmd(void *argument){ /* 计算到下一次调度任务所需要的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CMD; /**/ - CMD_Init(&cmd); uint32_t tick = osKernelGetTickCount(); /*控制任务运行频率的计时 */ while(1){ @@ -40,25 +37,17 @@ void Task_cmd(void *argument){ /*将各任务接收到的原始数据解析为通用的控制命令*/ - - - /*注意，不能将nuc和码盘导航一块使用*/ + if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器 + CMD_ParseRC(&cmd, &rc_ctrl); if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc CMD_ParseNuc(&cmd,&Nuc); - - CMD_CtrlSet(&cmd); - - if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器 - CMD_ParseRc(&cmd, &rc_ctrl); - - if(osMessageQueueGet(task_runtime.msgq.cmd.raw .ops_9_out , &cmd_ops_out, 0, 0) == osOK)//码盘 - CMD_ParseAction (&cmd ,&cmd_ops_out); + osKernelUnlock(); /* 同上 解锁RTOS内核 */ - /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */ - osMessageQueueReset(task_runtime.msgq.cmd.chassis); - osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0); + osMessageQueueReset(task_runtime.msgq.cmd.up_ctrl); + + osMessageQueuePut(task_runtime.msgq.cmd.up_ctrl,&cmd,0,0); tick += delay_tick; /*计算下一个唤醒时刻*/ osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */ diff --git a/User/task/init.c b/User/task/init.c index 54d4c71..d2cbadb 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -11,7 +11,6 @@ #include "task\user_task.h" #include "can_use.h" #include "cmd.h" -#include "Action.h" /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ @@ -86,6 +85,8 @@ void Task_Init(void *argument) { osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL); task_runtime.msgq.cmd.raw.nuc = osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL); + task_runtime.msgq.cmd.up_ctrl = + osMessageQueueNew(3u,sizeof(CMD_t), NULL); osKernelUnlock(); osThreadTerminate(osThreadGetId()); /* 结束自身 */ diff --git a/User/task/navi_task.c b/User/task/navi_task.c deleted file mode 100644 index 78dba44..0000000 --- a/User/task/navi_task.c +++ /dev/null @@ -1,68 +0,0 @@ -#include "user_task.h" -#include "navi.h" -#include "map.h" - -#ifdef DEBUG -ops_t o; -Action_POS_t pos_get; -CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值 - -#else - -static ops_t o; -static Action_POS_t pos_get; -static CMD_ACTION_t ops_out; - - -#endif - - -void Task_Navi(void *argument){ - - (void)argument; - - const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI; - - Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get); - - - uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/ - - while (1) - { -#ifdef DEBUG - /* 记录任务使用的的栈空间 */ - task_runtime.stack_water_mark.action = - osThreadGetStackSpace(osThreadGetId()); -#endif - osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0); - - osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0); - - osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0); - - go_path(&o,&ops_out); - - //将解算后的导航期望运动值放入消息队列供其他任务使用 - osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out); - osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0); - - tick += delay_tick; /* 计算下一个唤醒时刻*/ - osDelayUntil(tick); - } - -} - - - - -// -// -// -// -// -// -// - - - diff --git a/User/task/navi_task.h b/User/task/navi_task.h deleted file mode 100644 index 6f01d09..0000000 --- a/User/task/navi_task.h +++ /dev/null @@ -1 +0,0 @@ -#include "navi.h" \ No newline at end of file diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index 4956f3b..98d9c81 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -6,6 +6,8 @@ NUC_t nuc_raw; CMD_NUC_t cmd_fromnuc; #else +static NUC_t nuc_raw; +static CMD_NUC_t cmd_fromnuc; #endif diff --git a/User/task/up_task.c b/User/task/up_task.c index 7634fe9..1fe2e44 100644 --- a/User/task/up_task.c +++ b/User/task/up_task.c @@ -10,7 +10,7 @@ #include "vofa.h" -static CAN_t can; +static CAN_t can; #ifdef DEBUG @@ -19,6 +19,7 @@ CAN_Output_t UP_CAN_out; UP_t UP ; +CMD_t up_cmd; #else @@ -27,6 +28,7 @@ static CAN_Output_t up_can_out; static UP_t UP; +static CMD_t up_cmd; #endif float aaa=0; @@ -53,21 +55,21 @@ void Task_up(void *argument) #ifdef DEBUG task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId()); #endif - UP_UpdateFeedback(&UP, &can) ; + UP_UpdateFeedback(&UP, &can,&up_cmd) ; // GM6020_control(&UP, 100) ; - UP_angle(&UP, bbb); // UP_M3508_speed(&UP, 500); - - +// UP_angle_control(&UP,0,M2006); +// +// // VESC_M5065_Control(&UP, 20000); - - GO_SendData(&UP, 1,CCC); - GO_SendData(&UP, 0,aaa); - - ALL_Motor_Control(&UP,&UP_CAN_out); +// +// GO_SendData(&UP, 1,CCC); +// GO_SendData(&UP, 0,aaa); + UP_control(&UP,&UP_CAN_out,&up_cmd); + ALL_Motor_Control(&UP,&UP_CAN_out); osDelay(1); @@ -81,7 +83,8 @@ void Task_up(void *argument) /*can上设备数据获取*/ osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0); - + osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0); + /*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/ osKernelLock(); diff --git a/User/task/user_task.h b/User/task/user_task.h index c045325..0741e55 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -42,16 +42,12 @@ typedef struct { /* 任务 */ struct { osThreadId_t atti_esti; -// osThreadId_t chassis; osThreadId_t dr16; -// osThreadId_t action_ops; osThreadId_t up; + osThreadId_t nuc; -// osThreadId_t ai; osThreadId_t can; osThreadId_t cmd; - osThreadId_t nuc; -// osThreadId_t navi; osThreadId_t error_detect; } thread; @@ -66,14 +62,12 @@ typedef struct { struct { struct { osMessageQueueId_t rc; - osMessageQueueId_t nuc; + osMessageQueueId_t nuc; + osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/ - osMessageQueueId_t Action; - osMessageQueueId_t ops_9_out; - }raw; - osMessageQueueId_t chassis; + osMessageQueueId_t up_ctrl; osMessageQueueId_t status; } cmd; /* can任务放入、读取，电机或电容的输入输出 */ @@ -108,40 +102,29 @@ typedef struct { UBaseType_t can; UBaseType_t atti_esti; UBaseType_t dr16; - UBaseType_t action; UBaseType_t cmd; - UBaseType_t nuc; - UBaseType_t navi; UBaseType_t error_detect; + UBaseType_t nuc; UBaseType_t up; } stack_water_mark; /* stack使用 */ struct { -// float chassis; float can; float atti_esti; float r12ds; float cmd; - float nuc; -// float action; -// float navi; float error_detect; float up; } freq; /* 任务运行频率 */ struct { -// float chassis; float can; float atti_esti; -// float ai; float r12ds; float cmd; - float nuc; -// float navi; -// float action; float error_detect; float up; @@ -156,21 +139,15 @@ extern const osThreadAttr_t attr_init; extern const osThreadAttr_t attr_atti_esti; -//extern const osThreadAttr_t attr_chassis; - extern const osThreadAttr_t attr_can; extern const osThreadAttr_t attr_cmd; -//extern const osThreadAttr_t attr_ops_9pos; - -extern const osThreadAttr_t attr_nuc; - extern const osThreadAttr_t attr_error_detect; extern const osThreadAttr_t attr_dr16; -//extern const osThreadAttr_t attr_navi; +extern const osThreadAttr_t attr_nuc; extern const osThreadAttr_t attr_up; /* Exported functions prototypes -------------------------------------------- */ @@ -178,20 +155,14 @@ void Task_Init(void *argument); void Task_AttiEsti(void *argument); -//void Task_Chassis(void *argument); - void Task_can(void *argument); void Task_cmd(void *argument); -void Task_nuc(void *argument); - -//void Task_action(void *argument); - -//void Task_Navi(void *argument); - void Task_up(void *argument); +void Task_nuc(void *argument); + void Task_error_detect(void *argument); void Task_dr16(void *argument);