#include "pid.h"
#include "motor_control.h"
#include "math.h"

PID pid_location;
PID pid_speed;
PID pid_current;

float motor_speed = 0.0; //电机速度 
float prev_angle = 0;
float dt = 0;
float control_val = 0;


float location_out = 0;
float speed_out = 0;
float current_out = 0;
float target_pos = 0;
float current_pos = 0;
float current_curr = 0;
float current_speed = 0;

uint8_t pwm_output = 0;


/* 定义位置环PID参数相关宏 */
#define  L_KP      0.3f             /* P参数 */
#define  L_KI      0.00f             /* I参数 */
#define  L_KD      0.00f             /* D参数 */
/* 定义速度环PID参数相关宏 */
#define  S_KP      0.5f             /* P参数 */
#define  S_KI      0.02f             /* I参数 */
#define  S_KD      0.0f             /* D参数 */
/* 定义电流环PID参数相关宏 */
#define  C_KP      8.00f             /* P参数 */
#define  C_KI      4.0f             /* I参数 */
#define  C_KD      1.00f             /* D参数 */

#define  SMAPLSE_PID_SPEED   5     /* 采样周期 单位5ms */
#define  TARGET_CURRENT_MAX  0.9 
#define  TARGET_SPEED_MAX    0.8

void MC_Init(float angle)
{
	
	PID_Init(&pid_location, L_KP, L_KI, L_KD, 200, 100, -100);
	PID_Init(&pid_speed, 		S_KP, S_KI, S_KD, 200, 200, -200);
	PID_Init(&pid_current, 	C_KP, C_KI, C_KD, 200, 100, 0);
	
	motor_speed = 0;
	prev_angle = angle;
	dt = 1.0*SMAPLSE_PID_SPEED/1000;
	pwm_output = 50;
	
	 location_out = 0;
	 speed_out = 0;
	 current_out = 0;
	 target_pos = 0;
	 current_pos = 0;
	 current_curr = 0;
	 current_speed = 0;
	
}


uint8_t MC_Calculate(float setvalue, float angle, float current)
{
		target_pos = setvalue;
		current_pos = angle;
		current_curr = current;
		// 位置环
		control_val = PID_Increment(&pid_location, setvalue, angle);
		location_out = control_val;

		/* 目标速度上限处理 */
		if (control_val > TARGET_SPEED_MAX)
		{
				control_val = TARGET_SPEED_MAX;
		}
		else if (control_val < -TARGET_SPEED_MAX)
		{
				control_val = -TARGET_SPEED_MAX;
		}
	
		//速度环
		motor_speed = (angle - prev_angle);
		prev_angle = angle;
		
		current_speed = motor_speed;
		
		control_val = PID_Increment(&pid_speed, control_val, motor_speed);
		speed_out = control_val;

		//电流预处理
		if(control_val < 0 ){
				control_val = -control_val;
		}
		
		control_val = (control_val > TARGET_CURRENT_MAX) ? TARGET_CURRENT_MAX : control_val;    // 电流上限处理
	
		//电流环
		control_val = PID_Increment(&pid_current, control_val, current);
		current_out = control_val;

		if(control_val > 100.0){
			control_val = 100;
		}else if(control_val < 10 ){
			control_val = 10;
		}
		pwm_output = control_val;
		
		return pwm_output;

}