#include "up.h" #include "gpio.h" #include "user_math.h" #include "bsp_buzzer.h" #include "bsp_delay.h" #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 RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) #define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) 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_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 )); 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_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 ; for(int i=0;i<4;i++){ u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ; u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .motor3508 .as_array [i].rotor_ecd ; DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], 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-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;//校准前，原始数据 GO_calibration = 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_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] , &u->pid .Shoot_M2006angle , &u->pid .Shoot_M2006speed , u->motor_target .Shoot_M2006_angle ); DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] , &u->pid .Pitch_M2006_angle , &u->pid .Pitch_M2006_speed , u->motor_target .Pitch_M2006_angle ); GO_SendData(u,u->motor_target .go_shoot,1 ); for(int i=0;i<4;i++){ out ->motor_UP3508.as_array[i]=u->final_out.DJfinal_out [i] ; } out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ; out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ; return 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 ; is_pitch=0; } // break; case Pitch : if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动 { u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 } break ; return 0; } } }