Revision 2:01115080f6da, committed 2009-12-14

Comitter:

jarkman

Date:

Mon Dec 14 08:28:21 2009 +0000

Parent:

1:70d90598d2e7

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Blinker.cpp	Mon Dec 14 08:28:21 2009 +0000
@@ -0,0 +1,97 @@
+#include "stdafx.h"
+
+#include "mbed.h"
+#include "Blinker.h"
+
+#define EYE_CLOSED 0.25
+#define EYE_OPEN 0.85
+
+extern Logger pcSerial;
+
+Blinker::Blinker( ServoMinder *servoMinder )
+{
+    m_servoMinder = servoMinder;
+    m_servoMinder->moveTo( EYE_CLOSED );
+    
+    m_sleepiness = 0;
+    m_boredom = 0;
+    
+    m_tickTime = 0.1;
+    m_blinkTimer = 0;
+    
+    m_nextMove = -1;
+
+
+    m_ticker.attach( this, &Blinker::tick, m_tickTime );
+}
+
+void Blinker::setBoredom( float boredom )
+{
+    m_boredom = boredom;
+}
+
+void Blinker::setSleepiness( float sleepiness )
+{
+    float diff = fabs( m_sleepiness - sleepiness );
+    m_sleepiness = sleepiness;
+    
+    if( diff > 0.01 )
+        open();
+   
+}
+    
+float  Blinker::speedForSleepiness()
+{
+    // for sleepiness 0->1, return speed 2->0.5
+    return 2 - (1.5 * m_sleepiness);
+}
+
+float  Blinker::openPosForSleepiness()
+{
+    // for sleepiness 0->1, return EYE_OPEN ->( )/2
+    return  EYE_OPEN +  ( (EYE_CLOSED-EYE_OPEN) * 0.8 * m_sleepiness);
+}
+
+void Blinker::close()
+{
+    m_servoMinder->setSpeed( speedForSleepiness() );
+    m_servoMinder->moveTo( EYE_CLOSED );
+
+}
+   
+ void Blinker::open()
+{
+    m_servoMinder->setSpeed( speedForSleepiness() );
+    m_servoMinder->moveTo(  openPosForSleepiness() );
+} 
+
+void Blinker::blink()
+{
+    m_servoMinder->setSpeed( speedForSleepiness() );
+    m_servoMinder->moveTo( EYE_CLOSED );
+    m_nextMove = openPosForSleepiness();
+}
+
+void Blinker::tick()
+{
+
+    if( ! m_servoMinder->isMoving())
+        if( m_nextMove >= 0 )
+        {
+             m_servoMinder->moveTo( m_nextMove );
+             m_nextMove = -1;
+        }
+        
+    m_blinkTimer ++;
+    if( m_blinkTimer > 100 ) // 10 secs
+    {
+        m_blinkTimer = 0;
+        blink();
+    }
+    
+    if( m_boredom < m_sleepiness )
+        setSleepiness(  m_boredom ); // wake up quickly
+    else    
+        setSleepiness( m_sleepiness + (m_boredom - m_sleepiness) / 200 ); // fall asleep slowly
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Blinker.h	Mon Dec 14 08:28:21 2009 +0000
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "ServoMinder.h"
+class Blinker
+{
+public:
+    Blinker( ServoMinder *servoMinder );
+
+    void blink();
+    void close();
+    void open();
+    void setSleepiness( float sleepiness );
+    void setBoredom( float boredom );
+     
+private:
+   
+
+    ServoMinder *m_servoMinder;
+    
+    float m_boredom;
+    float m_sleepiness;
+    float m_tickTime;
+    Ticker m_ticker;
+    float m_nextMove;
+    
+    float m_blinkTimer;
+
+    float speedForSleepiness();
+    float openPosForSleepiness();
+    
+    void tick();
+
+};
\ No newline at end of file

--- a/MotionFinder.cpp	Thu Dec 10 21:16:57 2009 +0000
+++ b/MotionFinder.cpp	Mon Dec 14 08:28:21 2009 +0000
@@ -7,10 +7,12 @@
 
 
 #include "ServoMinder.h"
+#include "Blinker.h"
 
 
 
 extern Logger pcSerial;
+extern Blinker *blinker;
 
 // Motion detection for the mbed
 
@@ -29,7 +31,7 @@
     
    
     
-
+/*
     m_xServoMinder->moveTo( 1.0 );
     wait( 1 );
     m_xServoMinder->moveTo( 0.0 );
@@ -38,12 +40,13 @@
     wait( 1 );
 
     
-    m_yServoMinder->moveTo( 1.0 );
+    m_yServoMinder->moveTo( 0.7 );
     wait( 1 );
     m_yServoMinder->moveTo( 0.0 );
-    wait( 1 );
+    wait( 2 );
     m_yServoMinder->moveTo( 0.5 );
-    wait( 1 );
+    wait( 2 );
+    */
 }
 
     
@@ -151,18 +154,36 @@
     if( percentage < 3 ) // no real target, no COG
     {
         pcSerial.printf("No COG\r\n");
+
+       m_xServoMinder->setSpeed( 0.02 );
+       m_yServoMinder->setSpeed( 0.02 );
+       
+        blinker->setBoredom( 1 );
         
+        m_attentionX = m_attentionX + (((0.5 - m_attentionX))/20);
+        m_attentionY = m_attentionY + (((0.7 - m_attentionY))/20);
        // could implement some looking-around in this state
+       
+
 
     }
     else if( sumN > 0 )
     {
+         m_xServoMinder->setSpeed( 0.25 );
+         m_yServoMinder->setSpeed( 0.25 );
+         
         cogX = sumX / sumN;
         cogY = sumY / sumN;
 
         m_attentionX = ((float)cogX  / frame->m_width);
         m_attentionY = ((float)cogY  / frame->m_width); // use the larger dimension so x & y get the same scaling
 
+        //blinker->setBoredom( 0 );
+        float boredom = (2*percentage)/100.0;
+        if(boredom > 1) boredom = 1;
+        blinker->setBoredom( boredom );
+
+
         pcSerial.printf("COG is %d, %d\r\n", (int) cogX, (int) cogY);
 
     }

--- a/ServoMinder.cpp	Thu Dec 10 21:16:57 2009 +0000
+++ b/ServoMinder.cpp	Mon Dec 14 08:28:21 2009 +0000
@@ -19,11 +19,16 @@
     m_ticker.attach( this, &ServoMinder::tick, m_tickTime );
 }
 
+bool ServoMinder::isMoving()
+{
+    return fabs( m_servo->read() - m_target) > m_delta;
+}
+
 void ServoMinder::moveToAndWait( float target )
 {
     moveTo( target );
     
-    while( fabs( m_servo->read() - m_target) > m_delta )
+    while( isMoving() )
        wait( 0.001 ) ;
  }
         

--- a/ServoMinder.h	Thu Dec 10 21:16:57 2009 +0000
+++ b/ServoMinder.h	Mon Dec 14 08:28:21 2009 +0000
@@ -9,6 +9,7 @@
     void moveTo( float target );
     void moveToAndWait( float target );
      void setSpeed( float speed );
+     bool isMoving();
      
 private:
    

--- a/ucam.cpp	Thu Dec 10 21:16:57 2009 +0000
+++ b/ucam.cpp	Mon Dec 14 08:28:21 2009 +0000
@@ -7,6 +7,7 @@
 #include "MotionFinder.h"
 #include "Servo.h"
 #include "SerialBuffered.h"
+#include "Blinker.h"
 
 
 // ucam protocol implementation for mbed
@@ -29,38 +30,27 @@
 
 
 
-Servo xServo (p21);
+Servo xServo (p23);
 Servo yServo (p22);
-Servo eyelidServo (p23); 
-#define EYE_CLOSED 0.15
-#define EYE_OPEN 0.8 
+Servo eyelidServo (p21); 
+ServoMinder *eyelidMinder = new ServoMinder( &eyelidServo );
+Blinker *blinker = new Blinker( eyelidMinder ); 
+ 
 
 MotionFinder *motionFinder = NULL;
 
 
-void testEyelid()
-{
- ServoMinder *eyelidMinder = new ServoMinder( &eyelidServo );
-
-    eyelidMinder->setSpeed( 1 );
-      eyelidMinder->moveToAndWait( EYE_CLOSED );
-  
-      eyelidMinder->setSpeed( 0.1 );
-      eyelidMinder->moveToAndWait( EYE_OPEN );
-      eyelidMinder->setSpeed( 2 );
-      eyelidMinder->moveToAndWait( EYE_CLOSED );
-      eyelidMinder->moveToAndWait( EYE_OPEN );
-}
       
 void UCamInit() {
    
-    
+   
+
+    blinker->close();
       
     ucam.doStartup();
     Frame::initFrames();
     
-     
-    testEyelid();
+    blinker->open();
   
     motionFinder = new MotionFinder( new ServoMinder(&xServo), new ServoMinder(&yServo) );