Richard Sewell
This is the code we showed at Uncraftivism
ServoMinder.cpp@2:01115080f6da, 2009-12-14 (annotated)
- Committer:
- jarkman
- Date:
- Mon Dec 14 08:28:21 2009 +0000
- Revision:
- 2:01115080f6da
- Parent:
- 1:70d90598d2e7
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jarkman | 0:57f4fdadc97f | 1 | #include "stdafx.h" |
| jarkman | 0:57f4fdadc97f | 2 | |
| jarkman | 0:57f4fdadc97f | 3 | #include "mbed.h" |
| jarkman | 0:57f4fdadc97f | 4 | #include "ServoMinder.h" |
| jarkman | 0:57f4fdadc97f | 5 | |
| jarkman | 0:57f4fdadc97f | 6 | |
| jarkman | 0:57f4fdadc97f | 7 | extern Logger pcSerial; |
| jarkman | 0:57f4fdadc97f | 8 | |
| jarkman | 0:57f4fdadc97f | 9 | ServoMinder::ServoMinder( Servo *servo ) |
| jarkman | 0:57f4fdadc97f | 10 | { |
| jarkman | 0:57f4fdadc97f | 11 | m_servo = servo; |
| jarkman | 0:57f4fdadc97f | 12 | m_target = 0; |
| jarkman | 0:57f4fdadc97f | 13 | |
| jarkman | 0:57f4fdadc97f | 14 | |
| jarkman | 0:57f4fdadc97f | 15 | m_tickTime = 0.02; |
| jarkman | 0:57f4fdadc97f | 16 | |
| jarkman | 0:57f4fdadc97f | 17 | setSpeed( 0.25 ); // half-rotations per sec |
| jarkman | 0:57f4fdadc97f | 18 | |
| jarkman | 0:57f4fdadc97f | 19 | m_ticker.attach( this, &ServoMinder::tick, m_tickTime ); |
| jarkman | 0:57f4fdadc97f | 20 | } |
| jarkman | 0:57f4fdadc97f | 21 | |
| jarkman | 2:01115080f6da | 22 | bool ServoMinder::isMoving() |
| jarkman | 2:01115080f6da | 23 | { |
| jarkman | 2:01115080f6da | 24 | return fabs( m_servo->read() - m_target) > m_delta; |
| jarkman | 2:01115080f6da | 25 | } |
| jarkman | 2:01115080f6da | 26 | |
| jarkman | 1:70d90598d2e7 | 27 | void ServoMinder::moveToAndWait( float target ) |
| jarkman | 1:70d90598d2e7 | 28 | { |
| jarkman | 1:70d90598d2e7 | 29 | moveTo( target ); |
| jarkman | 1:70d90598d2e7 | 30 | |
| jarkman | 2:01115080f6da | 31 | while( isMoving() ) |
| jarkman | 1:70d90598d2e7 | 32 | wait( 0.001 ) ; |
| jarkman | 1:70d90598d2e7 | 33 | } |
| jarkman | 1:70d90598d2e7 | 34 | |
| jarkman | 0:57f4fdadc97f | 35 | void ServoMinder::moveTo( float target ) |
| jarkman | 0:57f4fdadc97f | 36 | { |
| jarkman | 1:70d90598d2e7 | 37 | |
| jarkman | 0:57f4fdadc97f | 38 | m_target = target; |
| jarkman | 1:70d90598d2e7 | 39 | |
| jarkman | 0:57f4fdadc97f | 40 | |
| jarkman | 0:57f4fdadc97f | 41 | |
| jarkman | 0:57f4fdadc97f | 42 | } |
| jarkman | 0:57f4fdadc97f | 43 | |
| jarkman | 0:57f4fdadc97f | 44 | void ServoMinder::setSpeed( float speed ) |
| jarkman | 0:57f4fdadc97f | 45 | { |
| jarkman | 0:57f4fdadc97f | 46 | m_speed = speed; |
| jarkman | 0:57f4fdadc97f | 47 | m_delta = m_speed * m_tickTime; |
| jarkman | 0:57f4fdadc97f | 48 | } |
| jarkman | 0:57f4fdadc97f | 49 | |
| jarkman | 0:57f4fdadc97f | 50 | void ServoMinder::tick() |
| jarkman | 0:57f4fdadc97f | 51 | { |
| jarkman | 0:57f4fdadc97f | 52 | float pos = m_servo->read(); |
| jarkman | 0:57f4fdadc97f | 53 | |
| jarkman | 0:57f4fdadc97f | 54 | |
| jarkman | 0:57f4fdadc97f | 55 | if( pos < m_target ) |
| jarkman | 0:57f4fdadc97f | 56 | { |
| jarkman | 0:57f4fdadc97f | 57 | pos += m_delta; |
| jarkman | 0:57f4fdadc97f | 58 | if( pos > m_target ) |
| jarkman | 0:57f4fdadc97f | 59 | pos = m_target; |
| jarkman | 0:57f4fdadc97f | 60 | |
| jarkman | 0:57f4fdadc97f | 61 | // can't trace in here - breaks the cam protocol |
| jarkman | 0:57f4fdadc97f | 62 | //pcSerial.printf("servo to %f\r\n", pos); |
| jarkman | 0:57f4fdadc97f | 63 | |
| jarkman | 0:57f4fdadc97f | 64 | if( pos < 0 ) |
| jarkman | 0:57f4fdadc97f | 65 | pos = 0; |
| jarkman | 0:57f4fdadc97f | 66 | if( pos > 1.0 ) |
| jarkman | 0:57f4fdadc97f | 67 | pos = 1.0; |
| jarkman | 0:57f4fdadc97f | 68 | |
| jarkman | 0:57f4fdadc97f | 69 | m_servo->write( pos ); |
| jarkman | 0:57f4fdadc97f | 70 | } |
| jarkman | 0:57f4fdadc97f | 71 | else if( pos > m_target ) |
| jarkman | 0:57f4fdadc97f | 72 | { |
| jarkman | 0:57f4fdadc97f | 73 | pos -= m_delta; |
| jarkman | 0:57f4fdadc97f | 74 | if( pos < m_target ) |
| jarkman | 0:57f4fdadc97f | 75 | pos = m_target; |
| jarkman | 0:57f4fdadc97f | 76 | |
| jarkman | 0:57f4fdadc97f | 77 | if( pos < 0 ) |
| jarkman | 0:57f4fdadc97f | 78 | pos = 0; |
| jarkman | 0:57f4fdadc97f | 79 | if( pos > 1.0 ) |
| jarkman | 0:57f4fdadc97f | 80 | pos = 1.0; |
| jarkman | 0:57f4fdadc97f | 81 | //pcSerial.printf("servo to %f\r\n", pos); |
| jarkman | 0:57f4fdadc97f | 82 | m_servo->write( pos ); |
| jarkman | 0:57f4fdadc97f | 83 | } |
| jarkman | 0:57f4fdadc97f | 84 | |
| jarkman | 0:57f4fdadc97f | 85 | |
| jarkman | 0:57f4fdadc97f | 86 | } |
| jarkman | 0:57f4fdadc97f | 87 |