File content as of revision 1:ce626b794a6c:

//////////////////////////////////////////////////////////////////////////////////
// Company: edinburgh of university
// Engineer: ZEjun DU
// 
// Create Date: 2017/08/20 13:06:52
// Design Name: Inverted Pendulum Balancer
// Module Name: calculating real-time angle
// Tool Versions: “Keil 5” or “Mbed Complie Online”
// Description: this part is to get real-time angle from angle sensor
//                           
// 
//////////////////////////////////////////////////////////////////////////////////

#include "get_angle.h"

#define NZEROS 3
#define NPOLES 3
#define GAIN   2.723997714e+01

AnalogIn angle_voltage(p20);// it is used to read the current voltage
float angle_offset = 0;//this is a offset to set the original

float angle;//the real-time angle

////////////////////////////////////////////////////////////
//this part is a amazing function based on 3 order digital Chebyshev filter
//the following part is the link of digital filter
//http://www-users.cs.york.ac.uk/~fisher/mkfilter/
//the sampling rate is 200Hz, the noise rate is about 20Hz according to FFT in MATLAB. the details cannot be represented here
////////////////////////////////////////////////////////////

float LPF_angle(float LPF_in){//3-order digital Chebyshev Low-pass filter
        float LPF_out;
        static float xv[NZEROS+1], yv[NPOLES+1];
        xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; 
        xv[3] = LPF_in / GAIN;
        yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; 
        yv[3] =   (xv[0] + xv[3]) + 3 * (xv[1] + xv[2])
                     + (  0.2457825234 * yv[0]) + ( -0.9894349708 * yv[1])
                     + (  1.4499664447 * yv[2]);
        LPF_out = yv[3];
        return LPF_out;  
} 

////////////////////////////////////////////////////////////// 
////this part is to get angle after filter
//////////////////////////////////////////////////////////////
void angle_update(void){
        float voltage_in;//the original angle value
        float voltage_out;//the angle after low-pass filter
        voltage_in  = angle_voltage.read() - angle_offset;// this equation will be used when the user set the original
        voltage_out = LPF_angle(voltage_in);          
        angle       = voltage_out*360*2;
        //angle       = voltage_in*360*2;//this equation is to show the real-time angle
}