127 lines
3.0 KiB
C
127 lines
3.0 KiB
C
#include "dji.h"
|
||
#include "TopDefine.h"
|
||
#include "remote_control.h"
|
||
#include "calc_lib.h"
|
||
|
||
//可以在此完成电机pid初始化，以及电机的控制
|
||
//然后再去task里发送电机控制指令
|
||
//应该也可以在此发送电机指令
|
||
extern RC_mess_t RC_mess;
|
||
|
||
//编码器数据
|
||
const motor_measure_t *motor_3508_data;//3508电机数据
|
||
|
||
pid_type_def speed_pid;//3508速度环pid结构体
|
||
pid_type_def angle_pid;//3508位置环pid结构体
|
||
|
||
//pid
|
||
|
||
fp32 M3508_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环)
|
||
fp32 M3508_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环)
|
||
//速度环pid参数
|
||
fp32 M3508_PID[3]={4.9,0.01,0.0};
|
||
|
||
int16_t result; //速度环
|
||
float angleSet[MOTOR_NUM]; //位置环
|
||
|
||
//trigger
|
||
//编码器数据
|
||
const motor_measure_t *trigger_motor_data;//3508电机数据
|
||
|
||
|
||
pid_type_def t_speed_pid;//2006速度环pid结构体
|
||
pid_type_def t_angle_pid;//2006位置环pid结构体
|
||
|
||
fp32 M2006_speed_PID[3]={15.0,0.3,0.0}; //P,I,D(速度环)
|
||
fp32 M2006_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环)
|
||
//速度环pid参数
|
||
fp32 M2006_PID[3]={4.9,0.01,0.0};
|
||
|
||
int16_t t_result; //速度环 //can最后发送的数据
|
||
float t_angleSet[1]; //位置环
|
||
|
||
//GM6020电机数据
|
||
const motor_measure_t *GM6020_motor_data;//3508电机数据
|
||
|
||
//6020pid
|
||
pid_type_def speed_6020_pid;
|
||
pid_type_def angle_6020_pid;
|
||
|
||
const fp32 PID_6020_angle[3] = {50, 0.1, 0};
|
||
const fp32 PID_6020_speed[3]={5,0.01,0};
|
||
|
||
|
||
|
||
void motor_init(void)
|
||
{
|
||
// motor_3508_data=get_motor_point(0);
|
||
trigger_motor_data=get_motor_point(0);
|
||
GM6020_motor_data=get_6020_motor_point(0);
|
||
|
||
|
||
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000);
|
||
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000);
|
||
|
||
PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,500000,500000);
|
||
PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000);
|
||
|
||
|
||
PID_init(&speed_6020_pid,PID_POSITION,PID_6020_speed,15000,15000);//6020 pid初始化
|
||
PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,600,500);
|
||
|
||
|
||
}
|
||
|
||
float trigger_angle = 0;
|
||
void trigger_pos(fp32 angle)
|
||
{
|
||
|
||
int16_t delta[1];
|
||
|
||
delta[0]=PID_calc(&t_angle_pid,trigger_motor_data->total_angle,angle);
|
||
t_result=PID_calc(&t_speed_pid,trigger_motor_data->speed_rpm,delta[0]);
|
||
|
||
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
||
osDelay(2);
|
||
|
||
}
|
||
|
||
|
||
void motor_speed(fp32 speed)
|
||
{
|
||
|
||
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,speed);
|
||
|
||
}
|
||
|
||
void motor_pos(fp32 angle)
|
||
{
|
||
int16_t delta[1];
|
||
|
||
delta[0]=PID_calc(&angle_pid,motor_3508_data->total_angle,angle);
|
||
result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,delta[0]);
|
||
|
||
}
|
||
|
||
//6020控制
|
||
void my_GM6020_control(fp32 angle)
|
||
{
|
||
|
||
fp32 angle_get;
|
||
fp32 my_angle;
|
||
int32_t M6020_speed;
|
||
|
||
//6020串级
|
||
angle_get= angle;
|
||
|
||
//外环角度环
|
||
my_angle=PID_calc(&angle_6020_pid,GM6020_motor_data->total_angle,angle_get);//角度环
|
||
//内环速度环
|
||
M6020_speed=PID_calc(&speed_6020_pid,GM6020_motor_data->speed_rpm, my_angle);//速度环
|
||
|
||
CAN_cmd_1FF(M6020_speed, M6020_speed, M6020_speed, M6020_speed,&hcan1);
|
||
osDelay(2);
|
||
|
||
}
|
||
|