/******************** (C) COPYRIGHT 2024 GeekRebot *****************************
 * File Name          : control.c
 * Current Version    : V1.0  & ST 3.5.0
 * Author             : zhanli 719901725@qq.com
 * Date of Issued     : 2024.01.06 zhanli: Create
 * Comments           : 平衡车控制代码
 ********************************************************************************/
#include "control.h"

/**----------------------------------------------------------------------
* Function    : EXTI15_10_IRQHandler
* Description : 所有的控制代码都在这里面, 5ms定时中断由MPU6050的INT引脚触发
                严格保证采样和数据处理的时间同步
* Author      : zhanli&719901725@qq.com
* Date        : 2023/05/20 zhanli
*---------------------------------------------------------------------**/ 
int EXTI15_10_IRQHandler(void)
{
    static u8 Flag_Target;                // 控制函数相关变量，提供10ms基准
    int Balance_Pwm, Velocity_Pwm;        // 平衡环PWM变量，速度环PWM变量
    int Motor_Left, Motor_Right;          // 电机PWM变量 应是Motor的 向Moto致敬
    if (INT == 0)
    {
        EXTI->PR = 1 << 12;               // 清除中断标志位
        Flag_Target = !Flag_Target;
        IMU_GetAngle();                      // 更新姿态	5ms读取一次
        Encoder_Left = -Read_Encoder(2);  // 读取左轮编码器的值，前进为正，后退为负
        Encoder_Right = -Read_Encoder(4); // 读取右轮编码器的值，前进为正，后退为负
                                          // 左轮A相接TIM2_CH1,右轮A相接TIM4_CH2,故这里两个编码器的极性相同
        if (Flag_Target == 1)             // 实际上10ms控制一次
            return 0;

        // 扫描按键状态 单击可启停电机
        Key();

        // 平衡PID控制,Gyro_Balance平衡角速度极性：前倾为正，后倾为负
        Balance_Pwm = Balance(Angle_Balance, Gyro_Balance);
        // 速度环PID控制,记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
        Velocity_Pwm = Velocity(Encoder_Left, Encoder_Right);

        Motor_Left = Balance_Pwm + Velocity_Pwm;  // 计算左轮电机最终PWM
        Motor_Right = Balance_Pwm + Velocity_Pwm; // 计算右轮电机最终PWM

        Motor_Left = Motor_PWMLimit(Motor_Left, 6900, -6900);
        Motor_Right = Motor_PWMLimit(Motor_Right, 6900, -6900); // PWM限幅

        if (Turn_Off(Angle_Balance) == 0)          // 如果不存在异常
            Motor_PWMSet(Motor_Left, Motor_Right); // 赋值给PWM寄存器
    }
    return 0;
}

/**----------------------------------------------------------------------
* Function    : Balance
* Description : 直立PD控制
* Input       : Angle:角度, Gyro:角速度
* Author      : zhanli&719901725@qq.com
* Date        : 2023/05/20 zhanli
*---------------------------------------------------------------------**/ 
int Balance(float Angle, float Gyro)
{
    // 直立环PD参数
    float Balance_Kp = 225, Balance_Kd = 1.08; 
    float Angle_bias, Gyro_bias;
    int balance;
    // 求出平衡的角度中值 和机械相关
    Angle_bias = Middle_angle - Angle; 
    Gyro_bias = 0 - Gyro;
    // 计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数
    balance = -Balance_Kp * Angle_bias - Gyro_bias * Balance_Kd; 
    return balance;
}

/**************************************************************************
Function: Speed PI control
Input   : encoder_left：Left wheel encoder reading；encoder_right：Right wheel encoder reading
Output  : Speed control PWM
函数功能：速度控制PWM
入口参数：encoder_left：左轮编码器读数；encoder_right：右轮编码器读数
返回  值：速度控制PWM
**************************************************************************/
int Velocity(int encoder_left, int encoder_right)
{
    float Velocity_Kp = 160, Velocity_Ki = 0.8; // 速度环PI参数
    static float velocity, Encoder_Least, Encoder_bias;
    static float Encoder_Integral;
    // 速度PI控制器
    Encoder_Least = 0 - (encoder_left + encoder_right); // 获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
    Encoder_bias *= 0.8;                                // 一阶低通滤波器
    Encoder_bias += Encoder_Least * 0.2;                // 一阶低通滤波器
                                                        // 相当于上次偏差的0.8 + 本次偏差的0.2，减缓速度差值，减少对直立的干扰
    Encoder_Integral += Encoder_bias;                   // 积分出位移 积分时间：10ms
    if (Encoder_Integral > 10000)
        Encoder_Integral = 10000;                       // 积分限幅
    if (Encoder_Integral < -10000)
        Encoder_Integral = -10000;                      // 积分限幅

    // 速度控制
    velocity = -Encoder_bias * Velocity_Kp - Encoder_Integral * Velocity_Ki; 

    // 电机关闭后清除积分
    if (Turn_Off(Angle_Balance) == 1 || Flag_Stop == 1)
        Encoder_Integral = 0; 
    return velocity;
}

/**************************************************************************
Function: Assign to PWM register
Input   : motor_left：Left wheel PWM；motor_right：Right wheel PWM
Output  : none
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回  值：无
**************************************************************************/
void Motor_PWMSet(int motor_left, int motor_right)
{
    if (motor_left > 0)
        BIN1 = 1, BIN2 = 0; // 前进
    else
        BIN1 = 0, BIN2 = 1; // 后退
    PWMB = myabs(motor_left);
    if (motor_right > 0)
        AIN2 = 1, AIN1 = 0; // 前进
    else
        AIN2 = 0, AIN1 = 1; // 后退
    PWMA = myabs(motor_right);
}

/**************************************************************************
Function: PWM limiting range
Input   : IN：Input  max：Maximum value  min：Minimum value
Output  : Output
函数功能：限制PWM赋值
入口参数：IN：输入参数  max：限幅最大值  min：限幅最小值
返回  值：限幅后的值
**************************************************************************/
int Motor_PWMLimit(int IN, int max, int min)
{
    int OUT = IN;
    if (OUT > max){
        OUT = max;
    }
    if (OUT < min){
        OUT = min;
    }
    return OUT;
}
/**************************************************************************
Function: Press the key to modify the car running state
Input   : none
Output  : none
函数功能：按键修改小车运行状态
入口参数：无
返回  值：无
**************************************************************************/
void Key(void)
{
    u8 tmp;
    tmp = click();
    if (tmp == 1){
        Flag_Stop = !Flag_Stop; // 单击控制小车的启停
    }
}

/**************************************************************************
Function: If abnormal, turn off the motor
Input   : angle：Car inclination；voltage：Voltage
Output  : 1：abnormal；0：normal
函数功能：异常关闭电机
入口参数：angle：小车倾角
返回  值：1：异常  0：正常
**************************************************************************/
u8 Turn_Off(float angle)
{
    u8 temp;
    if (angle < -40 || angle > 40 || 1 == Flag_Stop) // 倾角大于40度关闭电机
    {                                                // Flag_Stop置1关闭电机
        temp = 1;
        AIN1 = 0;
        AIN2 = 0;
        BIN1 = 0;
        BIN2 = 0;
    }
    else
        temp = 0;
    return temp;
}

/**************************************************************************
Function: Get angle
Input   : way：The algorithm of getting angle 1：DMP  2：kalman  3：Complementary filtering
Output  : none
函数功能：获取角度
入口参数：无
返回  值：无
**************************************************************************/
void IMU_GetAngle(void)
{
    Read_DMP();             // 读取加速度、角速度、倾角
    Angle_Balance = Pitch;  // 更新平衡倾角,前倾为正，后倾为负
    Gyro_Balance = gyro[0]; // 更新平衡角速度,前倾为正，后倾为负
}

/**************************************************************************
Function: Absolute value function
Input   : a：Number to be converted
Output  : unsigned int
函数功能：绝对值函数
入口参数：a：需要计算绝对值的数
返回  值：无符号整型
**************************************************************************/
int myabs(int a)
{
    int temp;
    if (a < 0)
        temp = -a;
    else
        temp = a;
    return temp;
}
