diff --git a/Core/Src/main.c b/Core/Src/main.c index f48044b..557aee4 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -80,7 +80,7 @@ int main(void) /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ - HAL_Init(); + HAL_Init(); /* USER CODE BEGIN Init */ diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index 9610097..19d1e8d 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -205,6 +205,11 @@ 1 can_out + + 11 + 1 + \\R2\../User/task/up_task.c\UP.motorfeedback.DJmotor_feedback + 0 diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index a717ef1..d54ecb2 100644 Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c index 52b02c4..49ab692 100644 --- a/User/Algorithm/user_math.c +++ b/User/Algorithm/user_math.c @@ -320,4 +320,15 @@ int abs_value(int num) { } else { return -num; // 如果数是负的,返回它的相反数 } -} \ No newline at end of file +} + +/** + * @brief 判断是否到达目标值 + * @param current 当前值 + * @param target 目标值 + * @param mistake 阈值 + * @return 如果到达目标值返回 true,否则返回 false + */ +bool is_reached(float current, float target, float mistake) { + return fabs(current - target) < mistake; +} diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h index 8021299..0f5ce0e 100644 --- a/User/Algorithm/user_math.h +++ b/User/Algorithm/user_math.h @@ -160,4 +160,6 @@ uint8_t average(uint8_t arr[], uint8_t n); int abs_value(int num); float expo_map(float value, float expo_factor) ; +bool is_reached(float current, float target, float mistake) ; + #endif diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c index 338b2e3..0291a32 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -22,7 +22,6 @@ 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; /*模式未改变直接返回*/ - //此处源代码处做了pid的reset 待添加 c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ; c->chassis_ctrl.mode =ctrl->CMD_mode ; @@ -85,20 +84,15 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq) void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算 { - 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] = -Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//右前 + c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//右后 + c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//左后 + c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//左前 } -//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) { @@ -215,11 +209,12 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) //电机速度限幅 - abs_limit_fp(&c->move_vec.Vx,8000.0f); + abs_limit_fp(&c->move_vec.Vx,6000.0f); - abs_limit_fp(&c->move_vec.Vy,8000.0f); + abs_limit_fp(&c->move_vec.Vy,6000.0f); + + abs_limit_fp(&c->move_vec.Vw,6000.0f); - abs_limit_fp(&c->move_vec.Vw,8000.0f); Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); diff --git a/User/Module/config.c b/User/Module/config.c index d0e7dd9..a21b32e 100644 --- a/User/Module/config.c +++ b/User/Module/config.c @@ -37,10 +37,10 @@ static const ConfigParam_t param ={ .out_limit = 3000.0f, }, .Pitch_M2006_angle_param = { - .p = 25.0f, + .p = 400.0f, .i = 0.0f, - .d = 1.5f, - .i_limit = 1000.0f, + .d = 3.0f, + .i_limit = 2000.0f, .out_limit = 3000.0f, }, .Pitch_M2006_speed_param={ @@ -97,11 +97,10 @@ static const ConfigParam_t param ={ /*运球*/ .DribbleConfig_Config = { .m3508_init_angle = 50, - .m3508_high_angle = 1200, - .go2_init_angle = 0, - .go2_flip_angle = -250, - .flip_timing = 200, - .go2_release_threshold = -550.0f, + .m3508_translate_angle = 1200, + .m3508_dribble_Reverse_speed=-3000, + . m3508_dribble_speed= 3000, // 转动速度 + }, /*投球*/ .PitchConfig_Config = { diff --git a/User/Module/up.c b/User/Module/up.c index 568ace0..eb850c5 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -36,16 +36,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param )); PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param )); - - PID_init (&u->pid .Dribble_M3508_speed ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param )); - + for(int i=0;i<3;i++){ + PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param )); + } u->go_cmd =u->param ->go_cmd ; // 初始化上层状态机 if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机,运球 u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 - u->DribbleContext .DribbleState = Dribble_PREPARE; + u->DribbleContext .DribbleState = DRIBBLE_PREPARE; u->DribbleContext .is_initialized = 1; } @@ -197,10 +197,10 @@ int8_t GO_SendData(UP_t *u, float pos, float limit) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) { //电机控制 ,传进can - DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); - DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); - DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); - DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] , + DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed); + DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed); + DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed); + 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 ); @@ -212,8 +212,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) // &u->pid .Pitch_M2006_angle , // &u->pid .Pitch_M2006_speed , // u->motor_target .Pitch_M2006_angle ); - u->final_out .DJfinal_out [5]=PID_calc (&(u->pid .Pitch_Angle_M2006_speed ), - u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle); + DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed, + (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 ); @@ -241,9 +243,12 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) if(u ==NULL) return 0; static int is_pitch=1; + + + switch (c->CMD_CtrlType ) { - case RCcontrol: //在手动模式下 + case RCcontrol: //在手动模式下 switch (c-> CMD_mode ) { @@ -259,15 +264,14 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 -// /*运球*/ -// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ; -// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; -// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态 + /*运球*/ + + RELAY1_TOGGLE(0);//关闭气缸 + + u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态 + u->motor_target.Dribble_M3508_speed=0; -// RELAY1_TOGGLE (1);//夹球,0夹1开 -// RELAY2_TOGGLE (0);//球,0接1收 -// break; @@ -282,29 +286,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) break ; case UP_Adjust: u->PitchContext.PitchConfig.go1_init_position += c->Vx ; -// u->PitchContext.PitchConfig.m2006_Screw_init = c->Vy; u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; -// u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init; + u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; break ; case Dribble: { - /*夹球 -> 3508 升起 ,同时go2翻转 -> 到位置后,继电器开,放球,同时3508降,go2翻回->接球,收 */ - if(u->DribbleContext.DribbleState== Dribble_PREPARE){ - - u->DribbleContext .DribbleState =STATE_GRAB_BALL; + if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){ + u->DribbleContext .DribbleState=DRIBBLE_PROCESS; } -// Dribble_Process(u,out); - - + Dribble_Process(u,out); }break ; - case Dribbl_transfer: - break ; -// Dribbl_transfer_a(u,out); - } - break; + default: @@ -319,7 +314,7 @@ return 0; } - +} @@ -334,10 +329,6 @@ return 0; int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) { -// go1_position=u->PitchContext .PitchConfig .go1_init_position ; -// M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init; - - switch(u->PitchContext .PitchState){ case PITCH_START: @@ -353,19 +344,15 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) case PITCH_PULL_TRIGGER: - if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1,可以认为已经勾上,误差为1 + if(is_reached ( u->motorfeedback .DJmotor_feedback [0].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1,可以认为已经勾上,误差为1 { u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; -// u->PitchContext.PitchConfig.go1_init_position += c->Vx ; -// u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy; - } break ; - } return 0; @@ -376,124 +363,38 @@ return 0; -int8_t Pitch_Adjust(UP_t *u, CAN_Output_t *out,CMD_t *c) + + + + +int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) { - - u->PitchContext.PitchConfig.go1_init_position += c->Vx ; - u->PitchContext.PitchConfig.Pitch_angle += c->Vy; - u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; - - + + switch (u->DribbleContext.DribbleState) { + + case DRIBBLE_TRANSLATE: + u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 + + break; + case DRIBBLE_PROCESS: + u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed; + if(is_reached(u->motorfeedback .DJmotor_feedback [0].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&& + is_reached(u->motorfeedback .DJmotor_feedback [1].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&& + is_reached(u->motorfeedback .DJmotor_feedback [2].rpm,u->motor_target.Dribble_M3508_speed,70.0f) + ) { + RELAY1_TOGGLE(1); //速度达到后打开气缸 + u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; + } + + break; + case DRIBBLE_DONE: + u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed; + + break; + default: + // 处理未知状态 + break; + } + return 0; } - - - - - -//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) -//{ -// -// switch (u->DribbleContext.DribbleState) { -// case STATE_GRAB_BALL://开始 -// -// RELAY1_TOGGLE (0);//夹球 -// -// u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起 -// u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle; - -// -// if((u->motorfeedback .M3508 .total_angle >400)) { -// RELAY2_TOGGLE (1); -// - -//// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){ -//// -//// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置,标志位改变 -//// } -// -// } -// break; - - -// case STATE_RELEASE_BALL: -// RELAY1_TOGGLE (1);//松球 - -// -// -// -//// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){ -//// u->motor_target.go_spin =-40; //恢复go2位置 -//// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置,标志位改变 - -//// } - -// break; - -// case STATE_CATCH_PREP: -//// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){ -//// u->motor_target.M3508_angle =0 ; //当go2到初始位置,3508降 -//// -//// RELAY2_TOGGLE (0);//接球 -//// } -// if(u->motorfeedback .M3508 .total_angle <51.0f){ -// RELAY1_TOGGLE (0);//夹球,0夹1开 - -// u->DribbleContext .DribbleState = STATE_TRANSFER; -// } -// -// break; -// - -// -// -// break ; - - - -// default: -// break; -// } - -// return 0; -//} - -//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out) -//{ -// switch (u->DribbleContext.DribbleState) { -// -// -// -// case STATE_TRANSFER: -// if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置,再夹上球 -// { -// -// u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ; -// } -// -//// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9) -//// { -//// RELAY1_TOGGLE (1);//夹球,0夹1开 -//// -//// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8) -//// { -//// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置,标志位改变 -//// } -//// } -// -// -// break ; - -// case STATE_CATCH_DONE: -// -// RELAY1_TOGGLE (1);//夹球,0夹1开 -// RELAY2_TOGGLE (0);//夹球,0接1收 - -// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ; - -// break; - -// break; - -// } -//} diff --git a/User/Module/up.h b/User/Module/up.h index b394340..315be94 100644 --- a/User/Module/up.h +++ b/User/Module/up.h @@ -76,23 +76,20 @@ typedef struct { /*运球*/ typedef enum { - Dribble_PREPARE, - STATE_GRAB_BALL, // 夹球升起阶段 - STATE_RELEASE_BALL, // 释放球体 - STATE_CATCH_PREP, // 接球准备 - STATE_CATCH_BALL, // 接球动作 - STATE_TRANSFER, //转移球 - STATE_CATCH_DONE //完成 + DRIBBLE_PREPARE, // 准备阶段 + DRIBBLE_START, + DRIBBLE_TRANSLATE, // 平移过程 + DRIBBLE_PROCESS, // 运球过程 + DRIBBLE_DONE // 运球结束 } DribbleState_t; /* 参数配置结构体 */ typedef struct { - fp32 m3508_init_angle; // 3508初始角度 - fp32 m3508_high_angle; // 3508升起角度 - fp32 go2_init_angle; // GO2初始角度 - fp32 go2_flip_angle; // GO2翻转角度 - fp32 flip_timing; // 翻转触发时机 - fp32 go2_release_threshold; // 释放球 + fp32 m3508_init_angle; // 平移前位置 + fp32 m3508_translate_angle; // 平移后位置 + fp32 m3508_dribble_speed; // 转动速度 + fp32 m3508_dribble_Reverse_speed; + } DribbleConfig_t; /* 状态机上下文 */ @@ -211,7 +208,7 @@ typedef struct{ pid_type_def Pitch_Angle_M2006_speed; - pid_type_def Dribble_M3508_speed;//三摩擦用的速度环 + pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环 pid_type_def Dribble_translate_M3508_speed;//平移用的速度环 pid_type_def Dribble_translate_M3508_angle;//平移用的角度环 @@ -252,6 +249,7 @@ int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle); int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle); int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed); +int8_t Dribble_Process(UP_t *u, CAN_Output_t *out); int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); diff --git a/User/device/can_use.c b/User/device/can_use.c index 6ebb56e..357db27 100644 --- a/User/device/can_use.c +++ b/User/device/can_use.c @@ -76,7 +76,7 @@ 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=feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f; + feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f; break; } } diff --git a/User/device/cmd.c b/User/device/cmd.c index 68e017f..899aad9 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -196,8 +196,9 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){ } else if(cmd ->CMD_CtrlType ==RCcontrol){ /*手动下的*/ - if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode =UP_Adjust; + 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 cmd ->CMD_mode =Normal; } } diff --git a/User/task/error_detect_task.c b/User/task/error_detect_task.c index 8ace272..ad43631 100644 --- a/User/task/error_detect_task.c +++ b/User/task/error_detect_task.c @@ -96,10 +96,10 @@ void Task_error_detect(void *argument){ //若所有设备在线,会不断翻转绿灯 存在设备离线则会红灯灯翻转 - if(online_dev == ERROR_LIST_LENGHT) - { +// if(online_dev == ERROR_LIST_LENGHT) +// { HAL_GPIO_TogglePin(GPIOH,LED_G_Pin); - } +// } // else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin); for (int i = 0; i < ERROR_LIST_LENGHT; i++)