Diff: main.cpp

Revision:

8:f81057354da4

Parent:

7:73a722360cdb

--- a/main.cpp	Sat Jun 21 20:14:09 2014 +0000
+++ b/main.cpp	Sat Jun 21 21:02:46 2014 +0000
@@ -27,28 +27,50 @@
 int whileflag = 1;
 int countingstuffpoop = 0;
 
+float alpha = 0.999;
+float beta;
+float floorpoop = 225;
+float stdpoop =  5;
+float scalepoop = 5;
+int cd = 5;
+
 void meas_int()
 {
     led1 = 1;
     sense1a = sensor1.read_ps1();
     sense1b = sensor1.read_ps2();
-    //sense2a = sensor2.get_ps1(1);
-    //sense2b = sensor2.get_ps2(1);
+    if(sense1a > floorpoop)
+    {
+        floorpoop = floorpoop*(1+beta);    
+    }
+    else
+    {
+        floorpoop = floorpoop*(1-beta);
+    }
 
-    countingstuffpoop++;
-    if(countingstuffpoop < NN)
+    if(sqrt(((float) sense1a - floorpoop)*(sense1a - floorpoop)) > stdpoop)
     {
-        led4 = 0;
-        sensor1alog[countingstuffpoop-1] = sense1a;
-        sensor1blog[countingstuffpoop-1] = sense1b;
+        stdpoop = stdpoop*(1+beta);    
     }
     else
     {
-        __disable_irq(); 
-        whileflag = 0;
+        stdpoop = stdpoop*(1-beta);
+    }
+    
+    if(sense1a > floorpoop + scalepoop*stdpoop)
+    {
         led4 = 1;
+        cd = 20;
     }
-
+    else
+    {
+        cd = cd - 1;
+        if(cd < 0)
+        {
+            cd = - 1;
+            led4 = 0;
+        }   
+    }    
     sensor1.clear_int();
     led1 = 0;
 }
@@ -69,33 +91,23 @@
     led2 = 0;
     //wait(10);
     blinkblink(1,2);
-
+    beta = 1-alpha;
     event.fall(&meas_int);
     //timer.attach(&meas_int, 0.01);
     sensor1.start_ps_auto();
     led4 = 1;
     //pi.left(0.1);
 
-    while(whileflag)
-    {
-        // Read each led sensor
-        led2 = whileflag;
-    }
-    led3 = 0;
     //pi.stop();
     blinkblink(1,5);
     
-
-    bt.printf("d = [");
-    for (int i = 0; i < NN; i++)
-    {
-        bt.printf("%d,  %d;\r\n", sensor1alog[i], sensor1blog[i]);
-        //bt.printf("%d;\r\n", sensorlog[i]);
-    }
-    bt.printf("0, 0];");
     led1 = 1;
     wait(0.3);
     led1 = 0;
     wait(0.3);
     led1 = 1;
+    while(1)
+    {
+        
+    }
 }
\ No newline at end of file