/** * @file chassis.c * @author NeoZeng neozng1@hnu.edu.cn * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出 * 注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向 * * @version 0.1 * @date 2022-12-04 * * @copyright Copyright (c) 2022 * */ #include "chassis.h" #include "robot_def.h" #include "dji_motor.h" #include "super_cap.h" #include "message_center.h" #include "referee_task.h" #include "general_def.h" #include "bsp_dwt.h" #include "referee_UI.h" #include "arm_math.h" #include "user_lib.h" #include "vofa.h" /* 根据robot_def.h中的macro自动计算的参数 */ #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长 #define P_MAX 50.0f//功率控制 单位：W /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */ #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度 #include "can_comm.h" #include "ins_task.h" static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm attitude_t *Chassis_IMU_data; #endif // CHASSIS_BOARD #ifdef ONE_BOARD static Publisher_t *chassis_pub; // 用于发布底盘的数据 static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令 #endif // !ONE_BOARD static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 static Chassis_Ctrl_Cmd_s Chassis_Power_Mx; const static float CHASSIS_ACCEL_X_NUM=0.1f; const static float CHASSIS_ACCEL_Y_NUM=0.1f; // 超级电容 SuperCapInstance *cap; // 超级电容全局 float power_set; // 超级电容功率设置 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back /* 用于自旋变速策略的时间变量 */ // static float t; /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 first_order_filter_type_t vx_filter; first_order_filter_type_t vy_filter; void ChassisInit() { // 四个轮子的参数一样,改tx_id和反转标志位即可 Motor_Init_Config_s chassis_motor_config = { .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { .Kp = 4.25f, // 4.5 .Ki = 0.6f, // 0.6 .Kd = 0.01f, // 0.01 .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .MaxOut = 30000, }, .current_PID = { .Kp = 2.2f, // 2.5 .Ki = 0.6f, // 0.8 .Kd = 0.5f, // 0.2 .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .MaxOut = 15000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP, //| CURRENT_LOOP, .power_limit_flag = NO_POWER_LIMIT, }, .motor_type = M3508, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. chassis_motor_config.can_init_config.tx_id = 1; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rf = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 2; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; motor_lf = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 3; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; motor_lb = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 4; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rb = DJIMotorInit(&chassis_motor_config); // //超级电容 // SuperCap_Init_Config_s cap_conf = { // .can_config = { // .can_handle = &hcan1, // .tx_id = 0x210, // .rx_id = 0x211, // }}; // cap = SuperCapInit(&cap_conf); // 超级电容初始化 // power_set = 70; // SuperCapSetPower(cap,power_set); // 超级电容限制功率 //用一阶滤波代替斜波函数生成 first_order_filter_init(&vx_filter, 0.002f, &CHASSIS_ACCEL_X_NUM); first_order_filter_init(&vy_filter, 0.002f, &CHASSIS_ACCEL_Y_NUM); // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 CANComm_Init_Config_s comm_conf = { .can_config = { .can_handle = &hcan2, .tx_id = 0x311, .rx_id = 0x312, }, .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), .send_data_len = sizeof(Chassis_Upload_Data_s), }; chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化 #endif // CHASSIS_BOARD #ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息 chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); #endif // ONE_BOARD } #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) #define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) /** * @brief 计算每个轮毂电机的输出,正运动学解算 * 用宏进行预替换减小开销,运动解算具体过程参考教程 */ static void MecanumCalculate() { vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER; vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER; vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER; vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; } //全向轮解算 static void OmniCalculate() { vt_rf = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz + chassis_vx - chassis_vy; vt_rb = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz - chassis_vx - chassis_vy; vt_lb = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz - chassis_vx + chassis_vy; vt_lf = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz + chassis_vx + chassis_vy; vt_rf /= (WHEEL_BASE * 1.414f); vt_rb /= (WHEEL_BASE * 1.414f); vt_lb /= (WHEEL_BASE * 1.414f); vt_lf /= (WHEEL_BASE * 1.414f); } static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f}; ///依据3508电机功率模型，预测电机输出功率 static float EstimatePower(DJIMotorInstance* chassis_motor) { float I_cmd = chassis_motor->motor_controller.current_PID.Output; float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; return power; } float vofa_send_data[6]; /** * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 * */ static void LimitChassisOutput() { float Plimit = 1.0f; float P_cmd = motor_rf->motor_controller.motor_power_predict + motor_rb->motor_controller.motor_power_predict + motor_lb->motor_controller.motor_power_predict + motor_lf->motor_controller.motor_power_predict + 3.6f; if(chassis_cmd_recv.buffer_energy<50&&chassis_cmd_recv.buffer_energy>=40) Plimit=0.9f; else if(chassis_cmd_recv.buffer_energy<40&&chassis_cmd_recv.buffer_energy>=35) Plimit=0.75f; else if(chassis_cmd_recv.buffer_energy<35&&chassis_cmd_recv.buffer_energy>=30) Plimit=0.5f; else if(chassis_cmd_recv.buffer_energy<30&&chassis_cmd_recv.buffer_energy>=20) Plimit=0.25f; else if(chassis_cmd_recv.buffer_energy<20&&chassis_cmd_recv.buffer_energy>=10) Plimit=0.125f; else if(chassis_cmd_recv.buffer_energy<10&&chassis_cmd_recv.buffer_energy>=0) Plimit=0.05f; else if(chassis_cmd_recv.buffer_energy==60) Plimit=1.0f; float K = ((float)(chassis_cmd_recv.chassis_power_limit - 3) * (Plimit + 0.50f) ) / P_cmd; //float K = (float)(chassis_cmd_recv.chassis_power_limit - 5) / P_cmd; motor_rf->motor_controller.motor_power_scale = K; motor_rb->motor_controller.motor_power_scale = K; motor_lf->motor_controller.motor_power_scale = K; motor_lb->motor_controller.motor_power_scale = K; { DJIMotorSetRef(motor_lf, vt_lf); DJIMotorSetRef(motor_rf, vt_rf); DJIMotorSetRef(motor_lb, vt_lb); DJIMotorSetRef(motor_rb, vt_rb); } } /** * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算 * 对于双板的情况,考虑增加来自底盘板IMU的数据 * */ static void EstimateSpeed() { // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有) // chassis_feedback_data.vx vy wz = // ... } /* 机器人底盘控制核心任务 */ void ChassisTask() { // 后续增加没收到消息的处理(双板的情况) // 获取新的控制信息 #ifdef ONE_BOARD SubGetMessage(chassis_sub, &chassis_cmd_recv); #endif #ifdef CHASSIS_BOARD chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm); #endif // CHASSIS_BOARD if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 DJIMotorStop(motor_lf); DJIMotorStop(motor_rf); DJIMotorStop(motor_lb); DJIMotorStop(motor_rb); } else { // 正常工作 DJIMotorEnable(motor_lf); DJIMotorEnable(motor_rf); DJIMotorEnable(motor_lb); DJIMotorEnable(motor_rb); } // 根据控制模式设定旋转速度 switch (chassis_cmd_recv.chassis_mode) { case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 chassis_cmd_recv.wz = 10.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); if(chassis_cmd_recv.wz > 30000) chassis_cmd_recv.wz = 30000; if(chassis_cmd_recv.wz < -30000) chassis_cmd_recv.wz = -30000; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 chassis_cmd_recv.wz = 30000; break; default: break; } // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) static float sin_theta, cos_theta; cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); //一阶低通滤波计算 first_order_filter_cali(&vx_filter, chassis_cmd_recv.vx); first_order_filter_cali(&vy_filter, chassis_cmd_recv.vy); chassis_cmd_recv.vx = vx_filter.out; chassis_cmd_recv.vy = vy_filter.out; chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; // chassis_vx = (1.0f - 0.30f) * chassis_vx + 0.30f * (chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta); // chassis_vy = (1.0f - 0.30f) * chassis_vy + 0.30f * (chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta); // 根据控制模式进行正运动学解算,计算底盘输出 //MecanumCalculate(); OmniCalculate(); // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 LimitChassisOutput(); // float vofa_send_data[2]; // vofa_send_data[0] = motor_lb->motor_controller.speed_PID.Ref; // vofa_send_data[1] = motor_lb->motor_controller.speed_PID.Measure; // vofa_justfloat_output(vofa_send_data, 8, &huart1); // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); //chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 //chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; //chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; chassis_feedback_data.cap_power = cap->cap_msg.cap_vol; // 推送反馈消息 #ifdef ONE_BOARD PubPushMessage(chassis_pub, (void *) &chassis_feedback_data); #endif #ifdef CHASSIS_BOARD CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); #endif // CHASSIS_BOARD }