Diff: controller/controller.cpp

Revision:

0:489498e8dae5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller/controller.cpp	Fri Aug 25 21:10:23 2017 +0000
@@ -0,0 +1,136 @@
+//////////////////////////////////////////////////////////////////////////////////
+// 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
+    }   
+}
\ No newline at end of file