ServoMinder.cpp

00001 #include "stdafx.h"
00002 
00003 #include "mbed.h"
00004 #include "ServoMinder.h"
00005 
00006 
00007 extern Logger pcSerial;
00008 
00009 ServoMinder::ServoMinder( Servo *servo )
00010 {
00011     m_servo = servo;
00012     m_target = 0;
00013     
00014 
00015     m_tickTime = 0.02;
00016 
00017     setSpeed( 0.25 ); // half-rotations per sec
00018 
00019     m_ticker.attach( this, &ServoMinder::tick, m_tickTime );
00020 }
00021 
00022 bool ServoMinder::isMoving()
00023 {
00024     return fabs( m_servo->read() - m_target) > m_delta;
00025 }
00026 
00027 void ServoMinder::moveToAndWait( float target )
00028 {
00029     moveTo( target );
00030     
00031     while( isMoving() )
00032        wait( 0.001 ) ;
00033  }
00034         
00035 void ServoMinder::moveTo( float target )
00036 {
00037 
00038     m_target = target;
00039 
00040     
00041     
00042 }
00043 
00044 void ServoMinder::setSpeed( float speed )
00045 {
00046     m_speed = speed;
00047     m_delta = m_speed * m_tickTime;
00048 }
00049 
00050 void ServoMinder::tick()
00051 {
00052     float pos = m_servo->read();
00053 
00054       
00055     if( pos < m_target  )
00056     {
00057         pos += m_delta;
00058         if( pos > m_target )
00059             pos = m_target;
00060         
00061         // can't trace in here - breaks the cam protocol
00062         //pcSerial.printf("servo to %f\r\n", pos);    
00063         
00064         if( pos < 0 )
00065             pos = 0;
00066         if( pos > 1.0 )
00067             pos = 1.0;
00068                 
00069         m_servo->write( pos );
00070     }
00071     else if( pos > m_target )
00072     {
00073         pos -= m_delta;
00074         if( pos < m_target )
00075             pos = m_target;
00076             
00077         if( pos < 0 )
00078             pos = 0;
00079         if( pos > 1.0 )
00080             pos = 1.0;
00081         //pcSerial.printf("servo to %f\r\n", pos);    
00082         m_servo->write( pos );
00083     }
00084 
00085     
00086 }
00087