#include "up.h" #include "gpio.h" #include "user_math.h" #include "bsp_buzzer.h" #include "bsp_delay.h" /*接线 上层用到三个光电 1.PE9 发射用 输出高电平 LIGHTA 2.PE11 运球上3508 输出高电平 LIGHTB 3.PE13 三摩擦检测球 输出高电平LIGHTC */ #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)) //2006编码值转轴角度 #define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度 #define CAN2_G3508_ID (0x200) // 定义继电器控制 #define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) int count = 0; // 满足条件的计数 int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) { u->param = param; /*初始化参数 */ /*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 .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param )); PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param )); PID_init (&u->pid .Pitch_Angle_M2006_speed ,PID_POSITION ,&(u->param->Pitch_Angle_M2006_speed_param)); PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param )); PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param )); 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 )); 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 .is_initialized = 1; } if (!u->PitchContext .is_initialized) { u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->PitchContext .is_initialized = 1; } BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数,bsp层 } /*can,上层状态更新*/ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) { u->motorfeedback .VESC.VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ; u->motorfeedback .VESC.VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ; u->motorfeedback .Encoder .angle = can ->Oid.angle ; u->motorfeedback .Encoder .ecd = can ->Oid.ecd ; for(int i=0;i<4;i++){ //范围0x201--0x204 u->motorfeedback .DJmotor_feedback[i].id =i+CAN2_G3508_ID+1; u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_dribble_motor3508 .as_array [i].rotor_speed ; u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_dribble_motor3508.as_array [i].rotor_ecd ; DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR3508_ECD_TO_ANGLE); } for(int i=0;i<2;i++){ //范围0x205--0x206 u->motorfeedback .DJmotor_feedback[i+4].id =i+CAN2_G3508_ID+4+1; u->motorfeedback .DJmotor_feedback[i+4].rpm =can ->motor .up_shoot_motor3508 .as_array [i].rotor_speed ; u->motorfeedback .DJmotor_feedback[i+4].ecd =can ->motor .up_shoot_motor3508.as_array [i].rotor_ecd ; DJ_processdata(&u->motorfeedback .DJmotor_feedback [i+4], MOTOR2006_ECD_TO_ANGLE); } u->cmd =c; return 0; } int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle) { int8_t cnt=0; fp32 angle ,delta; angle = f->ecd; if (f->init_cnt < 50) { f->orig_angle= angle; f->last_angle = angle; f->init_cnt++; return 0; } delta = angle - f->last_angle; if (delta > 4096) { f->round_cnt--; } else if (delta < -4096) { f->round_cnt++; } f->last_angle = angle; f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle; } /* 这里id范围为1-4, */ 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) { fp32 delta_angle,delta_speed; delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle); delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle); u->final_out .DJfinal_out [id-CAN2_G3508_ID-1]=delta_speed; return 0; } int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed) { fp32 delta_speed; delta_speed = PID_calc(Speed_pid , f->rpm , target_speed); u->final_out .DJfinal_out [id-CAN2_G3508_ID-1]=delta_speed; return 0; } 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; return 0; } /*go电机控制*/ 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; // 前馈力矩 float kp = u->go_cmd.K_P; // 位置刚度 float kd = u->go_cmd.K_W; // 速度阻尼 float q_desired = u->go_cmd.Pos; // 期望位置(rad) float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置(rad) float dq_desired = u->go_cmd.W; // 期望角速度(rad/s) float dq_current = u->motorfeedback.go_data->W; // 当前角速度(rad/s) // 计算输出力矩 tau float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current); /*限制最大输入来限制最大输出*/ if (pos - q_current > limit) { u->go_cmd.Pos = q_current + limit; // 限制位置 }else if (pos - q_current < -limit) { u->go_cmd.Pos = q_current - limit; // 限制位置 }else { u->go_cmd.Pos = pos; // 允许位置 } // 发送数据 GO_M8010_send_data(&u->go_cmd); return 0; } 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[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 ); DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] , &u->pid .Shoot_M2006angle , &u->pid .Shoot_M2006speed , u->motor_target .Shoot_M2006_angle ); // DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] , // &u->pid .Pitch_M2006_angle , // &u->pid .Pitch_M2006_speed , // u->motor_target .Pitch_M2006_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 ); for(int i=0;i<4;i++){ out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ; } for(int i=0;i<2;i++){ out ->motor_UP3508_shoot.as_array[i]=u->final_out.DJfinal_out [i+4] ; } out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ; out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ; return 0; } 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; static int is_pitch=1; 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; is_pitch=0; } //让初始go位置只读一次,后面按调整模式更改的来,后面稳定了之后,可以跟随调整模式下一块删 u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 /*运球*/ RELAY1_TOGGLE(0);//关闭气缸 u->motor_target.Dribble_M3508_speed=0; u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle; u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态 count = 0 ; break; case Pitch : if (u->PitchContext .PitchState ==PITCH_PREPARE) { u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 } Pitch_Process(u,out,c); break ; case UP_Adjust: //测试用 u->PitchContext.PitchConfig.go1_init_position += c->Vx ; u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; break ; case Dribble: { if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){ u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE; } //光电状态更新 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); a=HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); b=HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); Dribble_Process(u,out); }break ; default: break; } return 0; } } int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) { switch(u->PitchContext .PitchState){ case PITCH_START: u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold; 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; }//更改标志位 break ; case PITCH_PULL_TRIGGER: if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1,可以认为已经勾上,误差为1 { u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; } break ; } return 0; } int8_t Dribble_Process(UP_t *u, CAN_Output_t *out) { switch (u->DribbleContext.DribbleState) { case DRIBBLE_TRANSLATE: u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f)) { u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后,转移状态 } break; case DRIBBLE_PROCESS_DOWN: u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed; if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm, u->motorfeedback .DJmotor_feedback [1].rpm, u->motorfeedback .DJmotor_feedback [2].rpm, u->DribbleContext .DribbleConfig.m3508_dribble_speed,50.0f,50) ) { RELAY1_TOGGLE(1); //速度达到后打开气缸 if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测,反转 u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP; } } break; case DRIBBLE_PROCESS_UP: if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&& (u->motorfeedback .DJmotor_feedback [1].rpm>0)&& (u->motorfeedback .DJmotor_feedback [2].rpm>0) ){ if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){ u->DribbleContext .DribbleState=DRIBBLE_DONE; RELAY1_TOGGLE(0); //关闭气缸 } } break ; case DRIBBLE_DONE: u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0 /*标志位清零*/ u->DribbleContext.DribbleConfig.light_ball_flag=0; u->DribbleContext.DribbleConfig.light_3508_flag=0; break; default: // 处理未知状态 break; } return 0; } bool is_reached_multiple(float current1, float current2, float current3, float target, float mistake, int threshold) { // static bool reached = false; // 是否已经满足条件 if (count >=50) { return true; // 如果已经满足条件,始终返回 1 } // 判断三个值是否都满足条件 bool all_reached = (fabs(current1 - target) < mistake) && (fabs(current2 -( -target)) < mistake) && (fabs(current3 - ( -target)) < mistake); if (all_reached) { count++; // 所有条件都满足,计数加 1 if (count >= threshold) { return true; } } else { count = 0; // 如果有一个不满足条件,计数清零 } return false; // 未满足条件达到阈值,返回 0 }