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