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

