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

get_angle/get_angle.cpp@1:ce626b794a6c, 2017-08-25 (annotated)

Committer:
dudu941014
Date:
Fri Aug 25 21:25:46 2017 +0000
Revision:
1:ce626b794a6c
Parent:
0:489498e8dae5 
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.

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: calculating real-time angle
dudu941014 0:489498e8dae5 8 // Tool Versions: “Keil 5” or “Mbed Complie Online”
dudu941014 0:489498e8dae5 9 // Description: this part is to get real-time angle from angle sensor
dudu941014 0:489498e8dae5 10 //
dudu941014 0:489498e8dae5 11 //
dudu941014 0:489498e8dae5 12 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 13
dudu941014 0:489498e8dae5 14 #include "get_angle.h"
dudu941014 0:489498e8dae5 15
dudu941014 0:489498e8dae5 16 #define NZEROS 3
dudu941014 0:489498e8dae5 17 #define NPOLES 3
dudu941014 0:489498e8dae5 18 #define GAIN 2.723997714e+01
dudu941014 0:489498e8dae5 19
dudu941014 0:489498e8dae5 20 AnalogIn angle_voltage(p20);// it is used to read the current voltage
dudu941014 0:489498e8dae5 21 float angle_offset = 0;//this is a offset to set the original
dudu941014 0:489498e8dae5 22
dudu941014 0:489498e8dae5 23 float angle;//the real-time angle
dudu941014 0:489498e8dae5 24
dudu941014 0:489498e8dae5 25 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 26 //this part is a amazing function based on 3 order digital Chebyshev filter
dudu941014 0:489498e8dae5 27 //the following part is the link of digital filter
dudu941014 0:489498e8dae5 28 //http://www-users.cs.york.ac.uk/~fisher/mkfilter/
dudu941014 0:489498e8dae5 29 //the sampling rate is 200Hz, the noise rate is about 20Hz according to FFT in MATLAB. the details cannot be represented here
dudu941014 0:489498e8dae5 30 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 31
dudu941014 0:489498e8dae5 32 float LPF_angle(float LPF_in){//3-order digital Chebyshev Low-pass filter
dudu941014 0:489498e8dae5 33 float LPF_out;
dudu941014 0:489498e8dae5 34 static float xv[NZEROS+1], yv[NPOLES+1];
dudu941014 0:489498e8dae5 35 xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3];
dudu941014 0:489498e8dae5 36 xv[3] = LPF_in / GAIN;
dudu941014 0:489498e8dae5 37 yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3];
dudu941014 0:489498e8dae5 38 yv[3] = (xv[0] + xv[3]) + 3 * (xv[1] + xv[2])
dudu941014 0:489498e8dae5 39 + ( 0.2457825234 * yv[0]) + ( -0.9894349708 * yv[1])
dudu941014 0:489498e8dae5 40 + ( 1.4499664447 * yv[2]);
dudu941014 0:489498e8dae5 41 LPF_out = yv[3];
dudu941014 0:489498e8dae5 42 return LPF_out;
dudu941014 0:489498e8dae5 43 }
dudu941014 0:489498e8dae5 44
dudu941014 0:489498e8dae5 45 //////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 46 ////this part is to get angle after filter
dudu941014 0:489498e8dae5 47 //////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 48 void angle_update(void){
dudu941014 0:489498e8dae5 49 float voltage_in;//the original angle value
dudu941014 0:489498e8dae5 50 float voltage_out;//the angle after low-pass filter
dudu941014 0:489498e8dae5 51 voltage_in = angle_voltage.read() - angle_offset;// this equation will be used when the user set the original
dudu941014 0:489498e8dae5 52 voltage_out = LPF_angle(voltage_in);
dudu941014 0:489498e8dae5 53 angle = voltage_out*360*2;
dudu941014 0:489498e8dae5 54 //angle = voltage_in*360*2;//this equation is to show the real-time angle
dudu941014 0:489498e8dae5 55 }
dudu941014 0:489498e8dae5 56
dudu941014 0:489498e8dae5 57