#include "DRV8837D.h"
#include <math.h>

uint8_t use_pid_control = 0;   // 是否启用PID调节

// cur_angle_deg 是当前实际角度值
// target_angle_deg 是目标角度值
tPid pidMotor1Speed; 
tPid angle_pid;
tPid current_pid;
MotorDirection Motormode;

float target_angle_deg = 0.0f;

float diff_Angle = 0.0f; 	// 目标角度和当前实际角度的差值
extern float Motor_cur; 	// 电机电流
extern float Motor_spe;		// 电机转速

void PID_init(void)
{
	pidMotor1Speed.target_val = 0.0f; // 目标角度
	pidMotor1Speed.actual_val = 0.0f; // 实际角度
		
	pidMotor1Speed.target_speed = 200.0f;// 目标速度 度/秒
	
	pidMotor1Speed.err = 0.0f; //实际和目标角度差
	pidMotor1Speed.err_last = 0.0f;
	pidMotor1Speed.err_sum = 0.0f;
	
	pidMotor1Speed.Kp = 1.3f;
	pidMotor1Speed.Ki = 0.7f;
	pidMotor1Speed.Kd = 0.2f;	
	
	pidMotor1Speed.output_max = 1000;
  pidMotor1Speed.output_min = -1000;
  pidMotor1Speed.integral_max = 5;
	
}

 // 比例P调节
float P_realize(tPid * pid, float actual_val)
{
	pid ->actual_val = actual_val; // 传递真实值
	pid ->err = pid ->target_val - pid ->actual_val;//当前误差 = 目标值 - 真实值
	// 比例控制调节  输出 = Kp*当前误差
	pid ->output = pid ->Kp*pid ->err;
	return pid ->output;
	
}

// 比例P 积分I 控制函数
float PI_realize(tPid * pid, float actual_val)
{	
	pid ->actual_val = actual_val; // 传递真实值
	pid ->err = pid ->target_val - pid ->actual_val;//当前误差 = 目标值 - 真实值
	pid ->err_sum += pid ->err; // 误差累计值 = 当前误差累计和
	// 使用PI控制  输出 = Kp*当前误差 + Ki*误差累计值
	pid ->output = pid ->Kp*pid ->err +pid ->Ki*pid ->err_sum;
	return pid ->output;
}

// PID控制函数
float PID_Realize(tPid *pid, float actual_val, float dt)
{
    pid->actual_val = actual_val;

    pid->err = get_shortest_angle_error(pid->target_val, pid->actual_val);
    pid->err_sum += pid->err * dt;

    // 限制积分范围
    if (pid->err_sum > pid->integral_max) pid->err_sum = pid->integral_max;
    if (pid->err_sum < -pid->integral_max) pid->err_sum = -pid->integral_max;

    float P = pid->Kp * pid->err;
    float I = pid->Ki * pid->err_sum;
    float D = pid->Kd * (pid->err - pid->err_last) / dt;

    pid->output = P + I + D;

    // 死区控制，避免小抖动
//    if (pid->output > 0 && pid->output < 170)
//        pid->output = 170;
//    else if (pid->output < 0 && pid->output > -170)
//        pid->output = -170;

    // 限幅
    if (pid->output > pid->output_max)
        pid->output = pid->output_max;
    if (pid->output < pid->output_min)
        pid->output = pid->output_min;

    pid->err_last = pid->err;
    return pid->output;
}

// PID中间变量
float pid_integral = 0.0f ;
float pid_last_error = 0.0f ;

float PID_Incremental_Calc(tPid *pid, float target_speed, float Motor_spe)
{
	pid -> err = target_speed - Motor_spe;
	
	pid -> output += pid -> Kp * (pid -> err - pid -> err_last)+
									 pid -> Ki * pid -> err +
									 pid -> Kd * (pid -> err + pid -> PrevError - 2* pid  -> err_last);
	
	pid -> PrevError = pid -> err_last;
	pid -> err_last = pid -> err;
	if( pid -> output > pid -> output_max ) pid -> output = pid -> output_max;
	if( pid -> output < - pid -> output_max ) pid -> output = - pid -> output_max;
	
	return pid ->output;
	
}

// 电机角度	cur_angle_deg（0到360度）
// 电机转速 Motor_spe （-400到+400）
// 电机电流 Motor_cur （正常是0.0几，堵转最高是到0.3）
// 目标速度 target_speed

void Motor_PID_Control(void)
{
    float error = pidMotor1Speed.target_speed - Motor_spe;
    pid_integral += error;
    float derivative = error - pid_last_error;

    float output = pidMotor1Speed.Kp * error + pidMotor1Speed.Ki * pid_integral + pidMotor1Speed.Kd * derivative;

    pid_last_error = error;

    // 限制输出防止溢出
    if (output > 999.0f) output = 999.0f;
    if (output < -999.0f) output = -999.0f;

    if (output >= 0)
    {
        // 正转
        Motor_control_ZF((uint16_t)output, 0);
    }
    else
    {
        // 反转
        Motor_control_ZF(0, (uint16_t)(-output));
    }
}

/*角度 + 电流双环 PID*/
void Motor_Angle_Current_PID_Control(void)
{
    float dt = 0.001f;                 		// 5ms 控制周期
    float angle_actual = cur_angle_deg;		// 当前角度
    float current_actual = Motor_cur;     // 实际电流

    // === 外环：角度 PID，输出为目标电流 ===  
		float current_target = PID_Realize(&angle_pid, angle_actual, dt);

    // === 内环：电流 PID，输出为PWM值 ===
    current_pid.target_val = current_target;
    float pwm_output = PID_Realize(&current_pid, current_actual, dt);

    if (pwm_output >= 0)
        Motor_control_ZF((uint16_t)pwm_output, 0);
    else
        Motor_control_ZF(0, (uint16_t)(-pwm_output));
}


// 控制电机正转
void Motor_Forward(uint16_t speed)
{
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, speed);
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
}

// 控制电机反转
void Motor_Reverse(uint16_t speed)
{
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, speed); 
}
// 控制输出到电机
void Motor_Control(float control)
{
    uint16_t pwm = (uint16_t)fabsf(control); // 取正值作为PWM占空比
    if (pwm > 1000) pwm = 1000;

    if (control > 0)
		{
				HAL_GPIO_WritePin(GPIOB, MOTOR_EN_Pin, GPIO_PIN_SET);
        Motor_Forward(pwm);
		}
    else if (control < 0)
		{
				HAL_GPIO_WritePin(GPIOB, MOTOR_EN_Pin, GPIO_PIN_SET);
        Motor_Reverse(pwm);
		}
    else
    {
        // 停止			
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
				HAL_GPIO_WritePin(GPIOB, MOTOR_EN_Pin, GPIO_PIN_RESET);
    }
}

// 求最短旋转角度差(带方向)
float get_shortest_angle_error(float target, float actual)
{
    float error = target - actual;

    if (error > 180.0f)
        error -= 360.0f;
    else if (error < -180.0f)
        error += 360.0f;

    return error;
}
// 控制电机正反转
void Motor_control_ZF(uint16_t forward_speed ,uint16_t reverse_speed)
{
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, forward_speed); 	//电机正转
		__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, reverse_speed); 	//电机反转
}
/////////////////////////////////////////////////
/*


void motor_forward(void) // 电机正转
{
	// 电机红色线接OUT1 电机黑色线接OUT2
	
	HAL_GPIO_WritePin(GPIOB, MOTOR_EN_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_SET);

}
void motor_rollback(void) // 电机反转
{
	// 电机红色线接OUT1 电机黑色线接OUT2
	HAL_GPIO_WritePin(GPIOB, MOTOR_EN_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_SET);	// 反转
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);

}
void motor_stop(void) // 电机停转
{
	// 电机红色线接OUT1 电机黑色线接OUT2
	HAL_GPIO_WritePin(GPIOB, MOTOR_EN_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_RESET);// 停转
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
	
}
*/