Diff: main.cpp

Revision:

7:af18e8cfe499

Parent:

6:4b007c7f0b7a

Child:

8:8f09c13d5138

--- a/main.cpp	Wed Jun 26 18:06:30 2019 +0000
+++ b/main.cpp	Sat Aug 03 12:31:17 2019 +0000
@@ -18,7 +18,7 @@
 #define SERVOSTART 544
 #define SERVOEND 2400
 #define MESSAGE_BUFFER_SIZE 32
-#define MIN_DIST 35
+#define DELTA 8
 Semaphore messageReceived(0);
 
 DigitalOut led1(LED1);                        
@@ -50,11 +50,15 @@
 const uint8_t servoLimitsCenter[16]={31, 108, 27,   35,   0,  23, 155, 106, 150,  45, 85, 136,   0,   0,   0,   0};
 const uint8_t servoLimitsEnd[16]=   {40, 135, 45,   90, 180,  90,  70,  75,  40, 120, 90,  55, 180, 180, 180, 180};
 
+uint8_t sensorsZeros[16];
+
 
 volatile int lastpos=0;
 volatile int activated=0;
 volatile int idle=0;
 
+char dist[12];
+
 
 void setservo(int num, float ang);
 
@@ -304,6 +308,21 @@
     }
 }
 
+void setSensorsZeros (void){
+    memset(dist,0,12);
+    i2c.read(0x20,dist,12);
+    
+    pc.printf("SensorsZeros \n");
+    for(uint8_t i=0; i<12; i++)
+    {   
+        sensorsZeros[i]=dist[i];
+                
+        pc.printf("%d\t ",sensorsZeros[i]);
+    }
+    pc.printf("SensorsZeros \n");
+    }
+    
+
 int main()
 {
     
@@ -324,12 +343,16 @@
     servothread.start(servoworker);
     serialthread.start(serialworker);
     Action act = NONE;
-    char dist[12];
+    
     int activated=-1;
     int activated_t=0;
     wait(2.5);
     mp3.mp3_stop();
     int dist_tmp[16][4];
+    
+    setSensorsZeros();
+
+    
     while(1)
     {
         memset(dist,0,12);
@@ -350,10 +373,10 @@
         {   
             pc.printf("\t %d",dist[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 ) {
+             if (abs(sensorsZeros[i]-dist_tmp[i][0])>DELTA && 
+                 abs(sensorsZeros[i]-dist_tmp[i][1])>DELTA && 
+                 abs(sensorsZeros[i]-dist_tmp[i][2])>DELTA &&
+                 abs(sensorsZeros[i]-dist_tmp[i][3])>DELTA ) {
                  activated=sensormap[i];
                 
                 }