Merge pull request #250 from MyRobotLab/pir-tracking

fix tracking with pir
MyRobotLab · Nov 26, 2024 · 03f60f9 · 03f60f9
2 parents 6050449 + 41bbbe1
commit 03f60f9
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 26 deletions.
diff --git a/gestures/stopTrackHumans.py b/gestures/stopTrackHumans.py
@@ -7,6 +7,7 @@ def stopTrackHumans():
     if runtime.isStarted("i01.eyeTracking"):
       i01_eyeTracking.disable()
     if runtime.isStarted("i01.head"):
+      i01_head_rollNeck.disable()
       i01_head_rollNeck.setAutoDisable(True)
     i01_opencv.removeFilters()
     i01_opencv.stopCapture()

diff --git a/gestures/trackHumans.py b/gestures/trackHumans.py
@@ -4,7 +4,8 @@ def trackHumans():
       i01_fsm.fire("track")
     if runtime.isStarted('i01.head'):  
       i01_head_rollNeck.setAutoDisable(False)
-      i01_head_rollNeck.moveToBlocking(90)
+      i01_head_rollNeck.enable()
+      i01_head_rollNeck.moveTo(90)
       i01.setHeadSpeed(500.0,500.0,500.0,500.0,500.0)
     i01_opencv.removeFilters()
     i01.startPeer("headTracking")

diff --git a/life/sleepMode.py b/life/sleepMode.py
@@ -69,7 +69,7 @@ def sleepModeWakeUp():
   if i01.getConfig().customSound==1:
     if runtime.isStarted("i01.audioPlayer"):
       i01_audioPlayer.playFile('resource/InMoov2/system/sounds/Notifications/'+random.choice(os.listdir('resource/InMoov2/system/sounds/Notifications')),False)
-  elif runtime.isStarted('i01.opencv'):
+  if runtime.isStarted('i01.opencv'):
     if i01.getConfig().pirEnableTracking==1:
       trackHumans()
     if i01.getConfig().openCVFaceRecognizerActivated==1:
@@ -240,24 +240,23 @@ def humanDetected():
       initPir()
       sleepTimer.restartClock()
       #i01_ear.setWakeWordTimeout(i01_ear.getConfig().wakeWordIdleTimeoutSeconds)
-  if runtime.isStarted('i01.neoPixel'):
-    if i01.getConfig().flashOnPir==False:
-      i01_neoPixel.setAnimation("Larson Scanner", 255, 0, 255, 25)
-      sleep(2)
-      i01_neoPixel.clear()
-    if runtime.isStarted('i01.fsm'):  
-      if not i01_fsm.getState()=="isTracking" and WaitXsecondBeforeRelaunchTracking>=5:
-        WaitXsecondBeforeRelaunchTracking=0
-        if runtime.isStarted('i01.headTracking'):
-          initTracking()
-          if i01.getConfig().pirEnableTracking==1:
-            trackHumans()
-            trackingTimer.restartClock()
-        #if i01.getConfig().openCVFaceRecognizerActivated==1:facerecognizer()
-
+  if i01.getConfig().flashOnPir==False:
+    print "pir detected something"
+  if runtime.isStarted('i01.fsm'):
+    if not i01_fsm.getState()=="isTracking" and WaitXsecondBeforeRelaunchTracking>=5:
+      WaitXsecondBeforeRelaunchTracking=0
+      if runtime.isStarted('i01.headTracking'):
+        initTracking()
+        if i01.getConfig().pirEnableTracking==1:
+          trackHumans()
+          trackingTimer.restartClock()
+      #if i01.getConfig().openCVFaceRecognizerActivated==1:facerecognizer()
+    else:
+      i01_fsm.fire("idle")
+      #print "idle state"
+      #print WaitXsecondBeforeRelaunchTracking
 
 def sleepTimerRoutine(timedata):
-  #if RobotIsSleeping==0:
   if runtime.isStarted('i01.fsm'):
     if not i01_fsm.getState()=="sleep":
       if runtime.isStarted('i01.neoPixel'):i01_neoPixel.setAnimation("Color Wipe", 17,  126, 57, 20)
@@ -274,13 +273,15 @@ def sleepTimerRoutine(timedata):
 
 def trackingTimerRoutine(timedata):
   global WaitXsecondBeforeRelaunchTracking
-  print "trackingTimer stopped"
-  if i01_opencv.isTracking():
-    WaitXsecondBeforeRelaunchTracking=-5
-    if runtime.isStarted('i01.headTracking'):
-      initTracking()
-      stopTrackHumans()
-      trackingTimer.stopClock()
-    if runtime.isStarted('i01.neoPixel'):i01_neoPixel.clear()
+  if runtime.isStarted('i01.fsm'):
+    if i01_fsm.getState()=="isTracking":
+      WaitXsecondBeforeRelaunchTracking=-5
+      if runtime.isStarted('i01.headTracking'):
+        initTracking()
+        stopTrackHumans()
+        #print "trackingTimer stopped"
+        #print WaitXsecondBeforeRelaunchTracking
+        trackingTimer.stopClock()
+      if runtime.isStarted('i01.neoPixel'):i01_neoPixel.clear()
 
 print("sleepMode.py loaded")