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