TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master'

Browse Source
This commit is contained in:
Kidenygood
2023-04-03 20:14:24 +08:00
parent ef5cdda82c f694056d08
commit 607615e6b6
52 changed files with 903 additions and 418 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
application/balance_chassis/balance.c
View File

@@ -1,4 +1,10 @@
// app
#include "balance.h"
#include "vmc_project.h"
#include "gain_table.h"
#include "robot_def.h"
#include "general_def.h"
// module
#include "HT04.h"
#include "LK9025.h"
#include "bmi088.h"
@@ -6,9 +12,8 @@
#include "super_cap.h"
#include "controller.h"
#include "can_comm.h"
// standard
#include "stdint.h"
#include "robot_def.h"
#include "general_def.h"
#include "arm_math.h" // 需要用到较多三角函数
/* 底盘拥有的模块实例 */
@@ -47,7 +52,7 @@ static float T_leg_left, T_leg_right; // 左右驱动电机力矩
*
*/
static void CalcLQR()
{
{
}
/**
@@ -55,15 +60,15 @@ static void CalcLQR()
*
*/
static void VMCProject()
{
{ // 拟将功能封装到vmc_project.h中
}
/**
* @brief 腿部角度控制:转向和抗劈叉
*
*/
static PIDInstance swerving_pid;
static PIDInstance anti_crash_pid;
static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
static void SynthesizeMotion()
{
@@ -73,15 +78,16 @@ static void SynthesizeMotion()
* @brief 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数
*
*/
static PIDInstance leg_length_pid;
static PIDInstance roll_compensate_pid;
static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
static void LegControl()
{
}
/**
* @brief 离地监测和?跳跃控制
* @brief 离地监测和?跳跃控制?
* 通过模型解算地面的支持力完成离地检测
*
*/
static void FlyDetect()
@@ -89,7 +95,7 @@ static void FlyDetect()
}
/**
* @brief 功率限制
* @brief 功率限制,一般不需要
*
*/
static void WattLimit()
@@ -242,6 +248,7 @@ void BalanceInit()
PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
}
/* balanceTask可能需要以更高频率运行,以提高线性化的精确程度 */
void BalanceTask()
{
}

3
application/balance_chassis/gain_table.h
View File

@@ -2,13 +2,14 @@
#pragma once
#include "stdint.h"
#include "stm32f407xx.h"
#include "arm_math.h"
#include "math.h"
#define GAIN_TABLE_SIZE 100 // 增益表大小
// K 2x6,6个状态变量2个输出(Tp关节电机和T驱动轮电机)
static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {};
static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {0};
static interpolation_flag = 0; // 插值方式:1 线性插值 0 关闭插值

14
application/balance_chassis/vmc_project.h Normal file
View File

@@ -0,0 +1,14 @@
#ifndef VMC_PROJECT_H
#define VMC_PROJECT_H
#include "stm32f407xx.h"
#include "arm_math.h"
#include "math.h"
#include "general_def.h"
// 将五连杆和直杆的vmc映射放在此处,方便修改和调试,balance.c不会太长
#endif // !VMC_PROJECT_H

43
application/chassis/chassis.c
View File

@@ -17,8 +17,9 @@
#include "super_cap.h"
#include "message_center.h"
// referee需要移动到module层
/////////////////////////
#include "referee.h"
#include "rm_referee.h"
/////////////////////////
@@ -67,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 = 10,//4.5
.Ki = 0,//0
.Kd = 0,//0
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.MaxOut = 12000,
},
.current_PID = {
.Kp = 1,
.Ki = 0,
.Kp = 0.5,//0.4
.Ki = 0,//0
.Kd = 0,
.MaxOut = 4000,
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.MaxOut = 15000,
},
},
.controller_setting_init_config = {
@@ -88,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); // 裁判系统初始化
@@ -152,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;
}
@@ -219,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;

144
application/cmd/robot_cmd.c
View File

@@ -1,5 +1,7 @@
// app
#include "robot_def.h"
#include "robot_cmd.h"
// module
#include "remote_control.h"
#include "ins_task.h"
#include "master_process.h"
@@ -19,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; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
@@ -66,6 +68,7 @@ void RobotCMDInit()
};
cmd_can_comm = CANCommInit(&comm_conf);
#endif // GIMBAL_BOARD
gimbal_cmd_send.pitch = 0;
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
}
@@ -78,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)
@@ -105,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)) // 左侧开关状态为[中],视觉模式
@@ -118,9 +127,17 @@ 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;
// 摇杆控制的软件限位
// if (gimbal_cmd_send.pitch <= PITCH_MIN_ECD)
// {
// gimbal_cmd_send.pitch = PITCH_MIN_ECD;
// }
// else if (gimbal_cmd_send.pitch >= PITCH_MAX_ECD)
// {
// gimbal_cmd_send.pitch = PITCH_MAX_ECD;
// }
}
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
@@ -129,7 +146,7 @@ static void RemoteControlSet()
// 发射参数
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
; // 弹舱舵机控制,待添加servo_motor模块,开启
; // 弹舱舵机控制,待添加servo_motor模块,开启
else
; // 弹舱舵机控制,待添加servo_motor模块,关闭
@@ -138,13 +155,13 @@ static void RemoteControlSet()
shoot_cmd_send.friction_mode = FRICTION_ON;
else
shoot_cmd_send.friction_mode = FRICTION_OFF;
// 拨弹控制,目前固定为连发
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
if (rc_data[TEMP].rc.dial < -500)
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
else
shoot_cmd_send.load_mode = LOAD_STOP;
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
shoot_cmd_send.shoot_rate = 1;
shoot_cmd_send.shoot_rate = 8;
}
/**
@@ -153,14 +170,89 @@ static void RemoteControlSet()
*/
static void MouseKeySet()
{
// 待添加键鼠控制
// ...
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
{
case 0:
shoot_cmd_send.bullet_speed = 15;
break;
case 1:
shoot_cmd_send.bullet_speed = 18;
break;
default:
shoot_cmd_send.bullet_speed = 30;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
{
case 0:
shoot_cmd_send.load_mode = LOAD_STOP;
break;
case 1:
shoot_cmd_send.load_mode = LOAD_1_BULLET;
break;
case 2:
shoot_cmd_send.load_mode = LOAD_3_BULLET;
break;
default:
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
{
case 0:
shoot_cmd_send.lid_mode = LID_OPEN;
break;
default:
shoot_cmd_send.lid_mode = LID_CLOSE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
{
case 0:
shoot_cmd_send.friction_mode = FRICTION_OFF;
break;
default:
shoot_cmd_send.friction_mode = FRICTION_ON;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
{
case 0:
chassis_cmd_send.chassis_speed_buff = 40;
break;
case 1:
chassis_cmd_send.chassis_speed_buff = 60;
break;
case 2:
chassis_cmd_send.chassis_speed_buff = 80;
break;
default:
chassis_cmd_send.chassis_speed_buff = 100;
break;
}
switch (rc_data[TEMP].key[KEY_PRESS].shift) //待添加 按shift允许超功率 消耗缓冲能量
{
case 1:
break;
default:
break;
}
}
/**
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
* 停止的阈值'300'待修改成合适的值,或改为开关控制.
*
*
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
*
*/
@@ -173,13 +265,31 @@ static void EmergencyHandler()
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
}
// 遥控器右侧开关为[上],恢复正常运行
if (switch_is_up(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_READY;
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
}
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C]%2) //ctrl+c 进入急停
{
case 0:
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
break;
default:
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;
shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
break;
}
}
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
@@ -197,7 +307,6 @@ void RobotCMDTask()
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle();
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
RemoteControlSet();
@@ -207,11 +316,12 @@ 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;
;
// 推送消息,双板通信,视觉通信等
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置

128
application/gimbal/gimbal.c
View File

@@ -20,42 +20,42 @@ BMI088Instance* imu;
void GimbalInit()
{
BMI088_Init_Config_s imu_config = {
.spi_acc_config={
.GPIOx=CS1_ACCEL_GPIO_Port,
.cs_pin=CS1_ACCEL_Pin,
.spi_handle=&hspi1,
.spi_acc_config = {
.GPIOx = CS1_ACCEL_GPIO_Port,
.cs_pin = CS1_ACCEL_Pin,
.spi_handle = &hspi1,
},
.spi_gyro_config={
.GPIOx=CS1_GYRO_GPIO_Port,
.cs_pin=CS1_GYRO_Pin,
.spi_handle=&hspi1,
.spi_gyro_config = {
.GPIOx = CS1_GYRO_GPIO_Port,
.cs_pin = CS1_GYRO_Pin,
.spi_handle = &hspi1,
},
.acc_int_config={
.exti_mode=EXTI_TRIGGER_FALLING,
.GPIO_Pin=INT_ACC_Pin,
.GPIOx=INT_ACC_GPIO_Port,
.acc_int_config = {
.exti_mode = EXTI_TRIGGER_FALLING,
.GPIO_Pin = INT_ACC_Pin,
.GPIOx = INT_ACC_GPIO_Port,
},
.gyro_int_config={
.exti_mode=EXTI_TRIGGER_FALLING,
.GPIO_Pin=INT_GYRO_Pin,
.GPIOx=INT_GYRO_GPIO_Port,
.gyro_int_config = {
.exti_mode = EXTI_TRIGGER_FALLING,
.GPIO_Pin = INT_GYRO_Pin,
.GPIOx = INT_GYRO_GPIO_Port,
},
.heat_pid_config={
.Kp=0.0f,
.Kd=0.0f,
.Ki=0.0f,
.MaxOut=0.0f,
.DeadBand=0.0f,
.heat_pid_config = {
.Kp = 0.0f,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 0.0f,
.DeadBand = 0.0f,
},
.heat_pwm_config={
.channel=TIM_CHANNEL_1,
.htim=&htim1,
.heat_pwm_config = {
.channel = TIM_CHANNEL_1,
.htim = &htim1,
},
.cali_mode=BMI088_CALIBRATE_ONLINE_MODE,
.work_mode=BMI088_BLOCK_PERIODIC_MODE,
.cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
.work_mode = BMI088_BLOCK_PERIODIC_MODE,
};
// imu=BMI088Register(&imu_config);
// gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
Motor_Init_Config_s yaw_config = {
.can_init_config = {
@@ -64,60 +64,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,//50
.Ki = 200,//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
.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,
};
@@ -128,7 +136,7 @@ void GimbalInit()
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}
int aaaaaaa;
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
@@ -136,6 +144,7 @@ void GimbalTask()
// 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
switch (gimbal_cmd_recv.gimbal_mode)
{
@@ -159,16 +168,23 @@ 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;
default:
break;
}
// if(yaw_motor->motor_measure.total_angle>120)
// {
// aaaaaaa++;
// }
// 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ...
// 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;

3
application/referee/referee.md Normal file
View File

@@ -0,0 +1,3 @@
# referee
需要将此模块移动到module层，并新建一个rtos任务，以一定频率运行，用于ui刷新和多机通信

2
application/robot.c
View File

@@ -35,7 +35,7 @@ void RobotInit()
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
Referee_Interactive_init();
// Referee_Interactive_init();
ChassisInit();
#endif
// 初始化完成,开启中断

19
application/robot_def.h
View File

@@ -34,21 +34,27 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.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 PITCH_MAX_ANGLE 0 //云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE 0 //云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
// 发射参数
#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)) || \
@@ -128,6 +134,7 @@ typedef enum
// 功率限制,从裁判系统获取
typedef struct
{ // 功率控制
float chassis_power_mx;
} Chassis_Power_Data_s;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
@@ -144,7 +151,7 @@ typedef struct
float wz; // 旋转速度
float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode;
int chassis_speed_buff;
// UI部分
// ...

57
application/shoot/shoot.c
View File

@@ -1,5 +1,6 @@
#include "shoot.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "message_center.h"
#include "bsp_dwt.h"
@@ -25,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 = {
@@ -47,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
};
@@ -119,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)
@@ -181,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;
}
}