Chen Wei Ting / Mbed 2 deprecated middleyuan

read EMG, IMU, encoder

Dependencies:   mbed

Fork of LSM9DS1_project by 曾 宗圓

main.cpp@3:567765d3bcd1, 2018-08-01 (annotated)

Committer:
JJting
Date:
Wed Aug 01 01:01:13 2018 +0000
Revision:
3:567765d3bcd1
Parent:
2:c889fecf9afe 
original 20180801

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beanmachine44 0:9632b831b6c1 1 #include "mbed.h"
beanmachine44 0:9632b831b6c1 2 #include "LSM9DS1.h"
middleyuan 1:b1562831bbaf 3 #include "AS5145.h"
beanmachine44 0:9632b831b6c1 4
middleyuan 2:c889fecf9afe 5 LSM9DS1 imu2(D5, D7);
middleyuan 2:c889fecf9afe 6 LSM9DS1 imu3(D3, D6);
middleyuan 1:b1562831bbaf 7 LSM9DS1 imu(D14, D15);
middleyuan 2:c889fecf9afe 8 AnalogIn sEMG(D13);
middleyuan 2:c889fecf9afe 9 AnalogIn sEMG2(D1);
middleyuan 2:c889fecf9afe 10 AnalogIn sEMG3(D0);
middleyuan 2:c889fecf9afe 11 AnalogIn sEMG4(PC_4);
beanmachine44 0:9632b831b6c1 12 Serial pc(USBTX, USBRX);
middleyuan 1:b1562831bbaf 13 Ticker timer1;
middleyuan 1:b1562831bbaf 14 Ticker timer2;
JJting 3:567765d3bcd1 15 DigitalOut LED(A4); // check if the code is running
beanmachine44 0:9632b831b6c1 16
middleyuan 1:b1562831bbaf 17 float T = 0.001;
middleyuan 1:b1562831bbaf 18 /********************************************************************/
middleyuan 1:b1562831bbaf 19 //function declaration
middleyuan 1:b1562831bbaf 20 /********************************************************************/
middleyuan 1:b1562831bbaf 21 void init_TIMER(void);
middleyuan 1:b1562831bbaf 22 void timer1_interrupt(void);
middleyuan 1:b1562831bbaf 23 void setup(void);
middleyuan 1:b1562831bbaf 24 void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha);
middleyuan 1:b1562831bbaf 25 float lpf(float input, float output_old, float frequency);
middleyuan 1:b1562831bbaf 26 void angle_fn(float x1_hat[3],float x2_hat[3]);
middleyuan 1:b1562831bbaf 27 void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3]);
middleyuan 1:b1562831bbaf 28 void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3]);
middleyuan 1:b1562831bbaf 29 /********************************************************************/
middleyuan 1:b1562831bbaf 30 // sensor data
middleyuan 1:b1562831bbaf 31 /********************************************************************/
middleyuan 1:b1562831bbaf 32 int16_t Gyro_axis_data[9] = {0}; // X, Y, Z axis
middleyuan 1:b1562831bbaf 33 int16_t Acce_axis_data[9] = {0}; // X, Y, Z axis
middleyuan 1:b1562831bbaf 34 float Gyro_axis_data_f[9] = {0};
middleyuan 1:b1562831bbaf 35 float Gyro_axis_data_f_old[9] = {0};
middleyuan 1:b1562831bbaf 36 float Acce_axis_data_f[9] = {0};
middleyuan 1:b1562831bbaf 37 float Acce_axis_data_f_old[9] = {0};
middleyuan 1:b1562831bbaf 38 float axm[3] = {0.0f};
middleyuan 1:b1562831bbaf 39 float aym[3] = {0.0f};
middleyuan 1:b1562831bbaf 40 float azm[3] = {0.0f};
middleyuan 1:b1562831bbaf 41 float w1[3] = {0.0f};
middleyuan 1:b1562831bbaf 42 float w2[3] = {0.0f};
middleyuan 1:b1562831bbaf 43 float w3[3] = {0.0f};
middleyuan 1:b1562831bbaf 44 //estimator
middleyuan 1:b1562831bbaf 45 float x1_hat[3] = {0.0f};
middleyuan 1:b1562831bbaf 46 float x2_hat[3] = {0.0f};
middleyuan 1:b1562831bbaf 47 float sinroll[3] = {0.0f};
middleyuan 1:b1562831bbaf 48 float cosroll[3] = {0.0f};
middleyuan 1:b1562831bbaf 49 float sinpitch[3] = {0.0f};
middleyuan 1:b1562831bbaf 50 float pitch_angle[3] = {0.0f};
middleyuan 1:b1562831bbaf 51 float roll_angle[3] = {0.0f};
middleyuan 1:b1562831bbaf 52 float yaw_dot[3] = {0.0f};
middleyuan 1:b1562831bbaf 53 float pitch_dot[3] = {0.0f};
middleyuan 1:b1562831bbaf 54 float pitch_double_dot[3] = {0.0f};
middleyuan 1:b1562831bbaf 55 float pitch_double_dot_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 56 float pitch_double_dot_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 57 float pitch_dot_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 58 float axm_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 59 float axm_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 60 float w3aym_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 61 float w3aym_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 62 float w2azm_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 63 float w2azm_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 64 float aym_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 65 float aym_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 66 float w3axm_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 67 float w3axm_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 68 float w1azm_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 69 float w1azm_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 70 float azm_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 71 float azm_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 72 float w2axm_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 73 float w2axm_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 74 float w1aym_f[3] = {0.0f};
middleyuan 1:b1562831bbaf 75 float w1aym_f_old[3] = {0.0f};
middleyuan 1:b1562831bbaf 76 //sEMG variable
middleyuan 1:b1562831bbaf 77 float emg_value[4] = {0.0f};
beanmachine44 0:9632b831b6c1 78
beanmachine44 0:9632b831b6c1 79 int main()
beanmachine44 0:9632b831b6c1 80 {
JJting 3:567765d3bcd1 81 LED = 1;
JJting 3:567765d3bcd1 82 wait_ms(500);
JJting 3:567765d3bcd1 83
middleyuan 1:b1562831bbaf 84 pc.baud(230400);
middleyuan 1:b1562831bbaf 85 setup(); //Setup sensors
middleyuan 1:b1562831bbaf 86 AS5145_begin(); //begin encoder
JJting 3:567765d3bcd1 87
JJting 3:567765d3bcd1 88 wait_ms(500);
JJting 3:567765d3bcd1 89 LED = 0;
JJting 3:567765d3bcd1 90
middleyuan 1:b1562831bbaf 91 init_TIMER();
middleyuan 1:b1562831bbaf 92 while (1)
beanmachine44 0:9632b831b6c1 93 {
middleyuan 2:c889fecf9afe 94 //pc.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096);
middleyuan 2:c889fecf9afe 95 wait(0.05);
middleyuan 1:b1562831bbaf 96 //pc.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]);
JJting 3:567765d3bcd1 97 // pc.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096);
JJting 3:567765d3bcd1 98 pc.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]);
middleyuan 1:b1562831bbaf 99 //pc.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]);
middleyuan 1:b1562831bbaf 100 //pc.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]);
middleyuan 1:b1562831bbaf 101 //pc.printf("position: %d,%d\r\n", position[0], position[1]);
middleyuan 1:b1562831bbaf 102 //pc.printf("roll_angle: %2f\r\n",roll_angle);
middleyuan 1:b1562831bbaf 103 //pc.printf("A: %2f, %2f, %2f\r\n", imu2.ax*Acce_gain_x_2, imu2.ay*Acce_gain_y_2, imu2.az*Acce_gain_z_2);
beanmachine44 0:9632b831b6c1 104 }
beanmachine44 0:9632b831b6c1 105 }
middleyuan 1:b1562831bbaf 106 void setup()
middleyuan 1:b1562831bbaf 107 {
middleyuan 1:b1562831bbaf 108 imu.begin();
middleyuan 1:b1562831bbaf 109 imu2.begin();
middleyuan 1:b1562831bbaf 110 imu3.begin();
middleyuan 1:b1562831bbaf 111 }
middleyuan 1:b1562831bbaf 112 /********************************************************************/
middleyuan 1:b1562831bbaf 113 // init_TIMER
middleyuan 1:b1562831bbaf 114 /********************************************************************/
middleyuan 1:b1562831bbaf 115 void init_TIMER(void)
middleyuan 1:b1562831bbaf 116 {
middleyuan 1:b1562831bbaf 117 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
middleyuan 1:b1562831bbaf 118 timer2.attach_us(&ReadValue, 1000);//1ms interrupt period (1000 Hz)
middleyuan 1:b1562831bbaf 119 }
middleyuan 1:b1562831bbaf 120 /********************************************************************/
middleyuan 1:b1562831bbaf 121 // timer1_interrupt
middleyuan 1:b1562831bbaf 122 /********************************************************************/
middleyuan 1:b1562831bbaf 123 void timer1_interrupt(void)
middleyuan 1:b1562831bbaf 124 {
middleyuan 1:b1562831bbaf 125 int i;
middleyuan 1:b1562831bbaf 126 imu.readAccel();
middleyuan 1:b1562831bbaf 127 imu.readGyro();
middleyuan 1:b1562831bbaf 128 imu2.readAccel();
middleyuan 1:b1562831bbaf 129 imu2.readGyro();
middleyuan 1:b1562831bbaf 130 imu3.readAccel();
middleyuan 1:b1562831bbaf 131 imu3.readGyro();
middleyuan 1:b1562831bbaf 132 // sensor raw data
middleyuan 1:b1562831bbaf 133 Acce_axis_data[0] = imu.ax*Acce_gain_x;
middleyuan 1:b1562831bbaf 134 Acce_axis_data[1] = imu.ay*Acce_gain_y;
middleyuan 1:b1562831bbaf 135 Acce_axis_data[2] = imu.az*Acce_gain_z;
middleyuan 1:b1562831bbaf 136 Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
middleyuan 1:b1562831bbaf 137 Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
middleyuan 1:b1562831bbaf 138 Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
middleyuan 1:b1562831bbaf 139 Acce_axis_data[6] = -imu3.ax*Acce_gain_x_2;
middleyuan 1:b1562831bbaf 140 Acce_axis_data[7] = -imu3.az*Acce_gain_y_2;
middleyuan 1:b1562831bbaf 141 Acce_axis_data[8] = -imu3.ay*Acce_gain_z_2;
middleyuan 1:b1562831bbaf 142
middleyuan 1:b1562831bbaf 143 Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
middleyuan 1:b1562831bbaf 144 Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
middleyuan 1:b1562831bbaf 145 Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
middleyuan 1:b1562831bbaf 146 Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
middleyuan 1:b1562831bbaf 147 Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
middleyuan 1:b1562831bbaf 148 Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
middleyuan 1:b1562831bbaf 149 Gyro_axis_data[6] = -imu3.gx*Gyro_gain_x_2;
middleyuan 1:b1562831bbaf 150 Gyro_axis_data[7] = -imu3.gz*Gyro_gain_y_2;
middleyuan 1:b1562831bbaf 151 Gyro_axis_data[8] = -imu3.gy*Gyro_gain_z_2;
middleyuan 1:b1562831bbaf 152
middleyuan 1:b1562831bbaf 153 for(i=0;i<9;i++)
middleyuan 1:b1562831bbaf 154 {
middleyuan 1:b1562831bbaf 155 Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
middleyuan 1:b1562831bbaf 156 Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
middleyuan 1:b1562831bbaf 157 Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
middleyuan 1:b1562831bbaf 158 Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
middleyuan 1:b1562831bbaf 159 }
middleyuan 1:b1562831bbaf 160
middleyuan 1:b1562831bbaf 161 axm[0] = Acce_axis_data_f[0];
middleyuan 1:b1562831bbaf 162 aym[0] = Acce_axis_data_f[1];
middleyuan 1:b1562831bbaf 163 azm[0] = Acce_axis_data_f[2];
middleyuan 1:b1562831bbaf 164 w1[0] = Gyro_axis_data_f[0];
middleyuan 1:b1562831bbaf 165 w2[0] = Gyro_axis_data_f[1];
middleyuan 1:b1562831bbaf 166 w3[0] = Gyro_axis_data_f[2];
middleyuan 1:b1562831bbaf 167 axm[1] = Acce_axis_data_f[3];
middleyuan 1:b1562831bbaf 168 aym[1] = Acce_axis_data_f[4];
middleyuan 1:b1562831bbaf 169 azm[1] = Acce_axis_data_f[5];
middleyuan 1:b1562831bbaf 170 w1[1] = Gyro_axis_data_f[3];
middleyuan 1:b1562831bbaf 171 w2[1] = Gyro_axis_data_f[4];
middleyuan 1:b1562831bbaf 172 w3[1] = Gyro_axis_data_f[5];
middleyuan 1:b1562831bbaf 173 axm[2] = Acce_axis_data_f[6];
middleyuan 1:b1562831bbaf 174 aym[2] = Acce_axis_data_f[7];
middleyuan 1:b1562831bbaf 175 azm[2] = Acce_axis_data_f[8];
middleyuan 1:b1562831bbaf 176 w1[2] = Gyro_axis_data_f[6];
middleyuan 1:b1562831bbaf 177 w2[2] = Gyro_axis_data_f[7];
middleyuan 1:b1562831bbaf 178 w3[2] = Gyro_axis_data_f[8];
middleyuan 1:b1562831bbaf 179
middleyuan 1:b1562831bbaf 180
middleyuan 2:c889fecf9afe 181 estimator(axm,aym,azm,w3,w2,w1,120);
middleyuan 1:b1562831bbaf 182 angle_fn(x1_hat,x2_hat);
middleyuan 1:b1562831bbaf 183 pitch_dot_fn(w3,w2,w1,sinroll,cosroll);
middleyuan 1:b1562831bbaf 184 pitch_double_dot_fn(pitch_dot,pitch_dot_old);
middleyuan 1:b1562831bbaf 185
middleyuan 1:b1562831bbaf 186 for(i=0;i<3;i++)
middleyuan 1:b1562831bbaf 187 {
middleyuan 1:b1562831bbaf 188 pitch_dot_old[i] = pitch_dot[i];
middleyuan 1:b1562831bbaf 189 }
middleyuan 1:b1562831bbaf 190 emg_value[0] = sEMG.read();
middleyuan 1:b1562831bbaf 191 emg_value[1] = sEMG2.read();
middleyuan 1:b1562831bbaf 192 emg_value[2] = sEMG3.read();
middleyuan 1:b1562831bbaf 193 emg_value[3] = sEMG4.read();
middleyuan 1:b1562831bbaf 194 }
middleyuan 1:b1562831bbaf 195 /********************************************************************/
middleyuan 1:b1562831bbaf 196 // estimator
middleyuan 1:b1562831bbaf 197 /********************************************************************/
middleyuan 1:b1562831bbaf 198 void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha)
middleyuan 1:b1562831bbaf 199 {
middleyuan 1:b1562831bbaf 200 int i;
middleyuan 1:b1562831bbaf 201 for(i=0;i<3;i++)
middleyuan 1:b1562831bbaf 202 {
middleyuan 1:b1562831bbaf 203 axm_f[i] = lpf(axm[i],axm_f_old[i],alpha);
middleyuan 1:b1562831bbaf 204 axm_f_old[i] = axm_f[i];
middleyuan 1:b1562831bbaf 205 w3aym_f[i] = lpf(w3[i]*aym[i],w3aym_f_old[i],alpha);
middleyuan 1:b1562831bbaf 206 w3aym_f_old[i] = w3aym_f[i];
middleyuan 1:b1562831bbaf 207 w2azm_f[i] = lpf(w2[i]*azm[i],w2azm_f_old[i],alpha);
middleyuan 1:b1562831bbaf 208 w2azm_f_old[i] = w2azm_f[i];
middleyuan 1:b1562831bbaf 209 aym_f[i] = lpf(aym[i],aym_f_old[i],alpha);
middleyuan 1:b1562831bbaf 210 aym_f_old[i] = aym_f[i];
middleyuan 1:b1562831bbaf 211 w3axm_f[i] = lpf(w3[i]*axm[i],w3axm_f_old[i],alpha);
middleyuan 1:b1562831bbaf 212 w3axm_f_old[i] = w3axm_f[i];
middleyuan 1:b1562831bbaf 213 w1azm_f[i] = lpf(w1[i]*azm[i],w1azm_f_old[i],alpha);
middleyuan 1:b1562831bbaf 214 w1azm_f_old[i] = w1azm_f[i];
middleyuan 1:b1562831bbaf 215
middleyuan 1:b1562831bbaf 216 x1_hat[i] = axm_f[i] + w3aym_f[i]/alpha - w2azm_f[i]/alpha;
middleyuan 1:b1562831bbaf 217 x2_hat[i] = -w3axm_f[i]/alpha + aym_f[i] + w1azm_f[i]/alpha;
middleyuan 1:b1562831bbaf 218 }
middleyuan 1:b1562831bbaf 219
middleyuan 1:b1562831bbaf 220 }
middleyuan 1:b1562831bbaf 221 /********************************************************************/
middleyuan 1:b1562831bbaf 222 // angle_fn
middleyuan 1:b1562831bbaf 223 /********************************************************************/
middleyuan 1:b1562831bbaf 224 void angle_fn(float x1_hat[3],float x2_hat[3])
middleyuan 1:b1562831bbaf 225 {
middleyuan 1:b1562831bbaf 226 int i;
middleyuan 1:b1562831bbaf 227 for(i=0;i<3;i++)
middleyuan 1:b1562831bbaf 228 {
middleyuan 1:b1562831bbaf 229 sinroll[i] = x2_hat[i]*(-0.1020);
middleyuan 1:b1562831bbaf 230 if(sinroll[i] >= 1.0f)
middleyuan 1:b1562831bbaf 231 {
middleyuan 1:b1562831bbaf 232 sinroll[i] = 1.0;
middleyuan 1:b1562831bbaf 233 cosroll[i] = 0.0;
middleyuan 1:b1562831bbaf 234 }
middleyuan 1:b1562831bbaf 235 else if(sinroll[i] <= -1.0f)
middleyuan 1:b1562831bbaf 236 {
middleyuan 1:b1562831bbaf 237 sinroll[i] = -1.0;
middleyuan 1:b1562831bbaf 238 cosroll[i] = 0.0;
middleyuan 1:b1562831bbaf 239 }
middleyuan 1:b1562831bbaf 240 else cosroll[i] = sqrt(1-(sinroll[i]*sinroll[i]));
middleyuan 1:b1562831bbaf 241 roll_angle[i] = (asin(sinroll[i]))*180/pi;
middleyuan 1:b1562831bbaf 242 sinpitch[i] = x1_hat[i]*(0.1020f)/cosroll[i];
middleyuan 1:b1562831bbaf 243 if(sinpitch[i] >= 1.0f)
middleyuan 1:b1562831bbaf 244 {
middleyuan 1:b1562831bbaf 245 sinpitch[i] = 1.0;
middleyuan 1:b1562831bbaf 246 }
middleyuan 1:b1562831bbaf 247 else if(sinpitch[i] <= -1.0f)
middleyuan 1:b1562831bbaf 248 {
middleyuan 1:b1562831bbaf 249 sinpitch[i] = -1.0;
middleyuan 1:b1562831bbaf 250 }
middleyuan 1:b1562831bbaf 251
middleyuan 1:b1562831bbaf 252 pitch_angle[i] = (asin(sinpitch[i]))*180/pi;
middleyuan 1:b1562831bbaf 253 }
middleyuan 1:b1562831bbaf 254 }
middleyuan 1:b1562831bbaf 255 void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3])
middleyuan 1:b1562831bbaf 256 {
middleyuan 1:b1562831bbaf 257 int i;
middleyuan 1:b1562831bbaf 258 for(i=0;i<3;i++)
middleyuan 1:b1562831bbaf 259 {
middleyuan 1:b1562831bbaf 260 yaw_dot[i] = (w3[i]*cosroll[i] - w1[i]*sinroll[i])/cosroll[i];
middleyuan 1:b1562831bbaf 261 pitch_dot[i] = w2[i] - yaw_dot[i]*sinroll[i];
middleyuan 1:b1562831bbaf 262 }
middleyuan 1:b1562831bbaf 263 }
middleyuan 1:b1562831bbaf 264 void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3])
middleyuan 1:b1562831bbaf 265 {
middleyuan 1:b1562831bbaf 266 int i;
middleyuan 1:b1562831bbaf 267 for(i=0;i<3;i++)
middleyuan 1:b1562831bbaf 268 {
middleyuan 1:b1562831bbaf 269 pitch_double_dot[i] = (pitch_dot[i] - pitch_dot_old[i])/0.01f;
middleyuan 1:b1562831bbaf 270 pitch_double_dot_f[i] = lpf(pitch_double_dot[i],pitch_double_dot_f_old[i],30);
middleyuan 1:b1562831bbaf 271 pitch_double_dot_f_old[i] = pitch_double_dot_f[i];
middleyuan 1:b1562831bbaf 272 }
middleyuan 1:b1562831bbaf 273 }
middleyuan 1:b1562831bbaf 274 /********************************************************************/
middleyuan 1:b1562831bbaf 275 // lpf
middleyuan 1:b1562831bbaf 276 /********************************************************************/
middleyuan 1:b1562831bbaf 277 float lpf(float input, float output_old, float frequency)
middleyuan 1:b1562831bbaf 278 {
middleyuan 1:b1562831bbaf 279 float output = 0;
middleyuan 1:b1562831bbaf 280
middleyuan 1:b1562831bbaf 281 output = (output_old + frequency*T*input) / (1 + frequency*T);
middleyuan 1:b1562831bbaf 282
middleyuan 1:b1562831bbaf 283 return output;
middleyuan 1:b1562831bbaf 284 }