Diff: main.cpp

Revision:

4:2f4d485ebe58

Parent:

2:ed1f45f9b06a

Child:

5:cec7f85ce6c4

--- a/main.cpp	Sat Jun 22 17:25:59 2019 +0000
+++ b/main.cpp	Sat Jun 22 17:57:41 2019 +0000
@@ -18,7 +18,7 @@
 #define SERVOSTART 544
 #define SERVOEND 2400
 #define MESSAGE_BUFFER_SIZE 32
-
+#define MIN_DIST 35
 Semaphore messageReceived(0);
 
 DigitalOut led1(LED1);                        
@@ -182,8 +182,8 @@
         
         //if (activated) 
         {   
-            HSV.s=0.1;
-            HSV.v=abs(sin(t.read()*1.11));
+            HSV.s=1;//0.1;
+            HSV.v=0.1;//abs(sin(t.read()*1.11));
             HSV.h=abs(sin(t.read()/30))*360;
             RGB = hsv2rgb(HSV);
             int r =RGB.r*255.0;
@@ -220,11 +220,13 @@
         if (ang>servoLimitsEnd[num] && ang>servoLimitsStart[num]) 
         {
             //ang = servoLimitsEnd[num];
-            pc.printf("out of range %d %3.2f\n",num,ang);}
+         //   pc.printf("out of range %d %3.2f\n",num,ang);
+         }
         if (ang<servoLimitsStart[num] && ang<servoLimitsEnd[num]) 
         {
             //ang = servoLimitsStart[num];
-            pc.printf("out of range %d %3.2f\n",num,ang);}
+         //   pc.printf("out of range %d %3.2f\n",num,ang);
+         }
         //m.lock();
         pwmouts.set_pwm_pw(num,servoang(ang));
         //m.unlock();
@@ -318,22 +320,40 @@
     Action act = NONE;
     char dist[12];
     int activated=-1;
-    wait(2);
+    wait(2.5);
     mp3.mp3_stop();
+    int dist_tmp[16][4];
     while(1)
     {
         memset(dist,0,12);
         i2c.read(0x20,dist,12);
         activated=-1;
+        for(uint8_t j=3; j>0; j--)
+        {   
+            pc.printf("\n**");
+            for(uint8_t i=0; i<12; i++)
+            {   
+                dist_tmp[i][j]=dist_tmp[i][j-1];
+              //  pc.printf("%d\t ",dist_tmp[i][j]);
+            }
+            
+        }
+        //pc.printf("\n");
         for(uint8_t i=0; i<12; i++)
         {   
             pc.printf("\t %d",dist[i]);
-            if (dist[i]<35) {
-                activated=sensormap[i];
+            dist_tmp[i][0]=dist[i];
+             if (dist_tmp[i][0]<MIN_DIST && 
+                 dist_tmp[i][1]<MIN_DIST && 
+                 dist_tmp[i][2]<MIN_DIST &&
+                 dist_tmp[i][3]<MIN_DIST ) {
+                 activated=sensormap[i];
                 
-                } 
+                }
                 //else activated=-1;
         }
+        
+        
         pc.printf("\n");
         
         if (activated>=0)