HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

优化了点击pid计算流程，修复了bsp未初始化的异常，修复了数个未定义和隐式声明的警告

This commit is contained in:
NeoZng 2022-12-09 18:25:35 +08:00
parent 02b3af15c7
commit 37c23ddb79
16 changed files with 104 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -51,4 +51,4 @@ build
./idea
.vscode/.cortex-debug.peripherals.state.json
.vscode/.cortex-debug.registers.state.json
*.jdebug
*.jdebug*

1
.vscode/settings.json vendored
View File

@ -39,6 +39,7 @@
"general_def.h": "c",
"super_cap.h": "c",
"motor_def.h": "c",
"quaternionekf.h": "c"
},
"C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
}

2
HAL_N_Middlewares/Src/freertos.c
View File

@ -186,7 +186,7 @@ void StartDAEMONTASK(void const * argument)
{
while (1)
{
//500Hz
//100Hz
DaemonTask();
osDelay(10);
}

2
HAL_N_Middlewares/Src/main.c
View File

@ -33,7 +33,6 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "robot.h"
#include "message_center.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -114,7 +113,6 @@ int main(void)
RobotInit();
/* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */

41
application/chassis/chassis.c
View File

@ -59,17 +59,19 @@ void ChassisInit()
{
// 四个轮子的参数一样,改tx_id和反转标志位即可
Motor_Init_Config_s chassis_motor_config = {
.can_init_config.can_handle=&hcan1,
.can_init_config.can_handle = &hcan1,
.controller_param_init_config = {
.speed_PID = {
.Kp=10,
.Ki=0,
.Kd=0,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
.current_PID = {
.Kp=10,
.Ki=0,
.Kd=0,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
},
.controller_setting_init_config = {
@ -78,26 +80,27 @@ void ChassisInit()
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
},
.motor_type = M3508};
chassis_motor_config.can_init_config.tx_id=1;
chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
.motor_type = M3508,
};
chassis_motor_config.can_init_config.tx_id = 1;
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lf = DJIMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id=2,
chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
chassis_motor_config.can_init_config.tx_id = 2,
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rf = DJIMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id=3,
chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
chassis_motor_config.can_init_config.tx_id = 3,
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lb = DJIMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id=4,
chassis_motor_config.controller_setting_init_config.reverse_flag=MOTOR_DIRECTION_REVERSE;
chassis_motor_config.can_init_config.tx_id = 4,
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rb = DJIMotorInit(&chassis_motor_config);
referee_data = RefereeInit(&huart6);
SuperCap_Init_Config_s cap_conf = {
.can_config = {
.can_handle = &hcan2,

6
application/cmd/robot_cmd.c
View File

@ -99,8 +99,8 @@ static void RemoteControlSet()
if (switch_is_down(rc_data[TEMP].rc.s[1]) || vision_recv_data->target_state == NO_TARGET)
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.joystick[2];
gimbal_cmd_send.pitch += 0.0025f * (float)rc_data[TEMP].rc.joystick[3];
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.joystick[3];
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
}
// 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整
@ -145,7 +145,7 @@ static void EmergencyHandler()
robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
shoot_cmd_send.load_mode = SHOOT_STOP;
shoot_cmd_send.shoot_mode = SHOOT_OFF;
return;
}
// if(rc_data[TEMP].rc.joystick[4]<-300 && 各个模块正常)

32
application/gimbal/gimbal.c
View File

@ -16,24 +16,27 @@ static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
void GimbalInit()
{
Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
Motor_Init_Config_s yaw_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 2,
.tx_id = 1,
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 1,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 2000,
.DeadBand=0.3,
},
.speed_PID = {
.Kd = 1,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 2000,
},
.other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针
@ -44,25 +47,28 @@ void GimbalInit()
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.reverse_flag = MOTOR_DIRECTION_REVERSE,
.reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020};
// PITCH
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 1,
.tx_id = 3,
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
},
.speed_PID = {
.Kd = 1,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 2000,
.DeadBand=0.3,
},
.speed_PID = {
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 2000,
},
.other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针
@ -73,7 +79,7 @@ void GimbalInit()
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.reverse_flag = MOTOR_DIRECTION_REVERSE,
.reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020,
};

2
application/robot.c
View File

@ -1,3 +1,4 @@
#include "bsp_init.h"
#include "robot.h"
#include "robot_def.h"
@ -12,6 +13,7 @@
void RobotInit()
{
BSPInit();
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
ChassisInit();
#endif

8
application/robot_def.h
View File

@ -88,8 +88,14 @@ typedef enum
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
} gimbal_mode_e;
// 发射模式设置
typedef enum
{
SHOOT_ON,
SHOOT_OFF,
}shoot_mode_e;
typedef enum
{
FRICTION_OFF, // 摩擦轮关闭
FRICTION_ON, // 摩擦轮开启
@ -103,7 +109,6 @@ typedef enum
typedef enum
{
SHOOT_STOP, // 停止整个发射模块,后续可能隔离出来
LOAD_STOP, // 停止发射
LOAD_REVERSE, // 反转
LOAD_1_BULLET, // 单发
@ -150,6 +155,7 @@ typedef struct
// cmd发布的发射控制数据,由shoot订阅
typedef struct
{
shoot_mode_e shoot_mode;
loader_mode_e load_mode;
lid_mode_e lid_mode;
friction_mode_e friction_mode;

53
application/shoot/shoot.c
View File

@ -29,14 +29,16 @@ void ShootInit()
},
.controller_param_init_config = {
.speed_PID = {
.Kp=10,
.Ki=0,
.Kd=0,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
.current_PID = {
.Kp=10,
.Ki=0,
.Kd=0,
.Kp = 10,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
},
.controller_setting_init_config = {
@ -56,14 +58,16 @@ void ShootInit()
},
.controller_param_init_config = {
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
},
.controller_setting_init_config = {
@ -83,19 +87,22 @@ void ShootInit()
.controller_param_init_config = {
.angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kd = 10,
.Kp = 10,
.Ki = 1,
.Kd = 2,
.MaxOut = 200,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
},
.controller_setting_init_config = {
@ -121,7 +128,7 @@ void ShootTask()
SubGetMessage(shoot_sub, &shoot_cmd_recv);
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机
if (shoot_cmd_recv.load_mode == SHOOT_STOP)
if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
{
DJIMotorStop(friction_l);
DJIMotorStop(friction_r);
@ -175,15 +182,15 @@ void ShootTask()
{
case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
default:
break;

3
modules/algorithm/controller.c
View File

@ -29,10 +29,11 @@ static void f_PID_ErrorHandle(PIDInstance *pid); // 堵转保护
*/
void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
{
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_config_s));
// set rest of memory to 0
memset(&pid->Measure, 0, sizeof(PIDInstance) - sizeof(PID_Init_config_s));
}
/**

1
modules/daemon/daemon.c
View File

@ -1,5 +1,6 @@
#include "daemon.h"
#include "bsp_dwt.h" // 后续通过定时器来计时?
#include "stdlib.h"
static DaemonInstance *daemon_instances[DAEMON_MX_CNT] = {NULL};
static uint8_t idx;

2
modules/general_def.h
View File

@ -1,7 +1,9 @@
#ifndef GENERAL_DEF_H
#define GENERAL_DEF_H
#ifndef PI
#define PI 3.1415926535f
#endif // !PI
#define PI2 (PI * 2.0f) // 2 pi
#define RAD_2_ANGLE (180.0f / PI)

1
modules/imu/ins_task.c
View File

@ -68,6 +68,7 @@ attitude_t *INS_Init(void)
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
return (attitude_t*)&INS.Roll;
}
/* 注意以1kHz的频率运行此任务 */

11
modules/motor/dji_motor.c
View File

@ -223,7 +223,7 @@ void DJIMotorControl()
static Motor_Control_Setting_s *motor_setting;
static Motor_Controller_s *motor_controller;
static DJI_Motor_Measure_s *motor_measure;
static float pid_measure;
static float pid_measure,pid_ref;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; i++)
{
@ -233,6 +233,7 @@ void DJIMotorControl()
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
pid_ref=motor_controller->pid_ref; //保存设定值,防止motor_controller->pid_ref在计算过程中被修改
// pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
@ -243,7 +244,7 @@ void DJIMotorControl()
else // MOTOR_FEED
pid_measure = motor_measure->total_angle; // 对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环
motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref);
pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, pid_ref);
}
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
@ -254,17 +255,17 @@ void DJIMotorControl()
else // MOTOR_FEED
pid_measure = motor_measure->speed_angle_per_sec;
// 更新pid_ref进入下一个环
motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, pid_ref);
}
// 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref);
pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, pid_ref);
}
// 获取最终输出
set = (int16_t)motor_controller->pid_ref;
set = (int16_t)pid_ref;
if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1; // 设置反转

12
modules/remote/remote_control.c
View File

@ -19,11 +19,11 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
{
memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据
// 摇杆,直接解算时减去偏置
rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0
rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1
rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3
rc_ctrl[TEMP].rc.joystick[4] = (sbus_buf[16] | (sbus_buf[17] << 8)) - RC_CH_VALUE_OFFSET; // 左侧拨轮
rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 0
rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 1
rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2
rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 3
rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET; // 左侧拨轮
// 开关,0左1右
rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
@ -81,5 +81,5 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
conf.usart_handle = rc_usart_handle;
conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
rc_usart_instance = USARTRegister(&conf);
return &rc_ctrl;
return (RC_ctrl_t*)&rc_ctrl;
}