/** * @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 "LKMotor.h" #include "super_cap.h" #include "message_center.h" #include "referee_task.h" #include "user_lib.h" #include "general_def.h" #include "bsp_dwt.h" #include "referee_UI.h" #include "arm_math.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) // 轮子周长 #if defined(CHASSIS_MCNAMEE_WHEEL) #define CHASSIS_WHEEL_OFFSET 1.0f // 机器人底盘轮子修正偏移量 #elif defined(CHASSIS_STEERING_WHEEL) #define CHASSIS_WHEEL_OFFSET 30.0f // 机器人底盘轮子修正偏移量 #define SQRT2 1.41421356f // 根号2 #elif defined(CHASSIS_OMNI_WHEEL) #define CHASSIS_WHEEL_OFFSET 0.7071f // 机器人底盘轮子修正偏移量，根号2/2，即45度，用于修正全向轮的安装位置 #endif // CHASSIS_OMNI_WHEEL /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */ #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 referee_info_t* referee_data; // 用于获取裁判系统的数据 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI static SuperCapInstance *cap; // 超级电容 static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015 static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020 /* 用于自旋变速策略的时间变量 */ // static float t; /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅 static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅 static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID void ChassisInit() { // 轮电机9015初始化 Motor_Init_Config_s chassis_motor_config = { .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { .Kp = 95, // 4.5 .Ki = 0, // 0 .Kd = 0, // 0 .IntegralLimit = 8000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .MaxOut = 12000, }, .current_PID = { .Kp = 1, // 0.4 .Ki = 0, // 0 .Kd = 0, .IntegralLimit = 10500, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .MaxOut = 20000, }, }, .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, }, .motor_type = LK9015, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. chassis_motor_config.can_init_config.tx_id = 1; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rf = LKMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 2; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lb = LKMotorInit(&chassis_motor_config); // 舵电机6020初始化 Motor_Init_Config_s chassis_motor_steering_config = { .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .angle_PID = { .Kp = 12, .Ki = 0.2, .Kd = 0, .CoefA = 3, .CoefB = 0.1, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate, .IntegralLimit = 1000, .MaxOut = 5000, .Derivative_LPF_RC = 0.001, .DeadBand = 0.5, }, .speed_PID = { .Kp = 40, .Ki = 3, .Kd = 0, .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | PID_OutputFilter, .IntegralLimit = 4000, .MaxOut = 20000, .Output_LPF_RC = 0.03, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, }; chassis_motor_steering_config.can_init_config.tx_id = 1; motor_steering_rf = DJIMotorInit(&chassis_motor_steering_config); chassis_motor_steering_config.can_init_config.tx_id = 2; motor_steering_lb = DJIMotorInit(&chassis_motor_steering_config); PID_Init_Config_s chassis_follow_pid_conf = { .Kp = 6, // 6 .Ki = 0, .Kd = 0.495, // 0.5 // .DeadBand = 0.5f, //.CoefA = 0.2, //.CoefB = 0.3, //.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, //.IntegralLimit = 50, // 200 .MaxOut = 45, //.Derivative_LPF_RC = 0.01, // 0.01 }; PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf); // 裁判系统初始化 referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI SuperCap_Init_Config_s cap_conf = { .can_config = { .can_handle = &hcan2, .tx_id = 0x302, // 超级电容默认接收id .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 }}; cap = SuperCapInit(&cap_conf); // 超级电容初始化 // 发布订阅初始化,如果为双板,则需要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) // static void SteeringWheelRotateHandle() // { // if (fabsf(chassis_cmd_recv.vx) > 0 || fabsf(chassis_cmd_recv.vy) > 0) { // chassis_cmd_recv.wz = 0; // } // } /** * @brief 使舵电机角度最小旋转，取优弧，防止电机旋转不必要的行程 * 例如：上次角度为0，目标角度为135度， * 电机会选择逆时针旋转至-45度，而不是顺时针旋转至135度， * 两个角度都会让轮电机处于同一平行线上 * * @param angle 目标角度 * @param last_angle 上次角度 * */ static void MinmizeRotation(float *angle, const float *last_angle, float *speed) { float target_angle = *angle; float actual_angle = *last_angle; float rotation = target_angle - actual_angle; float norm_rotation = rotation; while (norm_rotation > 180.0f) { norm_rotation -= 360.0f; } while (norm_rotation < -180.0f) { norm_rotation += 360.0f; } float threshold = 110.0f; float target_single=fmodf(target_angle,360.0f); if(target_single<0.0f) target_single+=360.0f; float current_single=fmodf(actual_angle,360.0f); if(current_single<0.0f) current_single+=360.0f; if (norm_rotation > threshold) { int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f); *angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f; *speed = -(*speed); }else if(norm_rotation<-threshold){ int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f); *angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f; *speed = -(*speed); } // ANGLE_LIMIT_360_TO_180_ABS(*angle); // float rotation = *angle - *last_angle; // ANGLE_LIMIT_360_TO_180_ABS(rotation); // if (rotation > 90) { // *angle -= 180; // *speed = -(*speed); // } else if (rotation < -90) { // *angle += 180; // *speed = -(*speed); // } // ANGLE_LIMIT_360_TO_180_ABS(*angle); } /** * @brief 舵轮电机角度解算 * */ static void SteeringWheelCalculate() { float offset_rf, offset_lb; // 用于计算舵轮的角度 float at_rf_last, at_lb_last; // 上次的角度 at_lb_last = motor_steering_lb->measure.total_angle; // at_lf_last = motor_steering_lf->measure.total_angle; at_rf_last = motor_steering_rf->measure.total_angle; // at_rb_last = motor_steering_rb->measure.total_angle; if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) { // at_lf = at_lf_last; // at_rf = at_rf_last; // at_lb = at_lb_last; // at_rb = at_rb_last; vt_lb = 0, vt_rf = 0; } else { // 生成预计算变量，减少计算量，空间换时间 float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2; float temp_x = chassis_vx - w, temp_y = chassis_vy - w; // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x- // temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释 arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x- temp_x = chassis_vx + w; temp_y = chassis_vy + w;//与上次相同因此注释 // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+ // temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释 arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+ // 计算角度偏移 // offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x- offset_lb = -atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+ offset_rf = -atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb: y+ , x- // offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+ // at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb; // at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; // ANGLE_LIMIT_360_TO_180_ABS(at_lf); ANGLE_LIMIT_360_TO_180_ABS(at_rf); ANGLE_LIMIT_360_TO_180_ABS(at_lb); // ANGLE_LIMIT_360_TO_180_ABS(at_rb); // MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); MinmizeRotation(&at_rf, &at_rf_last, &vt_rf); MinmizeRotation(&at_lb, &at_lb_last, &vt_lb); // MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); } // DJIMotorSetRef(motor_steering_lf, at_lf); DJIMotorSetRef(motor_steering_rf, at_rf); DJIMotorSetRef(motor_steering_lb, at_lb); // DJIMotorSetRef(motor_steering_rb, at_rb); } /** * @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; // } /** * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 * */ static void LimitChassisOutput() { // 功率限制待添加 // referee_data->PowerHeatData.chassis_power; // referee_data->PowerHeatData.chassis_power_buffer; // 完成功率限制后进行电机参考输入设定 // LKMotorSetRef( motor_lf , vt_lf ); // LKMotorSetRef( motor_rb , vt_rb ); LKMotorSetRef( motor_rf , vt_rf ); LKMotorSetRef( motor_lb , vt_lb ); } /** * @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) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 LKMotorStop(motor_rf); LKMotorStop(motor_lb); DJIMotorStop(motor_steering_rf); DJIMotorStop(motor_steering_lb); } else { // 正常工作 LKMotorEnable(motor_rf); LKMotorEnable(motor_lb); DJIMotorEnable(motor_steering_rf); DJIMotorEnable(motor_steering_lb); } // 根据控制模式设定旋转速度 switch (chassis_cmd_recv.chassis_mode) { case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 // float single_round_offset = chassis_cmd_recv.offset_angle; float single_round_offset = chassis_cmd_recv.offset_angle; if(single_round_offset > 180.0f) { single_round_offset -= 360.0f; } else if(single_round_offset < -180.0f) { single_round_offset += 360.0f; } chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100; // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 chassis_cmd_recv.wz = 0.5; 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); 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; // 根据控制模式进行正运动学解算,计算底盘输出 SteeringWheelCalculate(); // MecanumCalculate(); // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 LimitChassisOutput(); // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; // // 当前只做了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; // 推送反馈消息 #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 }