ben winstone
/
ucam
As of Monday morning, so this is the code we showed at Uncraftivism.
Diff: ServoMinder.cpp
- Revision:
- 0:da6a22da11a2
diff -r 000000000000 -r da6a22da11a2 ServoMinder.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ServoMinder.cpp Fri Dec 11 21:59:44 2009 +0000 @@ -0,0 +1,87 @@ +#include "stdafx.h" + +#include "mbed.h" +#include "ServoMinder.h" + + +extern Logger pcSerial; + +ServoMinder::ServoMinder( Servo *servo ) +{ + m_servo = servo; + m_target = 0; + + + m_tickTime = 0.02; + + setSpeed( 0.25 ); // half-rotations per sec + + 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( isMoving() ) + wait( 0.001 ) ; + } + +void ServoMinder::moveTo( float target ) +{ + + m_target = target; + + + +} + +void ServoMinder::setSpeed( float speed ) +{ + m_speed = speed; + m_delta = m_speed * m_tickTime; +} + +void ServoMinder::tick() +{ + float pos = m_servo->read(); + + + if( pos < m_target ) + { + pos += m_delta; + if( pos > m_target ) + pos = m_target; + + // can't trace in here - breaks the cam protocol + //pcSerial.printf("servo to %f\r\n", pos); + + if( pos < 0 ) + pos = 0; + if( pos > 1.0 ) + pos = 1.0; + + m_servo->write( pos ); + } + else if( pos > m_target ) + { + pos -= m_delta; + if( pos < m_target ) + pos = m_target; + + if( pos < 0 ) + pos = 0; + if( pos > 1.0 ) + pos = 1.0; + //pcSerial.printf("servo to %f\r\n", pos); + m_servo->write( pos ); + } + + +} +