250 lines
8.0 KiB
C
250 lines
8.0 KiB
C
/**
|
||
****************************(C) COPYRIGHT 2021 QUT****************************
|
||
* @file motor_control.c/h
|
||
* @brief 3508/2006电机初始化、速度环、位置环；6020电机初始化、位置环（相对角度）
|
||
* @note 3508位置环使用的是增量式编码器，没有绝对位置，注意它所对应的角度是M3508的输出轴所对应的角度，由减速比经过换算得来，
|
||
* 若要修改对于的真实角度，应该在CAN_RECEIVE中修改相对应的解码函数。
|
||
同时3508的控制函数也可对应相同ID的2006配套使用（但注意相对应的角度和速度值可能会出现问题）需要修改相应的减速比
|
||
* @history
|
||
* Version Date Author Modification remark
|
||
* V1.0.0 Dec-23-2021 QUT 1. 完成
|
||
* V1.0.1 Dec-23-2023 QUT hks 1. 完成 仅仅对原先函数进行了少量修改，增添了部分注释，未对部分参数进行适配
|
||
|
||
@verbatim
|
||
==============================================================================
|
||
|
||
==============================================================================
|
||
@endverbatim
|
||
****************************(C) COPYRIGHT 2021 QUT****************************
|
||
*/
|
||
#include "motor_control.h"
|
||
#include "CAN_receive.h"
|
||
#include "pid.h"
|
||
#include "user_math.h"
|
||
#include "device\device.h"
|
||
|
||
//3508motor init ID1~4
|
||
//mode 1:velocity
|
||
// 2:pos
|
||
void M3508_motor_init(int mode);
|
||
//3508velocity_loop ID1~4
|
||
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
|
||
//3508position_loop ID1~4
|
||
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
|
||
//6020 init ID1
|
||
void GM6020_position_loop_init(void);
|
||
//6020 pos_loop ID1 relative angle ,this is used for 6020 angel range from 0 to 180
|
||
void GM6020_pos_loop(int32_t ecd);
|
||
|
||
|
||
|
||
const motor_measure_t *motor_data[4];//3508 motor _data
|
||
pid_type_def motor_pid[4];//3508velocity_pid
|
||
pid_type_def angle_pid[4];//3508position_pid
|
||
//position_pid time interval 10ms
|
||
const fp32 speed_PID[3]={5.0,0.3,0.0}; //P,I,D(velocity)
|
||
const fp32 angle_PID[3]={25.0,0.0,1.5}; //P,I,D(position)
|
||
//velocity_pid
|
||
const fp32 PID[3]={5.0,0.01,0.0};
|
||
|
||
const motor_measure_t *GM6020_data;//6020 data_motor
|
||
gimbal_PID_t gimbal_motor_relative_angle_pid;//6020 relative angle position_pid
|
||
pid_type_def gimbal_motor_gyro_pid;//6020 relative angle velocity_pid
|
||
gimbal_pid_control_t GM6020_pos_loop_control;//6020 position_control
|
||
const fp32 GM6020_PID[3]={3600.0,20.0,0};//6020 velocity_PID
|
||
|
||
//*****3508*****//
|
||
//3508 motor init ID1~4
|
||
//mode 1:velocity
|
||
// 2:position
|
||
void M3508_motor_init(int mode)
|
||
{
|
||
//get 3508 motor_data from CAN_receive
|
||
motor_data[0] = get_chassis_motor_measure_point(0);
|
||
motor_data[1] = get_chassis_motor_measure_point(1);
|
||
motor_data[2] = get_chassis_motor_measure_point(2);
|
||
motor_data[3] = get_chassis_motor_measure_point(3);
|
||
|
||
switch(mode)
|
||
{
|
||
//velocity_init
|
||
case 1:
|
||
for(int i=0;i<4;i++)
|
||
{
|
||
PID_init(&motor_pid[i],PID_POSITION,PID,16000,2000); //velocity_pid init
|
||
}break;
|
||
//pos_init
|
||
case 2:
|
||
for(int i=0;i<4;i++)
|
||
{
|
||
PID_init(&motor_pid[i],PID_POSITION,speed_PID,16000,2000); //velocity_pid init
|
||
PID_init(&angle_pid[i],PID_POSITION,angle_PID,1000,200); // pos_pid init
|
||
}break;
|
||
}
|
||
|
||
}
|
||
|
||
//3508 velocity
|
||
/**
|
||
* @brief send control current of motor (0x201, 0x202, 0x203, 0x204)
|
||
* the param is compatable with the fab data of 3508
|
||
* @param[in] speed1: (0x201) 3508 motor control current, range [-16384,16384]
|
||
* @param[in] speed2: (0x202) 3508 motor control current, range [-16384,16384]
|
||
* @param[in] speed3: (0x203) 3508 motor control current, range [-16384,16384]
|
||
* @param[in] speed4: (0x204) 3508 motor control current, range [-16384,16384]
|
||
* @retval none
|
||
*/
|
||
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4)
|
||
{
|
||
int32_t speed[4];
|
||
int32_t delta_speed[4];
|
||
int i;
|
||
|
||
speed[0] = speed1;
|
||
speed[1] = speed2;
|
||
speed[2] = speed3;
|
||
speed[3] = speed4;
|
||
|
||
for(i=0;i<4;i++)
|
||
{
|
||
delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,speed[i]);
|
||
}
|
||
|
||
CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]);
|
||
}
|
||
|
||
//3508 pos
|
||
/**
|
||
* @brief send control current of motor (0x201, 0x202, 0x203, 0x204)
|
||
* this also can used to control the same ID of M2006
|
||
* the param angle is correspond to the output shaft of M3508
|
||
|
||
* @Attention!!!!! the kind fo DJI ecd is incremental.
|
||
|
||
* @param[in] angle1: (0x201) 3508 motor control current, range [reserved]
|
||
* @param[in] angle2: (0x202) 3508 motor control current, range [reserved]
|
||
* @param[in] angle3: (0x203) 3508 motor control current, range [reserved]
|
||
* @param[in] angle4: (0x204) 3508 motor control current, range [reserved]
|
||
* @retval none
|
||
*/
|
||
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4)
|
||
{
|
||
fp32 angle[4];
|
||
fp32 delta_angle[4];
|
||
int32_t delta_speed[4];
|
||
int i;
|
||
|
||
angle[0] = angle1;
|
||
angle[1] = angle2;
|
||
angle[2] = angle3;
|
||
angle[3] = angle4;
|
||
|
||
//outer ring angle_loop
|
||
for(i=0;i<4;i++)
|
||
{
|
||
delta_angle[i] = PID_calc(&angle_pid[i],motor_data[i]->total_angle,angle[i]);
|
||
}
|
||
//inner ring velocity_loop
|
||
for(i=0;i<4;i++)
|
||
{
|
||
delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,delta_angle[i]);
|
||
}
|
||
|
||
CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]);
|
||
}
|
||
|
||
|
||
//*****6020*****//
|
||
//6020motor_init ID1
|
||
void GM6020_position_loop_init(void)
|
||
{
|
||
gimbal_PID_init(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,10.0f,0.0f,8.0f,5.0f,0.0f); //angle_loop_pid init
|
||
PID_init(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,PID_POSITION,GM6020_PID,30000.0,5000.0); //velocity_loop init
|
||
|
||
GM6020_data=get_G6020_motor_measure_point();
|
||
}
|
||
|
||
//6020Pos ID1(relative angle)
|
||
//this 6020is uesd only for angle_maximum(180)
|
||
//ecd encoder valueֵ(0-8191)
|
||
void GM6020_pos_loop(int32_t ecd)
|
||
{
|
||
//set positon
|
||
GM6020_pos_loop_control.angel_set=rad(ecd);
|
||
//calculate_relative_angle(-PI,PI)
|
||
GM6020_pos_loop_control.angel_get=motor_ecd_to_angle_change(GM6020_data->ecd,GM6020_pos_loop_control.angel_set);
|
||
//calculate_gyro (set-get)
|
||
GM6020_pos_loop_control.gyro_get=rad(GM6020_pos_loop_control.angel_set-GM6020_pos_loop_control.angel_get);
|
||
//angle_loop, pid
|
||
GM6020_pos_loop_control.gyro_set = gimbal_PID_calc(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,GM6020_pos_loop_control.angel_get, GM6020_pos_loop_control.angel_set,GM6020_pos_loop_control.gyro_get);
|
||
GM6020_pos_loop_control.current_set = PID_calc(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,GM6020_pos_loop_control.gyro_get, GM6020_pos_loop_control.gyro_set);
|
||
|
||
//control value
|
||
GM6020_pos_loop_control.current_given = (int16_t)(GM6020_pos_loop_control.current_set);
|
||
CAN_cmd_gimbal(GM6020_pos_loop_control.current_given,0,0,0);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd)
|
||
{
|
||
// if (pid == NULL)
|
||
// {
|
||
// return;
|
||
// }
|
||
pid->kp = kp;
|
||
pid->ki = ki;
|
||
pid->kd = kd;
|
||
|
||
pid->err = 0.0f;
|
||
pid->get = 0.0f;
|
||
|
||
pid->max_iout = max_iout;
|
||
pid->max_out = maxout;
|
||
}
|
||
|
||
fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta)
|
||
{
|
||
fp32 err;
|
||
// if (pid == NULL)
|
||
// {
|
||
// return 0.0f;
|
||
// }
|
||
pid->get = get;
|
||
pid->set = set;
|
||
|
||
err = set - get;
|
||
pid->err = rad_format(err);
|
||
pid->Pout = pid->kp * pid->err;
|
||
pid->Iout += pid->ki * pid->err;
|
||
pid->Dout = pid->kd * error_delta;
|
||
abs_limit_fp(&pid->Iout, pid->max_iout);
|
||
pid->out = pid->Pout + pid->Iout + pid->Dout;
|
||
abs_limit_fp(&pid->out, pid->max_out);
|
||
return pid->out;
|
||
}
|
||
|
||
//calculate relative_angle (-4096,4095)
|
||
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd)
|
||
{
|
||
int32_t relative_ecd = ecd - offset_ecd;
|
||
if (relative_ecd > 4095)
|
||
{
|
||
while(relative_ecd>4095)
|
||
{
|
||
relative_ecd -= 8191;
|
||
}
|
||
}
|
||
else if (relative_ecd < -4096)
|
||
{
|
||
while(relative_ecd<-4096)
|
||
{
|
||
relative_ecd += 8191;
|
||
}
|
||
}
|
||
return relative_ecd * MOTOR_ECD_TO_RAD;
|
||
}
|
||
|