zejun du / Mbed 2 deprecated inverted_pendulum_system

this code is for inverted pendulum balancer. it is based on two-loop PID controller. One controller is for Angle and another one is for velocity.

Dependencies:   HCSR04 L298HBridge mbed

stabilizer/stabilizer.cpp@0:489498e8dae5, 2017-08-25 (annotated)

Committer:
dudu941014
Date:
Fri Aug 25 21:10:23 2017 +0000
Revision:
0:489498e8dae5
this code is for inverted pendulum balancer. it is based on two-loop PID controller. one PID controller is for angle and another one is for velocity.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dudu941014 0:489498e8dae5 1 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 2 // Company: edinburgh of university
dudu941014 0:489498e8dae5 3 // Engineer: ZEjun DU
dudu941014 0:489498e8dae5 4 //
dudu941014 0:489498e8dae5 5 // Create Date: 2017/08/20 13:06:52
dudu941014 0:489498e8dae5 6 // Design Name: Inverted Pendulum Balancer
dudu941014 0:489498e8dae5 7 // Module Name: basic PID algorithm
dudu941014 0:489498e8dae5 8 // Tool Versions: “Keil 5” or “Mbed Complie Online”
dudu941014 0:489498e8dae5 9 // Description: this funcition is to control and manage the PID mode
dudu941014 0:489498e8dae5 10 // the main loop only need to invoke this module's funcition
dudu941014 0:489498e8dae5 11 //
dudu941014 0:489498e8dae5 12 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 13
dudu941014 0:489498e8dae5 14
dudu941014 0:489498e8dae5 15 #include "stabilizer.h"
dudu941014 0:489498e8dae5 16 #include "controller.h"
dudu941014 0:489498e8dae5 17 #include "communication.h"
dudu941014 0:489498e8dae5 18 #include "get_angle.h"
dudu941014 0:489498e8dae5 19 #include "distance.h"
dudu941014 0:489498e8dae5 20
dudu941014 0:489498e8dae5 21 float M_Thr =0;//this is a offset PWM result which can be configured by GUI
dudu941014 0:489498e8dae5 22 float M_Pitch =0;//this is a PWM variable from anle PID controller
dudu941014 0:489498e8dae5 23 float M_Alt = 0;//this is a PWM variable from distance PID controller
dudu941014 0:489498e8dae5 24 int PID_ctrl=0;//the flag to able or disable two PID controllers
dudu941014 0:489498e8dae5 25 int Velocity_ctrl = 0;//the flag to able or disable distance PID control
dudu941014 0:489498e8dae5 26
dudu941014 0:489498e8dae5 27 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 28 //this part is to control the angle and distance PID control
dudu941014 0:489498e8dae5 29 //system can invoke this function in main loop to get the PWM of angle controller
dudu941014 0:489498e8dae5 30 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 31 void stabilize_task(void)
dudu941014 0:489498e8dae5 32 {
dudu941014 0:489498e8dae5 33
dudu941014 0:489498e8dae5 34
dudu941014 0:489498e8dae5 35 M_Pitch=controller_Angle_PID(angle);//this part is to get the PWM of the angle PID controller. The Angle PID controller is always enabled.
dudu941014 0:489498e8dae5 36
dudu941014 0:489498e8dae5 37 if(Velocity_ctrl)//distance PID controller is enabled or disabled
dudu941014 0:489498e8dae5 38 {
dudu941014 0:489498e8dae5 39 M_Alt = controller_Velocity_PID(distance);//this part is to get the PWM of the distance PID controller
dudu941014 0:489498e8dae5 40 }
dudu941014 0:489498e8dae5 41
dudu941014 0:489498e8dae5 42 if(PID_ctrl)//PID mode open or close
dudu941014 0:489498e8dae5 43 {
dudu941014 0:489498e8dae5 44 MOTOR_Update((M_Thr + M_Pitch + M_Alt));//send the PID result to MOTOR function.
dudu941014 0:489498e8dae5 45 //it is used to judge the output from PID. if the output exceed the limitation, the maximum output will become limitation
dudu941014 0:489498e8dae5 46 }
dudu941014 0:489498e8dae5 47
dudu941014 0:489498e8dae5 48
dudu941014 0:489498e8dae5 49 }