#ifndef _MOTOR_CONTROL_H_
#define _MOTOR_CONTROL_H_

#include "struct_typedef.h"
#include "pid.h"



typedef struct
{
    fp32 kp;
    fp32 ki;
    fp32 kd;

    fp32 set;
    fp32 get;
    fp32 err;

    fp32 max_out;
    fp32 max_iout;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;

    fp32 out;
} gimbal_PID_t;

typedef struct
{
	gimbal_PID_t gimbal_motor_relative_angle_pid;
	pid_type_def gimbal_motor_gyro_pid;
	fp32 angel_set;
	fp32 angel_get;
	fp32 gyro_set;
	fp32 gyro_get;
	fp32 current_set;
	fp32 current_given;
}gimbal_pid_control_t;


#define FILTER_BUF_LEN		5

//#define MOTOR_ECD_TO_ANGLE 0.00231311936
#define MOTOR_ECD_TO_ANGLE  (360.0 / 8191.0 / (3591.0/187.0))
#define MOTOR_ECD_TO_RAD 0.000766990394f
#define rad_format(Ang) loop_fp32_constrain((Ang), (-4096*MOTOR_ECD_TO_RAD), (4095*MOTOR_ECD_TO_RAD))
#define rad(code) ((code)*MOTOR_ECD_TO_RAD)


void M3508_motor_init(int mode);
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
void GM6020_position_loop_init(void);
void GM6020_pos_loop(int32_t ecd);
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd);
void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd);
void motor_speed_change_low_to_high(int rag);
#endif