diff --git a/.vscode/settings.json b/.vscode/settings.json index f09fc5c..cb8bdf0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -35,7 +35,8 @@ "compare": "c", "*.tcc": "c", "type_traits": "c", - "typeinfo": "c" + "typeinfo": "c", + "general_def.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index f4e4d70..7a9886c 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -18,18 +18,12 @@ #include "message_center.h" #include "referee.h" #include "general_def.h" +#include "bsp_dwt.h" #include "arm_math.h" -/* 需要根据机器人底盘修改的参数,单位为mm(毫米) */ -#define WHEEL_BASE 300 // 纵向轴距(前进后退方向) -#define TRACK_WIDTH 300 // 横向轮距(左右平移方向) -#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 -#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 -#define RADIUS_WHEEL 60 // 轮子半径 -#define REDUCTION_RATIO 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 -/* 自动计算的参数 */ +/* 根据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) @@ -54,6 +48,9 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv; static Subscriber_t *chassis_sub; static Chassis_Upload_Data_s chassis_feedback_data; +/* 用于自旋变速策略的时间变量,后续考虑查表加速 */ +static float t; + /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 @@ -173,8 +170,8 @@ 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; + vt_lb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER; + vt_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER; } /** @@ -215,31 +212,31 @@ void ChassisTask() // 后续增加不同状态的过渡模式? switch (chassis_cmd_recv.chassis_mode) { - case CHASSIS_NO_FOLLOW: - chassis_cmd_recv.wz = 0; // 云台任意移动模式,不旋转,一般用于调整云台姿态 - break; - case CHASSIS_FOLLOW_GIMBAL_YAW: - chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不开pid,以误差角平方为输出 - break; - case CHASSIS_ROTATE: - // chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略 - break; case CHASSIS_ZERO_FORCE: DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 DJIMotorStop(motor_rf); DJIMotorStop(motor_lb); DJIMotorStop(motor_rb); break; + case CHASSIS_NO_FOLLOW: + chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 + break; + case CHASSIS_FOLLOW_GIMBAL_YAW: + chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角平方为速度输出 + break; + case CHASSIS_ROTATE: + // chassis_cmd_recv.wz=sin(t) // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 + break; default: break; } // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) - chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) - - chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle); - chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) - - chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle); + chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) - + chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); + chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) - + chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); // 根据控制模式进行正运动学解算,计算底盘输出 MecanumCalculate(); @@ -251,8 +248,9 @@ void ChassisTask() EstimateSpeed(); // 获取裁判系统数据 - // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; // + // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red + chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; + // 当前只做了17mm的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 chassis_feedback_data.bullet_speed = referee_data->GameRobotStat.shooter_id1_17mm_speed_limit; chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index fbad603..f0d0e20 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -5,13 +5,9 @@ #include "message_center.h" #include "general_def.h" -/* 根据每个机器人进行设定,若对云台有改动需要修改 */ -#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值 -#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值 - // 自动将编码器转换成角度值 -#define YAW_CHASSIS_ALIGN_ANGLE -#define PTICH_HORIZON_ALIGN_ANGLE +#define YAW_CHASSIS_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF) +#define PTICH_HORIZON_ALIGN_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF) static attitude_t *Gimbal_IMU_data; // 云台IMU数据 static dji_motor_instance *yaw_motor; // yaw电机 @@ -81,6 +77,7 @@ void GimbalInit() }, .motor_type = GM6020}; + // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); pitch_motor = DJIMotorInit(&pitch_config); @@ -104,29 +101,31 @@ void GimbalTask() SubGetMessage(gimbal_sub, &gimbal_cmd_recv); // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref - // 是否要增加不同模式之间的过渡? + // 是否要增加不同模式之间的过渡? switch (gimbal_cmd_recv.gimbal_mode) { + // 停止 case GIMBAL_ZERO_FORCE: DJIMotorStop(yaw_motor); DJIMotorStop(pitch_motor); break; + // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; - // 是否考虑直接让云台和底盘重合,其他模式都使用陀螺仪反馈? + // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED); - DJIMotorSetRef(yaw_motor, YAW_ALIGN_ECD); - DJIMotorSetRef(pitch_motor, PITCH_HORIZON_ECD); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; default: break; @@ -146,9 +145,9 @@ void GimbalTask() } */ - // 获取反馈数据 + // 设置反馈数据 gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data; - gimbal_feedback_data.yaw_motor_ecd = pitch_motor->motor_measure.ecd; + gimbal_feedback_data.yaw_motor_single_round_angle = pitch_motor->motor_measure.angle_single_round; // 推送消息 PubPushMessage(gimbal_pub, &gimbal_feedback_data); diff --git a/application/robot_def.h b/application/robot_def.h index 5e5b980..e31669f 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -23,7 +23,21 @@ // #define GIMBAL_BOARD //云台板 /* 重要参数定义,注意根据不同机器人进行修改 */ -#define YAW_MID_ECD +// 云台参数 +#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +// 发射参数 +#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f +#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量 +// 机器人底盘修改的参数,单位为mm(毫米) +#define WHEEL_BASE 300 // 纵向轴距(前进后退方向) +#define TRACK_WIDTH 300 // 横向轮距(左右平移方向) +#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 +#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 +#define RADIUS_WHEEL 60 // 轮子半径 +#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 + #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ @@ -129,13 +143,13 @@ typedef struct // cmd发布的发射控制数据,由shoot订阅 typedef struct -{ +{ loader_mode_e load_mode; lid_mode_e lid_mode; shoot_mode_e shoot_mode; Bullet_Speed_e bullet_speed; // 弹速枚举 uint8_t rest_heat; - float shoot_rate; //连续发射的射频,unit per s,发/秒 + float shoot_rate; // 连续发射的射频,unit per s,发/秒 } Shoot_Ctrl_Cmd_s; /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/ @@ -166,7 +180,7 @@ typedef struct typedef struct { attitude_t gimbal_imu_data; - uint16_t yaw_motor_ecd; + uint16_t yaw_motor_single_round_angle; } Gimbal_Upload_Data_s; typedef struct diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e5d07b9..f667e6f 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -5,9 +5,6 @@ #include "bsp_dwt.h" #include "general_def.h" -#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量 /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ static dji_motor_instance *friction_l; // 左摩擦轮 @@ -137,7 +134,7 @@ void ShootTask() if (hibernate_time + dead_time > DWT_GetTimeline_ms()) return; - // 若不在休眠状态,根据控制模式进行电机参考值设定和模式切换 + // 若不在休眠状态,根据控制模式进行拨盘电机参考值设定和模式切换 switch (shoot_cmd_recv.load_mode) { // 停止拨盘 @@ -162,7 +159,7 @@ void ShootTask() // 连发模式,对速度闭环,射频后续修改为可变 case LOAD_BURSTFIRE: DJIMotorOuterLoop(loader, SPEED_LOOP); - DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO / NUM_PER_CIRCLE); + DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_WHEEL / NUM_PER_CIRCLE); // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度 break; // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈) @@ -175,7 +172,7 @@ void ShootTask() break; } - // 根据收到的弹速设置设定摩擦轮参考值,需实测后填入 + // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入 switch (shoot_cmd_recv.bullet_speed) { case SMALL_AMU_15: diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index ee45e8d..e011708 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -1,8 +1,6 @@ #include "dji_motor.h" #include "general_def.h" -#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制 - static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ @@ -134,19 +132,19 @@ static void DecodeDJIMotor(can_instance *_instance) // resolve data and apply filter to current and speed measure->last_ecd = measure->ecd; measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); - measure->angle_single_round = ECD_ANGLE_COEF* measure->ecd; - measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + + measure->angle_single_round = ECD_ANGLE_COEF * measure->ecd; + measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]); - measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + + measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + RPM_2_ANGLE_PER_SEC * CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); measure->temperate = rxbuff[6]; // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180° - if (measure->ecd - measure->last_ecd > 4096) + if (measure->ecd - measure->last_ecd > 4096) measure->total_round--; else if (measure->ecd - measure->last_ecd < -4096) measure->total_round++; - measure->total_angle = measure->total_round * 360 + measure->angle_single_round; + measure->total_angle = measure->total_round * 360 + measure->angle_single_round; break; } } @@ -168,7 +166,7 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config) PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID); dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; - + // group motors, because 4 motors share the same CAN control message MotorSenderGrouping(&config->can_init_config); @@ -251,13 +249,13 @@ void DJIMotorControl() { if (motor_setting->speed_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_speed_feedback_ptr; - else + else // MOTOR_FEED pid_measure = motor_measure->speed_angle_per_sec; // 更新pid_ref进入下一个环 motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref); } - // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身的反馈 + // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 if (motor_setting->close_loop_type & CURRENT_LOOP) { motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref); diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 1adfb74..7053e9b 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -24,6 +24,8 @@ /* 滤波系数设置为1的时候即关闭滤波 */ #define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85 #define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95 +#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制 + /* DJI电机CAN反馈信息*/ typedef struct diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 0f109d1..2156a84 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -37,8 +37,8 @@ typedef enum /* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/ typedef enum { - MOTOR_FEED = 0, - OTHER_FEED = 1 + MOTOR_FEED, + OTHER_FEED, } Feedback_Source_e; /* 电机正反转标志 */