/**
****************************(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;
}