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

碳板步兵云台yaw轴调试完成

This commit is contained in:
宋家齐 2023-10-12 19:29:48 +08:00
parent fc6f91ac9a
commit e25cbdf2a3
5 changed files with 75 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
application/chassis/chassis.c
View File

@ -252,10 +252,10 @@ void ChassisTask()
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
LimitChassisOutput();
float vofa_send_data[2];
vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
vofa_justfloat_output(vofa_send_data,8,&huart1);
// float vofa_send_data[2];
// vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
// vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
// vofa_justfloat_output(vofa_send_data,8,&huart1);
// 根据电机的反馈速度和IMU(如果有)计算真实速度
EstimateSpeed();

25
application/gimbal/gimbal.c
View File

@ -6,6 +6,7 @@
#include "general_def.h"
#include "bmi088.h"
#include "vofa.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor;
@ -26,7 +27,7 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 8, // 8
.Kp = 0.5, // 8
.Ki = 0,
.Kd = 0,
.DeadBand = 0.1,
@ -36,8 +37,8 @@ void GimbalInit()
.MaxOut = 500,
},
.speed_PID = {
.Kp = 50, // 50
.Ki = 200, // 200
.Kp = 12000, // 50
.Ki = 3000, // 200
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
@ -51,7 +52,7 @@ void GimbalInit()
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020};
@ -59,7 +60,7 @@ void GimbalInit()
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 2,
.tx_id = 4,
},
.controller_param_init_config = {
.angle_PID = {
@ -93,7 +94,7 @@ void GimbalInit()
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
pitch_motor = DJIMotorInit(&pitch_config);
// pitch_motor = DJIMotorInit(&pitch_config);
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
@ -124,7 +125,7 @@ void GimbalTask()
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);
// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
@ -135,7 +136,7 @@ void GimbalTask()
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);
// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break;
default:
break;
@ -145,6 +146,14 @@ void GimbalTask()
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ...
float vofa_send_data[4];
vofa_send_data[0]=yaw_motor->motor_controller.speed_PID.Ref;
vofa_send_data[1]=yaw_motor->motor_controller.speed_PID.Measure;
vofa_send_data[2]=yaw_motor->motor_controller.angle_PID.Ref;
vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure;
vofa_justfloat_output(vofa_send_data,16,&huart1);
// 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;

4
application/robot.c
View File

@ -32,7 +32,7 @@ void RobotInit()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit();
//暂时注释云台和发射任务 调试底盘
// GimbalInit();
GimbalInit();
// ShootInit();
#endif
@ -51,7 +51,7 @@ void RobotTask()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDTask();
//暂时注释云台和发射任务 调试底盘
// GimbalTask();
GimbalTask();
// ShootTask();
#endif

55
modules/imu/ins_task.c
View File

@ -20,13 +20,19 @@
#include "general_def.h"
#include "master_process.h"
#include "vofa.h"
static INS_t INS;
static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0};
const float xb[3] = {1, 0, 0};
//const float xb[3] = {1, 0, 0};
//const float yb[3] = {0, 1, 0};
//const float zb[3] = {0, 0, 1};
const float xb[3] = {0, 0, 1};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
const float zb[3] = {-1, 0, 0};
// 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0;
@ -60,9 +66,9 @@ static void InitQuaternion(float *init_q4)
for (uint8_t i = 0; i < 100; ++i)
{
BMI088_Read(&BMI088);
acc_init[X] += BMI088.Accel[X];
acc_init[X] += -BMI088.Accel[2];
acc_init[Y] += BMI088.Accel[Y];
acc_init[Z] += BMI088.Accel[Z];
acc_init[Z] += BMI088.Accel[0];
DWT_Delay(0.001);
}
for (uint8_t i = 0; i < 3; ++i)
@ -72,6 +78,9 @@ static void InitQuaternion(float *init_q4)
float angle = acosf(Dot3d(acc_init, gravity_norm));
Cross3d(acc_init, gravity_norm, axis_rot);
Norm3d(axis_rot);
//q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
//其中a表示旋转角度，(x,y,z)表示旋转轴 计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
init_q4[0] = cosf(angle / 2.0f);
for (uint8_t i = 0; i < 2; ++i)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
@ -97,7 +106,7 @@ attitude_t *INS_Init(void)
IMU_Param.flag = 1;
float init_quaternion[4] = {0};
InitQuaternion(init_quaternion);
InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
// imu heat init
PID_Init_Config_s config = {.MaxOut = 2000,
@ -129,12 +138,12 @@ void INS_Task(void)
{
BMI088_Read(&BMI088);
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
INS.Accel[X] = -BMI088.Accel[2];
INS.Accel[Y] = BMI088.Accel[1];
INS.Accel[Z] = BMI088.Accel[0];
INS.Gyro[X] = -BMI088.Gyro[2];
INS.Gyro[Y] = BMI088.Gyro[1];
INS.Gyro[Z] = BMI088.Gyro[0];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
@ -162,11 +171,35 @@ void INS_Task(void)
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
// // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
// if (INS.Yaw - INS.YawAngleLast > 180.0f)
// {
// INS.YawRoundCount--;
// }
// else if (INS.Yaw - INS.YawAngleLast < -180.0f)
// {
// INS.YawRoundCount++;
// }
// INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
// float vofa_send_data[9];
// vofa_send_data[0]=INS.Yaw;
// vofa_send_data[1]=INS.Pitch;
// vofa_send_data[2]=INS.Roll;
// vofa_send_data[3]=INS.Gyro[X];
// vofa_send_data[4]=INS.Gyro[Y];
// vofa_send_data[5]=INS.Gyro[Z];
// vofa_send_data[6]=BMI088.Accel[X];
// vofa_send_data[7]=BMI088.Accel[Y];
// vofa_send_data[8]=BMI088.Accel[Z];
// vofa_justfloat_output(vofa_send_data,36,&huart1);
VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
}

8
modules/imu/ins_task.h
View File

@ -23,6 +23,10 @@
#define Y 1
#define Z 2
//#define X 2
//#define Y 1
//#define Z 0
#define INS_TASK_PERIOD 1
typedef struct
@ -34,6 +38,7 @@ typedef struct
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct
@ -63,6 +68,9 @@ typedef struct
float Yaw;
float YawTotalAngle;
float YawAngleLast;
float YawRoundCount;
uint8_t init;
} INS_t;