TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

24对抗赛

Browse Source
This commit is contained in:
chenfu
2024-06-01 00:11:37 +08:00
parent da591dcfb4
commit c1ab3edf88
11 changed files with 136 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.vscode/settings.json vendored
View File

@@ -8,7 +8,11 @@
"user_lib.h": "c", "user_lib.h": "c",
"servo_motor.h": "c", "servo_motor.h": "c",
"main.h": "c", "main.h": "c",
"buzzer.h": "c" "buzzer.h": "c",
"robot_def.h": "c",
"bsp_log.h": "c",
"bsp_usart.h": "c",
"memory.h": "c"
}, },
"C_Cpp.errorSquiggles": "disabled" "C_Cpp.errorSquiggles": "disabled"
} }

3
Makefile
View File

@@ -205,8 +205,7 @@ AS_DEFS =
C_DEFS = \ C_DEFS = \
-DUSE_HAL_DRIVER \ -DUSE_HAL_DRIVER \
-DSTM32F407xx \ -DSTM32F407xx \
-DARM_MATH_CM4 \ -DARM_MATH_CM4
-DDISABLE_LOG_SYSTEM
# AS includes # AS includes
AS_INCLUDES = \ AS_INCLUDES = \

35
application/chassis/balance.c
View File

@@ -20,7 +20,6 @@
#include "speed_estimation.h" #include "speed_estimation.h"
#include "fly_detection.h" #include "fly_detection.h"
#include "buzzer.h" #include "buzzer.h"
// 计时变量 // 计时变量
static uint32_t balance_dwt_cnt; static uint32_t balance_dwt_cnt;
static float del_t; static float del_t;
@@ -30,7 +29,7 @@ static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data; // 底盘单独调试用 static RC_ctrl_t *rc_data; // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
static Chassis_Can_Comm chassis_can_recv;
// 四个关节电机和两个驱动轮电机 // 四个关节电机和两个驱动轮电机
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2]; static LKMotorInstance *l_driven, *r_driven, *driven[2];
@@ -290,8 +289,8 @@ static void ResetChassis()
// chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入 // 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w); LKMotorSetRef(l_driven, chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w); LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
// 若关节完成复位,进入ready态 // 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 && if (abs(lf->measure.total_angle) < 0.05 &&
@@ -356,15 +355,15 @@ static void WokingStateSet()
HTMotorOuterLoop(joint[i], OPEN_LOOP); HTMotorOuterLoop(joint[i], OPEN_LOOP);
// 设置目标速度/腿长/距离 // 设置目标速度/腿长/距离
l_side.target_len += chassis_cmd_recv.delta_leglen; l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
r_side.target_len += chassis_cmd_recv.delta_leglen; r_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
// 腿长限幅 // 腿长限幅
VAL_LIMIT(l_side.target_len, 0.12, 0.30); VAL_LIMIT(l_side.target_len, 0.12, 0.30);
VAL_LIMIT(r_side.target_len, 0.12, 0.30); VAL_LIMIT(r_side.target_len, 0.12, 0.30);
// 加速度限幅,防止键盘控制摔倒 // 加速度限幅,防止键盘控制摔倒
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t; chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
VAL_LIMIT(r_side.target_len, 0.002, 2.0); // VAL_LIMIT(chassis.target_v, 0.002, 2.0);
// 角度输入 // 角度输入
if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
{ {
@@ -420,11 +419,11 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
} }
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺 else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺
{ {
PIDCalculate(&steer_v_pid, chassis.wz, chassis_cmd_recv.rotate_w); PIDCalculate(&steer_v_pid, chassis.wz, (float)chassis_cmd_recv.rotate_w);
} }
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE_REVERSE) else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE_REVERSE)
{ {
PIDCalculate(&steer_v_pid, chassis.wz, -chassis_cmd_recv.rotate_w); PIDCalculate(&steer_v_pid, chassis.wz, (float)chassis_cmd_recv.rotate_w);
} }
l_side.T_wheel -= steer_v_pid.Output; l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output; r_side.T_wheel += steer_v_pid.Output;
@@ -463,16 +462,16 @@ static void WattLimitSet() /* 设定运动模态的输出 */
// 裁判系统,双板通信,电容功率控制等 // 裁判系统,双板通信,电容功率控制等
static void CommNPower() static void CommNPower()
{ {
static uint8_t supercap_send_cnt = 0; //static uint8_t supercap_send_cnt = 0;
// CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data); // CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
supercap_send_cnt++; // supercap_send_cnt++;
if (supercap_send_cnt % 5 == 0) // if (supercap_send_cnt % 5 == 0)
{ // {
DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送 // DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送
DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit; // DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit;
SuperCapSend(cap, (uint8_t *)&DataSend2Cap); // SuperCapSend(cap, (uint8_t *)&DataSend2Cap);
supercap_send_cnt = 0; // supercap_send_cnt = 0;
} // }
/* 更新ui数据 */ /* 更新ui数据 */
ui_data.direction = chassis_cmd_recv.direction; ui_data.direction = chassis_cmd_recv.direction;
ui_data.friction_mode = chassis_cmd_recv.friction_mode; ui_data.friction_mode = chassis_cmd_recv.friction_mode;

89
application/cmd/robot_cmd.c
View File

@@ -14,11 +14,13 @@
// bsp // bsp
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "bsp_log.h" #include "bsp_log.h"
#include "string.h"
static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
static Chassis_Can_Comm chassis_can_send;
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
@@ -120,16 +122,17 @@ static void CalcOffsetAngle()
* @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
* *
*/ */
int aaaa;
static void RemoteControlSet() static void RemoteControlSet()
{ {
memcpy(&rc_data->key_count, 0, sizeof(rc_data->key_count)); memset(&rc_data->key_count, 0, sizeof(rc_data->key_count));
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
// 云台参数,确定云台控制数据 // 云台参数,确定云台控制数据
gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1; gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1;
chassis_cmd_send.rotate_w = 0.0f; chassis_cmd_send.rotate_w = 0;
chassis_cmd_send.delta_leglen = 0.0f;
// 摇杆控制的软件限位 // 摇杆控制的软件限位
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
@@ -152,16 +155,17 @@ static void RemoteControlSet()
// 右侧开关状态为[下],底盘旋转 // 右侧开关状态为[下],底盘旋转
if (switch_is_down(rc_data[TEMP].rc.switch_right)) if (switch_is_down(rc_data[TEMP].rc.switch_right))
{ {
chassis_cmd_send.rotate_w = 4.0f;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
if (abs(rc_data[TEMP].rc.rocker_r_) > 300) if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
{ {
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
chassis_cmd_send.rotate_w = 2;
} }
if (abs(rc_data[TEMP].rc.rocker_r1) > 300) if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
{ {
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
chassis_cmd_send.rotate_w = -2;
} }
chassis_cmd_send.vx = 0; chassis_cmd_send.vx = 0;
} }
@@ -174,7 +178,7 @@ static void RemoteControlSet()
// 左侧开关状态为[中]，腿长控制 // 左侧开关状态为[中]，腿长控制
if (switch_is_mid(rc_data[TEMP].rc.switch_left)) if (switch_is_mid(rc_data[TEMP].rc.switch_left))
{ {
chassis_cmd_send.delta_leglen = -0.000001f * (abs(rc_data[TEMP].rc.dial) > 100 ? (float)rc_data[TEMP].rc.dial : 0); chassis_cmd_send.delta_leglen = -(abs(rc_data[TEMP].rc.dial) > 100 ? (int8_t)(rc_data[TEMP].rc.dial / 6) : 0);
} }
// 摩擦轮控制,拨轮向上打为负,向下为正 // 摩擦轮控制,拨轮向上打为负,向下为正
@@ -194,7 +198,7 @@ static void RemoteControlSet()
// 发射参数 // 发射参数
shoot_cmd_send.bullet_speed = 30; shoot_cmd_send.bullet_speed = 30;
shoot_cmd_send.shoot_rate = 10; shoot_cmd_send.shoot_rate = 15;
} }
/** /**
@@ -203,14 +207,14 @@ static void RemoteControlSet()
*/ */
static void MouseKeySet() static void MouseKeySet()
{ {
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; //云台默认为陀螺仪模式
shoot_cmd_send.shoot_mode = SHOOT_ON;
gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f; // 系数待测 gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f; // 系数待测
gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f; gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f;
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); // 限制pitch角度
if (chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) if (chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘速度输入 默认无前进轮速
{ {
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d); chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
} }
@@ -219,9 +223,10 @@ static void MouseKeySet()
chassis_cmd_send.vx = 0; chassis_cmd_send.vx = 0;
} }
chassis_cmd_send.delta_leglen = (float)(rc_data[TEMP].key[KEY_PRESS].e - rc_data[TEMP].key[KEY_PRESS].c) * 0.001f; chassis_cmd_send.delta_leglen = (rc_data[TEMP].key[KEY_PRESS].e - rc_data[TEMP].key[KEY_PRESS].c) * 10; // E C控制腿长
if (rc_data[TEMP].key[KEY_PRESS].w || rc_data[TEMP].key[KEY_PRESS].s) //wsad控制底盘方向 但会有额外速度输入 不使用
if (rc_data[TEMP].key[KEY_PRESS].w || rc_data[TEMP].key[KEY_PRESS].s)
{ {
chassis_direction = CHASSIS_ALIGN; chassis_direction = CHASSIS_ALIGN;
} }
@@ -229,7 +234,18 @@ static void MouseKeySet()
{ {
chassis_direction = CHASSIS_SIDLE; chassis_direction = CHASSIS_SIDLE;
} }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2)
// switch(rc_data[TEMP].key_count[KEY_PRESS][Key_X] % 2) // x键旋转
// {
// case 0:
// chassis_direction = CHASSIS_ALIGN;
// break;
// case 1:
// chassis_direction = CHASSIS_SIDLE;
// break;
// }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键开启底盘跟随云台 仅重置底盘后开启
{ {
case 0: case 0:
break; break;
@@ -237,6 +253,7 @@ static void MouseKeySet()
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
break; break;
} }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮 switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮
{ {
case 0: case 0:
@@ -249,8 +266,18 @@ static void MouseKeySet()
if (shoot_cmd_send.friction_mode == FRICTION_ON) if (shoot_cmd_send.friction_mode == FRICTION_ON)
{ {
if (rc_data[TEMP].mouse.press_l) switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V 拨弹模式切换
{
case 0:
shoot_cmd_send.load_mode = LOAD_BURSTFIRE; shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
break;
case 1:
shoot_cmd_send.load_mode = LOAD_1_BULLET;
break;
}
if (rc_data[TEMP].mouse.press_l) // 鼠标左键发射
{
}
else else
shoot_cmd_send.load_mode = LOAD_STOP; shoot_cmd_send.load_mode = LOAD_STOP;
} }
@@ -267,17 +294,21 @@ static void MouseKeySet()
break; break;
} }
if (rc_data[TEMP].mouse.press_r) if (rc_data[TEMP].mouse.press_r) // 鼠标右键视觉模式
{ {
gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw); gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch); gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch);
} }
if (rc_data[TEMP].key[KEY_PRESS].q) if (rc_data[TEMP].key[KEY_PRESS].q) // Q键底盘旋转
{ {
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
chassis_cmd_send.rotate_w = 6.0f; chassis_cmd_send.rotate_w = 6;
} }
if (rc_data[TEMP].key[KEY_PRESS].r) else{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
}
if (rc_data[TEMP].key[KEY_PRESS].r) // R键底盘复位
{ {
chassis_cmd_send.chassis_mode = CHASSIS_RESET; chassis_cmd_send.chassis_mode = CHASSIS_RESET;
} }
@@ -319,7 +350,8 @@ static void EmergencyHandler()
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停 switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停
{ {
case 0: case 0:
// 没按保持源状态不变
// 云台会先自动恢复
break; break;
default: default:
@@ -336,6 +368,7 @@ static void EmergencyHandler()
} }
} }
} }
static uint8_t can_comm_cnt = 0;
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask() void RobotCMDTask()
{ {
@@ -358,9 +391,23 @@ void RobotCMDTask()
// UI数据 // UI数据
chassis_cmd_send.friction_mode = shoot_cmd_send.friction_mode; chassis_cmd_send.friction_mode = shoot_cmd_send.friction_mode;
chassis_cmd_send.loader_mode = shoot_cmd_send.load_mode; chassis_cmd_send.loader_mode = shoot_cmd_send.load_mode;
// 临时写法,chassis_can_send只在以下行代码中使用,后续修改
// chassis_can_send.vx = chassis_cmd_send.vx;
// chassis_can_send.offset_angle = chassis_cmd_send.offset_angle;
// chassis_can_send.delta_leglen = chassis_cmd_send.delta_leglen;
// chassis_can_send.can_comm.chassis_direction_e = chassis_cmd_send.direction;
// chassis_can_send.can_comm.chassis_mode = chassis_cmd_send.chassis_mode;
// chassis_can_send.can_comm.friction_mode_e = chassis_cmd_send.friction_mode;
// chassis_can_send.can_comm.loader_mode = chassis_cmd_send.loader_mode;
// chassis_can_send.can_comm.ui_mode = chassis_cmd_send.ui_mode;
// chassis_can_send.can_comm.vision_mode = chassis_cmd_send.vision_mode;
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); can_comm_cnt++;
if (can_comm_cnt % 2 == 0)
{
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
can_comm_cnt = 0;
}
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
} }

2
application/gimbal/gimbal.c
View File

@@ -135,7 +135,7 @@ void GimbalTask()
// DJIMotorSetFeedfoward(yaw_motor,SPEED_FEEDFORWARD); // DJIMotorSetFeedfoward(yaw_motor,SPEED_FEEDFORWARD);
DJIMotorEnable(yaw_motor); DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor); DJIMotorEnable(pitch_motor);
//DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);

28
application/robot_def.h
View File

@@ -34,7 +34,7 @@
#define PITCH_MIN_ANGLE (-30.0f) // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE (-30.0f) // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
// 发射参数 // 发射参数
#define ONE_BULLET_DELTA_ANGLE 75 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define ONE_BULLET_DELTA_ANGLE 30 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f #define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 12 // 拨盘一圈的装载量 #define NUM_PER_CIRCLE 12 // 拨盘一圈的装载量
#define LOAD_RATIO 2.5f // 中心供弹需要机械减速比 #define LOAD_RATIO 2.5f // 中心供弹需要机械减速比
@@ -159,8 +159,8 @@ typedef struct
{ {
// 控制部分 // 控制部分
float vx; // 前进方向速度 float vx; // 前进方向速度
float rotate_w; // 旋转速度, 目前仅在复位模式下使用 int8_t rotate_w; // 旋转速度, 目前仅在复位模式下使用
float delta_leglen; // 腿长 int8_t delta_leglen; // 腿长
float offset_angle; // 底盘和归中位置的夹角 float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode; chassis_mode_e chassis_mode;
chassis_direction_e direction; chassis_direction_e direction;
@@ -171,8 +171,28 @@ typedef struct
vision_mode_e vision_mode; // 视觉状态 vision_mode_e vision_mode; // 视觉状态
ui_mode_e ui_mode; // UI状态 ui_mode_e ui_mode; // UI状态
loader_mode_e loader_mode; // 拨盘状态 loader_mode_e loader_mode; // 拨盘状态
} Chassis_Ctrl_Cmd_s; } Chassis_Ctrl_Cmd_s;
typedef union
{
struct // 用于访问键盘状态
{
uint16_t chassis_mode : 3;
uint16_t chassis_direction_e : 1;
uint16_t friction_mode_e : 1;
uint16_t vision_mode : 2;
uint16_t ui_mode : 1;
uint16_t loader_mode : 3;
uint16_t w_speed_buf : 2;
};
uint16_t can_com;
} Can_Comm_s;
typedef struct
{
float vx ;
int8_t delta_leglen;
int16_t offset_angle;
Can_Comm_s can_comm;
}Chassis_Can_Comm;
// cmd发布的云台控制数据,由gimbal订阅 // cmd发布的云台控制数据,由gimbal订阅
typedef struct typedef struct

8
application/shoot/shoot.c
View File

@@ -70,7 +70,7 @@ void ShootInit()
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 10,//10 .Kp = 15,//10
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.MaxOut = 20000, .MaxOut = 20000,
@@ -167,7 +167,7 @@ void ShootTask()
break; break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE; load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE*REDUCTION_RATIO_LOADER*LOAD_RATIO;
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, load_angle_set); // 控制量增加一发弹丸的角度 DJIMotorSetRef(loader, load_angle_set); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
@@ -175,11 +175,11 @@ void ShootTask()
break; break;
// 三连发,如果不需要后续可能删除 // 三连发,如果不需要后续可能删除
case LOAD_3_BULLET: case LOAD_3_BULLET:
load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * 3; load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE*REDUCTION_RATIO_LOADER*LOAD_RATIO;
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, load_angle_set); // 增加3发 DJIMotorSetRef(loader, load_angle_set); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 1800; // 完成3发弹丸发射的时间 dead_time = 1000; // 完成3发弹丸发射的时间
break; break;
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE: case LOAD_BURSTFIRE:

2
modules/can_comm/can_comm.c
View File

@@ -118,7 +118,7 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data)
send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i; send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i;
CANSetDLC(instance->can_ins, send_len); CANSetDLC(instance->can_ins, send_len);
memcpy(instance->can_ins->tx_buff, instance->raw_sendbuf + i, send_len); memcpy(instance->can_ins->tx_buff, instance->raw_sendbuf + i, send_len);
CANTransmit(instance->can_ins, 1); CANTransmit(instance->can_ins, 0.5);
} }
} }

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

@@ -254,7 +254,7 @@ void DJIMotorControl()
// pid_ref会顺次通过被启用的闭环充当数据的载体 // pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) if (motor_setting->outer_loop_type == ANGLE_LOOP)
{ {
if (motor_setting->angle_feedback_source == OTHER_FEED) if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr; pid_measure = *motor_controller->other_angle_feedback_ptr;
@@ -308,7 +308,7 @@ void DJIMotorControl()
{ {
if (sender_enable_flag[i]) if (sender_enable_flag[i])
{ {
CANTransmit(&sender_assignment[i], 1); CANTransmit(&sender_assignment[i], 0.5);
} }
} }
} }

29
modules/motor/motor_task.c
View File

@@ -8,25 +8,20 @@
#include "robot_def.h" #include "robot_def.h"
void MotorControlTask() void MotorControlTask()
{ {
// static uint8_t cnt = 0; 设定不同电机的任务频率
// if(cnt%5==0) //200hz
// if(cnt%10==0) //100hz
#ifdef GIMBAL_BOARD #ifdef GIMBAL_BOARD
static uint8_t cnt = 0;
// 设定不同电机的任务频率
if (cnt % 2 == 0) // 500hz
{
DJIMotorControl(); DJIMotorControl();
#endif // DEBUG cnt = 0;
#ifdef CHASSIS_BOARD }
LKMotorControl(); cnt++;
#endif // DEBUG
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ #endif // DEBUG
//LKMotorControl(); #ifdef CHASSIS_BOARD
LKMotorControl();
#endif // DEBUG
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
// HTMotorControl();
// 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
// StepMotorControl();
} }

2
modules/remote/remote_control.c
View File

@@ -9,7 +9,7 @@
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小 #define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
// 遥控器数据 // 遥控器数据
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断 static RC_ctrl_t rc_ctrl[2] = {0}; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
static uint8_t rc_init_flag = 0; // 遥控器初始化标志位 static uint8_t rc_init_flag = 0; // 遥控器初始化标志位
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了 // 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了