HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

PITCH云台，小陀螺

This commit is contained in:
周楚杰 2024-01-16 19:51:18 +08:00
parent 0bf1ba7f3e
commit c2388e74e5
7 changed files with 87 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
application/chassis/chassis.c
View File

@ -70,7 +70,7 @@ void ChassisInit()
.MaxOut = 12000,
},
.current_PID = {
.Kp = 0.5, // 0.4
.Kp = 0.4, // 0.4
.Ki = 0, // 0
.Kd = 0,
.IntegralLimit = 3000,
@ -217,7 +217,7 @@ void ChassisTask()
chassis_cmd_recv.wz = 1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = 400;
chassis_cmd_recv.wz = 1600;
break;
default:
break;

13
application/cmd/robot_cmd.c
View File

@ -110,9 +110,9 @@ static void CalcOffsetAngle()
static void RemoteControlSet()
{
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
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)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
@ -132,12 +132,15 @@ static void RemoteControlSet()
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
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;
if(gimbal_cmd_send.pitch>=18.0) gimbal_cmd_send.pitch=18.0f;
if(gimbal_cmd_send.pitch<=-40.0) gimbal_cmd_send.pitch=-40.0f;
}
// 云台软件限位
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
chassis_cmd_send.vx = 10.1f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 10.1f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
// 发射参数
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
@ -152,7 +155,7 @@ static void RemoteControlSet()
shoot_cmd_send.friction_mode = FRICTION_OFF;
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
if (rc_data[TEMP].rc.dial < -500)
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
shoot_cmd_send.load_mode = LOAD_1_BULLET;
else
shoot_cmd_send.load_mode = LOAD_STOP;
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,

18
application/gimbal/gimbal.c
View File

@ -63,20 +63,20 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 50, // 10
.Ki = 0,
.Kp = 15, // 10
.Ki = 0.1f,
.Kd = 1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
.IntegralLimit = 100,//100
.MaxOut = 500,//500
},
.speed_PID = {
.Kp = 100, // 50
.Kp = 30, // 50
.Ki = 0, // 350
.Kd = 3, // 0
.Kd = 1, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.MaxOut = 20000,
.IntegralLimit = 150,//2500
.MaxOut = 2000,//20000
},
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
@ -90,7 +90,7 @@ void GimbalInit()
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
},
.motor_type = GM6020,
.motor_type = M3508,
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);

8
application/robot_def.h
View File

@ -26,15 +26,15 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 5009 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_CHASSIS_ALIGN_ECD 891 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
#define ONE_BULLET_DELTA_ANGLE 1368 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 19.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49
#define NUM_PER_CIRCLE 5 // 拨盘一圈的装载量
// 机器人底盘修改的参数,单位为mm(毫米)
#define WHEEL_BASE 420 // 纵向轴距(前进后退方向)
#define TRACK_WIDTH 450 // 横向轮距(左右平移方向)

76
application/shoot/shoot.c
View File

@ -6,6 +6,7 @@
#include "bsp_dwt.h"
#include "general_def.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
// static servo_instance *lid; 需要增加弹舱盖
@ -27,16 +28,16 @@ void ShootInit()
},
.controller_param_init_config = {
.speed_PID = {
.Kp = 0, // 20
.Ki = 0, // 1
.Kp = 20, // 20
.Ki = 1, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
.MaxOut = 15000,
},
.current_PID = {
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kp = 0.7f, // 0.7
.Ki = 0.1f, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
@ -52,51 +53,52 @@ void ShootInit()
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = M3508};
friction_config.can_init_config.tx_id = 1,
friction_config.can_init_config.tx_id = 3,
friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改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 = 3,
.can_handle = &hcan1,
.tx_id = 5,
},
.controller_param_init_config = {
.angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 0, // 10
.Ki = 0,
.Kp = 10, // 10
.Ki = 0.1f,
.Kd = 0,
.MaxOut = 200,
.MaxOut = 4500,
},
.speed_PID = {
.Kp = 0, // 10
.Ki = 0, // 1
.Kp = 10, // 10
.Ki = 1, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 5000,
.MaxOut = 10000,
},
.current_PID = {
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kp = 0.3f, // 0.7
.Ki = 0.1f, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 5000,
.MaxOut = 15000,
},
},
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = CURRENT_LOOP | SPEED_LOOP,
.close_loop_type = CURRENT_LOOP | SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
},
.motor_type = M2006 // 英雄使用m3508
.motor_type = M3508 // 英雄使用m3508
};
loader = DJIMotorInit(&loader_config);
@ -126,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)
@ -141,8 +143,8 @@ void ShootTask()
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 2000; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
case LOAD_3_BULLET:
@ -155,7 +157,7 @@ void ShootTask()
case LOAD_BURSTFIRE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是)
break;
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// 也有可能需要从switch-case中独立出来
@ -174,21 +176,21 @@ void ShootTask()
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
switch (shoot_cmd_recv.bullet_speed)
{
case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
case BIG_AMU_10:
DJIMotorSetRef(friction_l, 10);
DJIMotorSetRef(friction_r, 10);
break;
case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
case BIG_AMU_16:
DJIMotorSetRef(friction_l, 16);
DJIMotorSetRef(friction_r, 16);
break;
case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
case BULLET_SPEED_NONE:
DJIMotorSetRef(friction_l, 39000);
DJIMotorSetRef(friction_r, 39000);
break;
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
}
}
@ -209,5 +211,7 @@ void ShootTask()
}
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
}

28
daplink.cfg
View File

@ -1,18 +1,34 @@
# choose st-link/j-link/dap-link etc.
# choose CMSIS-DAP Debugger
adapter driver cmsis-dap
##adapter driver dap-link
# select SWD port
transport select swd
##transport select swd
# 0x10000 = 64K Flash Size
# 1MB on robomaster-C
set FLASH_SIZE 0x100000
##set FLASH_SIZE 0x100000
source [find target/stm32f4x.cfg]
##source [find target/stm32f4x.cfg]
# download speed = 10MHz
# 10MHz on FireDebugger
adapter speed 10000
##adapter speed 10000
# connect under reset
# reset_config srst_only
# reset_config srst_only
source [find interface/cmsis-dap.cfg]
transport select swd
# increase working area to 64KB
set WORKAREASIZE 0x10000
source [find target/stm32f4x.cfg]
reset_config none

3
modules/motor/DJImotor/dji_motor.c
View File

@ -270,7 +270,8 @@ void DJIMotorControl()
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor_controller->speed_feedforward_ptr; if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_ref += *motor_controller->speed_feedforward_ptr;
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED
pid_measure = measure->speed_aps;