This is the code we showed at Uncraftivism

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
jarkman
Date:
Mon Dec 14 08:28:21 2009 +0000
Parent:
1:70d90598d2e7
Commit message:

Changed in this revision

Blinker.cpp Show annotated file Show diff for this revision Revisions of this file
Blinker.h Show annotated file Show diff for this revision Revisions of this file
MotionFinder.cpp Show annotated file Show diff for this revision Revisions of this file
ServoMinder.cpp Show annotated file Show diff for this revision Revisions of this file
ServoMinder.h Show annotated file Show diff for this revision Revisions of this file
ucam.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 70d90598d2e7 -r 01115080f6da Blinker.cpp
--- /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
+}
+
diff -r 70d90598d2e7 -r 01115080f6da Blinker.h
--- /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
diff -r 70d90598d2e7 -r 01115080f6da MotionFinder.cpp
--- 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);
 
     }
diff -r 70d90598d2e7 -r 01115080f6da ServoMinder.cpp
--- 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 ) ;
  }
         
diff -r 70d90598d2e7 -r 01115080f6da ServoMinder.h
--- 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:
    
diff -r 70d90598d2e7 -r 01115080f6da ucam.cpp
--- 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) );