Richard Sewell
This is the code we showed at Uncraftivism
ServoMinder.cpp@1:70d90598d2e7, 2009-12-10 (annotated)
- Committer:
- jarkman
- Date:
- Thu Dec 10 21:16:57 2009 +0000
- Revision:
- 1:70d90598d2e7
- Parent:
- 0:57f4fdadc97f
- Child:
- 2:01115080f6da
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 | 1:70d90598d2e7 | 22 | void ServoMinder::moveToAndWait( float target ) |
| jarkman | 1:70d90598d2e7 | 23 | { |
| jarkman | 1:70d90598d2e7 | 24 | moveTo( target ); |
| jarkman | 1:70d90598d2e7 | 25 | |
| jarkman | 1:70d90598d2e7 | 26 | while( fabs( m_servo->read() - m_target) > m_delta ) |
| jarkman | 1:70d90598d2e7 | 27 | wait( 0.001 ) ; |
| jarkman | 1:70d90598d2e7 | 28 | } |
| jarkman | 1:70d90598d2e7 | 29 | |
| jarkman | 0:57f4fdadc97f | 30 | void ServoMinder::moveTo( float target ) |
| jarkman | 0:57f4fdadc97f | 31 | { |
| jarkman | 1:70d90598d2e7 | 32 | |
| jarkman | 0:57f4fdadc97f | 33 | m_target = target; |
| jarkman | 1:70d90598d2e7 | 34 | |
| jarkman | 0:57f4fdadc97f | 35 | |
| jarkman | 0:57f4fdadc97f | 36 | |
| jarkman | 0:57f4fdadc97f | 37 | } |
| jarkman | 0:57f4fdadc97f | 38 | |
| jarkman | 0:57f4fdadc97f | 39 | void ServoMinder::setSpeed( float speed ) |
| jarkman | 0:57f4fdadc97f | 40 | { |
| jarkman | 0:57f4fdadc97f | 41 | m_speed = speed; |
| jarkman | 0:57f4fdadc97f | 42 | m_delta = m_speed * m_tickTime; |
| jarkman | 0:57f4fdadc97f | 43 | } |
| jarkman | 0:57f4fdadc97f | 44 | |
| jarkman | 0:57f4fdadc97f | 45 | void ServoMinder::tick() |
| jarkman | 0:57f4fdadc97f | 46 | { |
| jarkman | 0:57f4fdadc97f | 47 | float pos = m_servo->read(); |
| jarkman | 0:57f4fdadc97f | 48 | |
| jarkman | 0:57f4fdadc97f | 49 | |
| jarkman | 0:57f4fdadc97f | 50 | if( pos < m_target ) |
| jarkman | 0:57f4fdadc97f | 51 | { |
| jarkman | 0:57f4fdadc97f | 52 | pos += m_delta; |
| jarkman | 0:57f4fdadc97f | 53 | if( pos > m_target ) |
| jarkman | 0:57f4fdadc97f | 54 | pos = m_target; |
| jarkman | 0:57f4fdadc97f | 55 | |
| jarkman | 0:57f4fdadc97f | 56 | // can't trace in here - breaks the cam protocol |
| jarkman | 0:57f4fdadc97f | 57 | //pcSerial.printf("servo to %f\r\n", pos); |
| jarkman | 0:57f4fdadc97f | 58 | |
| jarkman | 0:57f4fdadc97f | 59 | if( pos < 0 ) |
| jarkman | 0:57f4fdadc97f | 60 | pos = 0; |
| jarkman | 0:57f4fdadc97f | 61 | if( pos > 1.0 ) |
| jarkman | 0:57f4fdadc97f | 62 | pos = 1.0; |
| jarkman | 0:57f4fdadc97f | 63 | |
| jarkman | 0:57f4fdadc97f | 64 | m_servo->write( pos ); |
| jarkman | 0:57f4fdadc97f | 65 | } |
| jarkman | 0:57f4fdadc97f | 66 | else if( pos > m_target ) |
| jarkman | 0:57f4fdadc97f | 67 | { |
| jarkman | 0:57f4fdadc97f | 68 | pos -= m_delta; |
| jarkman | 0:57f4fdadc97f | 69 | if( pos < m_target ) |
| jarkman | 0:57f4fdadc97f | 70 | pos = m_target; |
| jarkman | 0:57f4fdadc97f | 71 | |
| jarkman | 0:57f4fdadc97f | 72 | if( pos < 0 ) |
| jarkman | 0:57f4fdadc97f | 73 | pos = 0; |
| jarkman | 0:57f4fdadc97f | 74 | if( pos > 1.0 ) |
| jarkman | 0:57f4fdadc97f | 75 | pos = 1.0; |
| jarkman | 0:57f4fdadc97f | 76 | //pcSerial.printf("servo to %f\r\n", pos); |
| jarkman | 0:57f4fdadc97f | 77 | m_servo->write( pos ); |
| jarkman | 0:57f4fdadc97f | 78 | } |
| jarkman | 0:57f4fdadc97f | 79 | |
| jarkman | 0:57f4fdadc97f | 80 | |
| jarkman | 0:57f4fdadc97f | 81 | } |
| jarkman | 0:57f4fdadc97f | 82 |