24对抗赛

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

View File

@@ -8,7 +8,11 @@
"user_lib.h": "c",
"servo_motor.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"
}

View File

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

View File

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

View File

@@ -14,11 +14,13 @@
// bsp
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "string.h"
static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
static Chassis_Can_Comm chassis_can_send;
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
@@ -120,16 +122,17 @@ static void CalcOffsetAngle()
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
*
*/
int aaaa;
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.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
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);
@@ -152,16 +155,17 @@ static void RemoteControlSet()
// 右侧开关状态为[下],底盘旋转
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;
if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
chassis_cmd_send.rotate_w = 2;
}
if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
chassis_cmd_send.rotate_w = -2;
}
chassis_cmd_send.vx = 0;
}
@@ -174,7 +178,7 @@ static void RemoteControlSet()
// 左侧开关状态为[中],腿长控制
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.shoot_rate = 10;
shoot_cmd_send.shoot_rate = 15;
}
/**
@@ -203,14 +207,14 @@ static void RemoteControlSet()
*/
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.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);
}
@@ -219,9 +223,10 @@ static void MouseKeySet()
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;
}
@@ -229,7 +234,18 @@ static void MouseKeySet()
{
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:
break;
@@ -237,6 +253,7 @@ static void MouseKeySet()
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮
{
case 0:
@@ -249,8 +266,18 @@ static void MouseKeySet()
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;
break;
case 1:
shoot_cmd_send.load_mode = LOAD_1_BULLET;
break;
}
if (rc_data[TEMP].mouse.press_l) // 鼠标左键发射
{
}
else
shoot_cmd_send.load_mode = LOAD_STOP;
}
@@ -267,17 +294,21 @@ static void MouseKeySet()
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.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.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;
}
@@ -319,7 +350,8 @@ static void EmergencyHandler()
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停
{
case 0:
// 没按保持源状态不变
// 云台会先自动恢复
break;
default:
@@ -336,6 +368,7 @@ static void EmergencyHandler()
}
}
}
static uint8_t can_comm_cnt = 0;
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask()
{
@@ -358,9 +391,23 @@ void RobotCMDTask()
// UI数据
chassis_cmd_send.friction_mode = shoot_cmd_send.friction_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(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
}

View File

@@ -135,7 +135,7 @@ void GimbalTask()
// DJIMotorSetFeedfoward(yaw_motor,SPEED_FEEDFORWARD);
DJIMotorEnable(yaw_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(pitch_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);

View File

@@ -34,7 +34,7 @@
#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 NUM_PER_CIRCLE 12 // 拨盘一圈的装载量
#define LOAD_RATIO 2.5f // 中心供弹需要机械减速比
@@ -159,8 +159,8 @@ typedef struct
{
// 控制部分
float vx; // 前进方向速度
float rotate_w; // 旋转速度, 目前仅在复位模式下使用
float delta_leglen; // 腿长
int8_t rotate_w; // 旋转速度, 目前仅在复位模式下使用
int8_t delta_leglen; // 腿长
float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode;
chassis_direction_e direction;
@@ -171,8 +171,28 @@ typedef struct
vision_mode_e vision_mode; // 视觉状态
ui_mode_e ui_mode; // UI状态
loader_mode_e loader_mode; // 拨盘状态
} 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订阅
typedef struct

View File

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

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;
CANSetDLC(instance->can_ins, 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);
}
}

View File

@@ -254,7 +254,7 @@ void DJIMotorControl()
// 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)
pid_measure = *motor_controller->other_angle_feedback_ptr;
@@ -308,7 +308,7 @@ void DJIMotorControl()
{
if (sender_enable_flag[i])
{
CANTransmit(&sender_assignment[i], 1);
CANTransmit(&sender_assignment[i], 0.5);
}
}
}

View File

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

View File

@@ -9,7 +9,7 @@
#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; // 遥控器初始化标志位
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了