更改了宏定义的文件
This commit is contained in:
NeoZng
parent
8e7935876e
commit
3c8ffda61d
|
|
@ -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",
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
/* 电机正反转标志 */
|
||||
|
|
|
|||
Loading…
Reference in New Issue