mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 03:27:45 +08:00
Merge from captain chen
This commit is contained in:
NeoZng
@@ -68,16 +68,20 @@ void ChassisInit()
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 4000,
|
||||
.Kp = 4.5,//9
|
||||
.Ki = 0,//0.02
|
||||
.Kd = 0.01,//0.01
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
|
||||
.MaxOut = 12000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kp = 0.4,//0.7
|
||||
.Ki = 0,//0.1
|
||||
.Kd = 0,
|
||||
.MaxOut = 4000,
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
@@ -89,20 +93,20 @@ void ChassisInit()
|
||||
.motor_type = M3508,
|
||||
};
|
||||
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
||||
chassis_motor_config.can_init_config.tx_id = 4;
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
chassis_motor_config.can_init_config.tx_id = 1;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_lf = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 3,
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
chassis_motor_config.can_init_config.tx_id = 2;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_rf = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 1,
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
chassis_motor_config.can_init_config.tx_id = 4;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_lb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 2,
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
chassis_motor_config.can_init_config.tx_id = 3;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_rb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
// referee_data = RefereeInit(&huart6); // 裁判系统初始化
|
||||
@@ -153,7 +157,7 @@ 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_lb = chassis_vx - chassis_vy -chassis_cmd_recv.wz * LB_CENTER;
|
||||
vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
|
||||
}
|
||||
|
||||
@@ -220,10 +224,10 @@ void ChassisTask()
|
||||
chassis_cmd_recv.wz = 0;
|
||||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
|
||||
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f);
|
||||
chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle);
|
||||
break;
|
||||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
// chassis_cmd_recv.wz=sin(t)
|
||||
chassis_cmd_recv.wz=4000;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
@@ -21,7 +21,7 @@ static CANCommInstance *cmd_can_comm; // 双板通信
|
||||
#ifdef ONE_BOARD
|
||||
static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
|
||||
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
|
||||
#endif // ONE_BOARD
|
||||
#endif // ONE_BOARD
|
||||
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
|
||||
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
|
||||
@@ -68,6 +68,7 @@ void RobotCMDInit()
|
||||
};
|
||||
cmd_can_comm = CANCommInit(&comm_conf);
|
||||
#endif // GIMBAL_BOARD
|
||||
gimbal_cmd_send.pitch = 0;
|
||||
|
||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
}
|
||||
@@ -80,9 +81,9 @@ void RobotCMDInit()
|
||||
static void CalcOffsetAngle()
|
||||
{
|
||||
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
||||
static float angle;
|
||||
static float angle;
|
||||
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
|
||||
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
|
||||
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
|
||||
if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
|
||||
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
else if (angle > 180.0f + YAW_ALIGN_ANGLE)
|
||||
@@ -107,9 +108,15 @@ static void RemoteControlSet()
|
||||
{
|
||||
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
}
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
}
|
||||
|
||||
// 云台参数,确定云台控制数据
|
||||
if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
|
||||
@@ -120,9 +127,8 @@ static void RemoteControlSet()
|
||||
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
|
||||
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
|
||||
gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.rocker_l_;
|
||||
gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.rocker_l1;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
|
||||
gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
|
||||
}
|
||||
|
||||
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
|
||||
@@ -131,7 +137,7 @@ static void RemoteControlSet()
|
||||
|
||||
// 发射参数
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
|
||||
; // 弹舱舵机控制,待添加servo_motor模块,开启
|
||||
; // 弹舱舵机控制,待添加servo_motor模块,开启
|
||||
else
|
||||
; // 弹舱舵机控制,待添加servo_motor模块,关闭
|
||||
|
||||
@@ -146,7 +152,7 @@ static void RemoteControlSet()
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
|
||||
shoot_cmd_send.shoot_rate = 1;
|
||||
shoot_cmd_send.shoot_rate = 8;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -162,7 +168,7 @@ static void MouseKeySet()
|
||||
/**
|
||||
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
|
||||
* 停止的阈值'300'待修改成合适的值,或改为开关控制.
|
||||
*
|
||||
*
|
||||
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
|
||||
*
|
||||
*/
|
||||
@@ -171,16 +177,16 @@ static void EmergencyHandler()
|
||||
// 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
|
||||
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
{
|
||||
robot_state = ROBOT_STOP;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_OFF;
|
||||
robot_state = ROBOT_STOP; // 机器人状态保持急停
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; // 云台模式改为零力矩
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; // 底盘模式改为零力矩
|
||||
shoot_cmd_send.shoot_mode = SHOOT_OFF; // 射击模式改为关闭
|
||||
}
|
||||
// 遥控器右侧开关为[上],恢复正常运行
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right))
|
||||
{
|
||||
robot_state = ROBOT_READY;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
robot_state = ROBOT_READY; // 机器人状态改为准备就绪
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON; // 射击模式改为开启
|
||||
}
|
||||
}
|
||||
|
||||
@@ -199,7 +205,6 @@ void RobotCMDTask()
|
||||
|
||||
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
|
||||
CalcOffsetAngle();
|
||||
|
||||
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
|
||||
RemoteControlSet();
|
||||
@@ -209,8 +214,8 @@ void RobotCMDTask()
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
// 设置视觉发送数据,还需增加加速度和角速度数据
|
||||
vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed;
|
||||
vision_send_data.enemy_color = chassis_fetch_data.enemy_color;
|
||||
vision_send_data.bullet_speed = 15;
|
||||
vision_send_data.enemy_color = 0;
|
||||
vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
|
||||
vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw;
|
||||
vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
//
|
||||
#include "gimbal.h"
|
||||
#include "robot_def.h"
|
||||
|
||||
//
|
||||
#include "dji_motor.h"
|
||||
#include "ins_task.h"
|
||||
#include "message_center.h"
|
||||
@@ -54,8 +55,8 @@ void GimbalInit()
|
||||
.cali_mode=BMI088_CALIBRATE_ONLINE_MODE,
|
||||
.work_mode=BMI088_BLOCK_PERIODIC_MODE,
|
||||
};
|
||||
// imu=BMI088Register(&imu_config);
|
||||
// gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
|
||||
// imu=BMI088Register(&imu_config);
|
||||
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
|
||||
// YAW
|
||||
Motor_Init_Config_s yaw_config = {
|
||||
.can_init_config = {
|
||||
@@ -64,60 +65,68 @@ void GimbalInit()
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 20,
|
||||
.Kp = 8, //8
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.DeadBand = 0.3,
|
||||
.DeadBand = 0.1,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 100,
|
||||
|
||||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kp = 50,//40
|
||||
.Ki = 200,
|
||||
.Kd = 0,
|
||||
.MaxOut = 4000,
|
||||
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 3000,
|
||||
.MaxOut = 20000,
|
||||
},
|
||||
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
|
||||
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
||||
// .other_speed_feedback_ptr=&gimba_IMU_data.wz;
|
||||
.other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2],
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020};
|
||||
// PITCH
|
||||
Motor_Init_Config_s pitch_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 2,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 30,
|
||||
.Kp =10,//10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 4000,
|
||||
.DeadBand = 0.3,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
|
||||
.IntegralLimit =100,
|
||||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 4000,
|
||||
.Kp=50,//50
|
||||
.Ki =350,//350
|
||||
.Kd =0,//0.1
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
|
||||
.IntegralLimit =2500,
|
||||
.MaxOut = 20000,
|
||||
},
|
||||
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
|
||||
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
||||
// .other_speed_feedback_ptr=&gimba_IMU_data.wy,
|
||||
.other_speed_feedback_ptr=(&gimba_IMU_data->Gyro[0]),
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020,
|
||||
};
|
||||
@@ -160,10 +169,10 @@ void GimbalTask()
|
||||
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
|
||||
DJIMotorEnable(yaw_motor);
|
||||
DJIMotorEnable(pitch_motor);
|
||||
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);
|
||||
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); // yaw和pitch会在robot_cmd中处理好多圈和单圈
|
||||
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
|
||||
break;
|
||||
|
||||
@@ -34,21 +34,25 @@
|
||||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
// 发射参数
|
||||
#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||
#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量
|
||||
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
|
||||
// 机器人底盘修改的参数,单位为mm(毫米)
|
||||
#define WHEEL_BASE 300 // 纵向轴距(前进后退方向)
|
||||
#define WHEEL_BASE 350 // 纵向轴距(前进后退方向)
|
||||
#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 // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
|
||||
|
||||
#define GYRO2GIMBAL_DIR_YAW 1 //陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
|
||||
#define GYRO2GIMBAL_DIR_PITCH 1 //陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
|
||||
#define GYRO2GIMBAL_DIR_ROLL 1 //陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反
|
||||
|
||||
// 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错
|
||||
#if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \
|
||||
(defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \
|
||||
|
||||
@@ -26,20 +26,23 @@ void ShootInit()
|
||||
Motor_Init_Config_s friction_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 6,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kp = 0,//20
|
||||
.Ki = 0,//1
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 10000,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kp = 0,//0.7
|
||||
.Ki = 0,//0.1
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 10000,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
@@ -48,47 +51,52 @@ void ShootInit()
|
||||
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
friction_config.can_init_config.tx_id = 1,
|
||||
friction_l = DJIMotorInit(&friction_config);
|
||||
|
||||
friction_config.can_init_config.tx_id = 5; // 右摩擦轮,改txid和方向就行
|
||||
friction_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
|
||||
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
friction_r = DJIMotorInit(&friction_config);
|
||||
|
||||
// 拨盘电机
|
||||
Motor_Init_Config_s loader_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 7,
|
||||
.tx_id = 3,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||||
.Kp = 10,
|
||||
.Kp = 0,//10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 200,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kp = 0,//10
|
||||
.Ki = 0,//1
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 5000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kp = 0,//0.7
|
||||
.Ki = 0,//0.1
|
||||
.Kd = 0,
|
||||
.MaxOut = 3000,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 5000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
|
||||
.close_loop_type = CURRENT_LOOP | SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
|
||||
},
|
||||
.motor_type = M2006 // 英雄使用m3508
|
||||
};
|
||||
@@ -120,8 +128,8 @@ void ShootTask()
|
||||
|
||||
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
|
||||
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
|
||||
if (hibernate_time + dead_time > DWT_GetTimeline_ms())
|
||||
return;
|
||||
// if (hibernate_time + dead_time > DWT_GetTimeline_ms())
|
||||
// return;
|
||||
|
||||
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
|
||||
switch (shoot_cmd_recv.load_mode)
|
||||
@@ -182,8 +190,8 @@ void ShootTask()
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||
DJIMotorSetRef(friction_l, 1000);
|
||||
DJIMotorSetRef(friction_r, 1000);
|
||||
DJIMotorSetRef(friction_l, 30000);
|
||||
DJIMotorSetRef(friction_r, 30000);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user