#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; }