Diff: main.cpp

Revision:

6:fd9d4fed9946

Parent:

5:01ad87ee4f28

--- a/main.cpp	Sat Mar 16 07:30:44 2019 +0000
+++ b/main.cpp	Sat Mar 16 15:42:24 2019 +0000
@@ -92,8 +92,8 @@
     serial.baud(9600);
     serial.printf("=== Wakeup ===\n");
     
-    tick.attach(&onInterrupt, CHECK_INTERVAL);
-    btn.fall(&onBtnON);
+    //tick.attach(&onInterrupt, CHECK_INTERVAL);
+    //btn.fall(&onBtnON);
     
     colorSensor.init(TCS34725_INTEGRATIONTIME_101MS, TCS34725_GAIN_1X);
 
@@ -104,7 +104,16 @@
     printf("IP is %s\n", udp.getIPAdr());
 
     while (true) {
-        osSignalWait(0x01, osWaitForever);
+        //osSignalWait(0x01, osWaitForever);
+        MessageID msgID;
+        int timestamp;
+        ColorID colorID;
+        CourseID courseID;
+        bool isOK = udp.receive(msgID, timestamp, colorID, courseID);
+        if (!isOK) continue;
+        printf("receive:%d, %d, %d\n", msgID);
+        
+        if (msgID != MESSAGE_H) continue;
         
         //  センサーの反応がなくてもとにかく送り出すように変更
         // if (input && !isBtnON) continue;
@@ -119,7 +128,7 @@
         ColorUtil::HSV hsv = ColorUtil::RGBtoHSV(rgb);
         ColorUtil::BALL_COLOR col = ColorUtil::detectBallColor(hsv);
 
-        ColorID colorID = COLOR_NONE;        
+        colorID = COLOR_NONE;        
         switch (col) {
             case ColorUtil::BALL_RED:
                 colorID = COLOR_RED;
@@ -173,6 +182,10 @@
 };
 INT_STATE intState = INT_NONE;
 
+Timer myTimer[3];
+
+bool sensorFlag[3];
+
 void onSensorON()
 {
     if (intState != INT_NONE) return;
@@ -182,28 +195,18 @@
     led1 = true;
 }
 
+void intSensor(int idx);   
+
 void onSensor1ON() {
-    if (intState != INT_NONE) return;
-    
-    printf("sensor1");
-    intState = INT_SENSOR_1;
-    led1 = true;
+    intSensor(INT_SENSOR_1);
 }
 
 void onSensor2ON() {
-    if (intState != INT_NONE) return;
-    
-    printf("sensor2");
-    intState = INT_SENSOR_2;
-    led1 = true;    
+    intSensor(INT_SENSOR_2);
 }
 
 void onSensor3ON() {
-    if (intState != INT_NONE) return;
-    
-    printf("sensor3");
-    intState = INT_SENSOR_3;
-    led1 = true;    
+    intSensor(INT_SENSOR_3);
 }
 
 ColorID nowColor[] = {
@@ -227,10 +230,9 @@
     lightTimer.attach(&lightTimerDone, LIGHT_TIME_WHILE_ON);
 }
 
-
-void sendMessageD() {
+void sendMessageD(int state) {
     ColorID colorID = COLOR_NONE;
-    switch (intState) {
+    switch (state) {
         case INT_NONE:  // Error
             return;
             
@@ -241,7 +243,7 @@
             break;
             
         default:
-            sensorIndex = intState;
+            sensorIndex = state;
             colorID = nowColor[sensorIndex];
             // nowColor[sensorIndex] = COLOR_NONE; // keep color for dummy!
     }
@@ -272,9 +274,27 @@
     }
 }
 
+void intSensor(int idx) { 
+    if(idx<INT_SENSOR_1 || idx>INT_SENSOR_3)
+        return;
+         
+    int dtime = myTimer[idx].read_ms();
+    if(dtime > 500) {
+        printf("sensor%d", idx+1);
+        //intState = INT_SENSOR_1;
+        //led1 = true;
+        sensorFlag[idx] = true;
+        myTimer[idx].reset();
+    } 
+}
 
 int main()
 {
+    for(int i=0;i<3;i++) {
+        myTimer[i].start();
+        sensorFlag[i] = false;
+    }
+    
     serial.baud(9600);
     serial.printf("=== Wakeup (D)===\n");
     
@@ -293,12 +313,17 @@
 
     while (true) {
         if (intState != INT_NONE) {
-            sendMessageD();
+            sendMessageD(intState);
             wait_ms(500);
             intState = INT_NONE;
             led1 = false;
         }
-        
+        for(int i=0;i<3;i++) {
+            if(sensorFlag[i]) {
+                sensorFlag[i] = false;
+                sendMessageD(INT_SENSOR_1+i);
+            }
+        }
         MessageID msgID;
         int timestamp;
         ColorID colorID;