main.cpp

00001 #include "mbed.h"
00002 #include "SI1143.h"
00003 #include "m3pi.h"
00004 #define NN 1000
00005 
00006 //DigitalOut enable(p30);
00007 //Ticker timer;
00008 
00009 I2C* i2c = new I2C(p28, p27);
00010 Serial bt(p13, p14);
00011 m3pi pi;
00012 
00013 DigitalOut led1(LED1);
00014 DigitalOut led2(LED2);
00015 DigitalOut led3(LED3);
00016 DigitalOut led4(LED4);
00017 
00018 InterruptIn event(p26);
00019 
00020 SI1143 sensor1(i2c, p30, p29, 0x01);
00021 //SI1143 sensor2(i2c, p25, p26, 0x02); // p25, p26
00022 
00023 //InterruptIn prox_meas(p26); //26
00024 int sense1a, sense1b, sense2a, sense2b;
00025 int sensor1alog[NN];
00026 int sensor1blog[NN];
00027 int whileflag = 1;
00028 int countingstuffpoop = 0;
00029 
00030 float alpha = 0.999;
00031 float beta;
00032 float floorpoop = 225;
00033 float stdpoop =  5;
00034 float scalepoop = 5;
00035 int cd = 5;
00036 
00037 void meas_int()
00038 {
00039     led1 = 1;
00040     sense1a = sensor1.read_ps1();
00041     sense1b = sensor1.read_ps2();
00042     if(sense1a > floorpoop)
00043     {
00044         floorpoop = floorpoop*(1+beta);    
00045     }
00046     else
00047     {
00048         floorpoop = floorpoop*(1-beta);
00049     }
00050 
00051     if(sqrt(((float) sense1a - floorpoop)*(sense1a - floorpoop)) > stdpoop)
00052     {
00053         stdpoop = stdpoop*(1+beta);    
00054     }
00055     else
00056     {
00057         stdpoop = stdpoop*(1-beta);
00058     }
00059     
00060     if(sense1a > floorpoop + scalepoop*stdpoop)
00061     {
00062         led4 = 1;
00063         cd = 20;
00064     }
00065     else
00066     {
00067         cd = cd - 1;
00068         if(cd < 0)
00069         {
00070             cd = - 1;
00071             led4 = 0;
00072         }   
00073     }    
00074     sensor1.clear_int();
00075     led1 = 0;
00076 }
00077 
00078 void blinkblink(int t, int n)
00079 {
00080     for (int i = 1; i <= n; i++)
00081     {
00082     led4 = 1;
00083     wait(t);
00084     led4 = 0;
00085     wait(t);    
00086     }
00087 }
00088 
00089 int main()
00090 {
00091     led2 = 0;
00092     //wait(10);
00093     blinkblink(1,2);
00094     beta = 1-alpha;
00095     event.fall(&meas_int);
00096     //timer.attach(&meas_int, 0.01);
00097     sensor1.start_ps_auto();
00098     led4 = 1;
00099     //pi.left(0.1);
00100 
00101     //pi.stop();
00102     blinkblink(1,5);
00103     
00104     led1 = 1;
00105     wait(0.3);
00106     led1 = 0;
00107     wait(0.3);
00108     led1 = 1;
00109     while(1)
00110     {
00111         
00112     }
00113 }