File content as of revision 0:489498e8dae5:

//////////////////////////////////////////////////////////////////////////////////
// Company: edinburgh of university
// Engineer: ZEjun DU
// 
// Create Date: 2017/08/20 13:06:52
// Design Name: Inverted Pendulum Balancer
// Module Name: create the PID struct, and control and manage two PID controllers
// Tool Versions: “Keil 5” or “Mbed Complie Online”
// Description: this part is to get PWM result from two PID controllers
//                           
// 
//////////////////////////////////////////////////////////////////////////////////

#include "controller.h"

PidObject pidAngle;//create Angle PID controller
PidObject pidVelocity;//create Velocity PID controller

static float LastEncoder,Encoder,LastDistance_result;

float AngleOutput;//the angle PID controller's output
float velOutput;//the Velocity PID controller's output
L298HBridge Motor_PWM(p21, p22, p23);//it defines the pins of controlling motor

float Motor;//the PWM result from two PID controller

float I_term;//show the changle of I_term


////////////////////////////////////////////////////////////
//this funcition is to initialize two PID controllers
////////////////////////////////////////////////////////////
void controller_init(void)
{
    pidInit(&pidAngle, 0, 0, 0, 0, 50, controllerAng_update_dt);//to create and initialize angle PID controller
    pidInit(&pidVelocity, 0, 0, 0, 0, 500, controllerVel_Update_dt);//to create and initialize Velocity PID controller
}


////////////////////////////////////////////////////////////
//this funcition is to control and manage the Angle PID controller
//the system can invoke this function to get the result of Angle PWM
////////////////////////////////////////////////////////////
float controller_Angle_PID(float angle_result)
{
  // Update PID for angle
    pidAngle.error = pidAngle.desired - angle_result * 10;// in the actual control, the effect of plusing 0.01 for Kp Ki Kd is too small. thus the error is multiplied 10 rather than change GUI's code 
    if(pidAngle.error > AngleErrLimit)//this part is to limit the error of angle
        pidAngle.error = AngleErrLimit;//if the angle's error is too lager, the result of angle will become limitation
    else if(pidAngle.error < -AngleErrLimit)//this part is to limit the error of angle
        pidAngle.error = -AngleErrLimit;//if the angle's error is too small, the result of angle will become limitation
    AngleOutput = get_pid(&pidAngle);//get the angle output from basic PID control theroy

    if(AngleOutput > AngleOutputLimit)//judge the angle output
        AngleOutput = AngleOutputLimit;//if the angle's output is too lager, the output of the angle PID controller will become limitation
    else if(AngleOutput < -AngleOutputLimit)//judge the angle output
        AngleOutput = -AngleOutputLimit;//if the angle's output is too small, the output of the angle PID controller will become limitation

    return AngleOutput;//return the output of the angle PID controller 
  
}


////////////////////////////////////////////////////////////
//this funcition is to control and manage the Velocity PID controller
//the system can invoke this function to get the result of Velocity PWM
////////////////////////////////////////////////////////////
float controller_Velocity_PID(float Distance_result)
{
    LastEncoder =(Distance_result- LastDistance_result) / controllerVel_Update_dt;   //to calculate the velocity    
    Encoder *= 0.8;            //this equation can be ragarded as a low pass filter, it is used to reduce the effect of Velocity PID controller                                               
    Encoder += LastEncoder*0.2; //this equation can be replaced by the "mean function", for example, (Encoder(1)+Encoder(2)+Encoder(3))/3

    pidVelocity.error = pidVelocity.desired - Encoder * 10;// in the actual control, the effect of plusing 0.01 for Kp Ki Kd is too small. thus the error is multiplied 10 rather than change GUI's code 
    
    if(pidVelocity.error > VelocityLimit)//this part is to limit the error of Velocity
        pidVelocity.error = VelocityLimit;//if the Velocity's error is too lager, the result of Velocity will become limitation
    else if(pidVelocity.error < -VelocityLimit)//this part is to limit the error of Velocity
        pidVelocity.error = -VelocityLimit;//if the Velocity's error is too lager, the result of Velocity will become limitation
    velOutput= get_pid_vel(&pidVelocity,Distance_result);//in order to set intput of I_term, the output of velocity become this
    I_term = show_I_term();//show the change of I_term on GUI

    if(velOutput > velOutputLimit)//judge the velocity output
        velOutput = velOutputLimit;//if the velocity's output is too lager, the output of the veloctiy PID controller will become limitation
    else if(velOutput < -velOutputLimit)//judge the velocity output
        velOutput = -velOutputLimit;//if the velocity's output is too small, the output of the velocity PID controller will become limitation
    
    
    LastDistance_result = Distance_result;//record the last velocity
         
    return velOutput;//return the result of the velocity PID controller
}


////////////////////////////////////////////////////////////
//this funcition is to reset parameters of two PID controllers
////////////////////////////////////////////////////////////
void controllerResetAllPID(void)
{
  pidReset(&pidAngle);//reset angle PID controller
  pidReset(&pidVelocity);//reset velocity PID controller
}


////////////////////////////////////////////////////////////
//this funcition is to control the motor according to current PID controller
////////////////////////////////////////////////////////////
void MOTOR_Update(float Motor)
{
    if(Motor >= 1000)//this judgement limits the output of motor
    {
        Motor = 1000;
    }
    else if(Motor <= -1000)//this judgement limits the output of motor
    {   
        Motor = -1000;
    }
    if(Motor > 0)//this judgement can give the direction of current motor value. go forward
    {
        Motor_PWM.Fwd();//go forward
        Motor_PWM.Speed((float)Motor + offset_moving);//set the force to motor according to PID controllers
    }
    //if the balancer is under the load, the car cannot get enough force to make the movement at 10% PWM or 20% PWM 
    // after many times tests, the offset PWM is 40%
    
    if(Motor == 0)//if the output is 0, the motor will be commended to stop
    {
        Motor_PWM.Stop();
        Motor_PWM.Speed(Motor);
    }
    if(Motor < 0)//this judgement can give the direction of current motor value. go reserve
    {
        Motor_PWM.Rev();//go reserve 
        Motor_PWM.Speed(-(float)Motor + offset_moving);//set the force to motor according to PID controllers
    }   
}