#include "Chassis.h" #include "gpio.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->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType; c->chassis_ctrl.mode = ctrl->CMD_mode; return CHASSIS_OK; } 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.chassis_motor3508.as_array[i].rotor_speed; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current; } 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)); } PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param)); PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_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滤波 c->sick_cali .sickparam=c->param ->sickparam ; return CHASSIS_OK; } fp32 gyro_kp = 1.0f; fp32 PIAN = 0; void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { fp64 Nor_Vx, Nor_Vy; normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw; // 右前 c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw; // 右后 c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw; // 左后 c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw; // 左前 } float aaaaa=0; 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->chassis_ctrl.ctrl) { case RCcontrol: // 手动控制 /* 在cmd里对数据进行处理 包括方向和油门 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同 */ c->NUC_send .send [0]=0; c->NUC_send .send [1]=0; c->NUC_send .send [2]=10; c->NUC_send .send [3]=100; switch (c->chassis_ctrl.mode) { case Normal: if(ctrl->Vw){ c->move_vec.Vw = ctrl->Vw * 6000; c->move_vec.Vx = ctrl->Vy * 6000; c->move_vec.Vy = ctrl->Vx * 6000; } else { c->move_vec.Vw = ctrl->Vw * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa; c->move_vec.Vx = ctrl->Vy * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ; c->move_vec.Vy = ctrl->Vx * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ; } break; case Pitch: c->move_vec.Vw = 0; c->move_vec.Vx = 0; c->move_vec.Vy = 0; break; case UP_Adjust: c->move_vec.Vw = ctrl->Vw * 6000; c->move_vec.Vx = 0; c->move_vec.Vy = 0; break; default: c->move_vec.Vw = 0; c->move_vec.Vx = 0; c->move_vec.Vy = 0; break; } break; case AUTO: // 自动模式 switch (c->chassis_ctrl.mode) { case AUTO_MID360: // 自动雷达 // 全向轮方向, 注意xy方向 c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f); c->NUC_send .send [0]=0; break; case AUTO_MID360_Pitch: c->move_vec.Vw = 0; c->move_vec.Vy = 0; c->move_vec.Vx = 0; c->NUC_send .send [0]=0; break; case AUTO_MID360_Adjust: c->move_vec.Vw = ctrl->Vw * 6000; c->move_vec.Vx = ctrl->Vy * 6000; c->move_vec.Vy = ctrl->Vx * 6000; c->NUC_send .send [0]=1; break; default: c->move_vec.Vw = 0; c->move_vec.Vx = 0; c->move_vec.Vy = 0; c->NUC_send .send [0]=0; break; } break; default: c->move_vec.Vw = 0; c->move_vec.Vx = 0; c->move_vec.Vy = 0; break; } // 电机速度限幅 abs_limit_fp(&c->move_vec.Vx, 6000.0f); abs_limit_fp(&c->move_vec.Vy, 6000.0f); abs_limit_fp(&c->move_vec.Vw, 6000.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->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i]; } c->NUC_send .send [1]=1; return CHASSIS_OK; } /* sick0,下 sick1,左2 sick2,左1 */ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { if (c == NULL) return CHASSIS_ERR_NULL; if (ctrl == NULL) return CHASSIS_ERR_NULL; fp32 sick0 = c->sick_cali.sick_dis[0]; fp32 sick1 = c->sick_cali.sick_dis[1]; fp32 sick2 = c->sick_cali.sick_dis[2]; const sickparam_t *param = &c->sick_cali.sickparam; fp32 diff = fabsf(sick0 - sick1); // 1. yaw修正 if (diff > param->w_error) { c->move_vec.Vw = PID_calc(&(c->pid.chassis_SickVxPID), diff, 0); c->move_vec.Vx = 0; c->move_vec.Vy = 0; } // 2. xy修正 else if (diff > param->xy_error) { c->move_vec.Vw = 0; c->move_vec.Vx = PID_calc(&(c->pid.chassis_SickVxPID), sick0, param->x_set); c->move_vec.Vy = PID_calc(&(c->pid.chassis_SickVyPID), sick2, param->y_set); } // 3. 修正完成 else { c->move_vec.Vw = 0; c->move_vec.Vx = 0; c->move_vec.Vy = 0; } return CHASSIS_OK; }