/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter

« back to all changes in this revision

Viewing changes to src/rc-interface/motors.cc

  • Committer: Tim Marston
  • Date: 2014-01-15 22:18:56 UTC
  • Revision ID: tim@ed.am-20140115221856-lcqttlborwi7ot7k
rc-interface: mostly completed it (untested, but rx and tx code should work)

Show diffs side-by-side

added added

removed removed

src/rc-interface/motors.cc
9
9
#include <limits.h>
10
10
 
11
11
 
12
 
static int channels_[ NUM_MOTORS ];
13
 
 
14
 
 
15
12
void Motors::setup()
16
13
{
17
14
        // setup outputs
18
 
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_MOTORS; pin++ ) {
 
15
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_CHANNELS; pin++ ) {
19
16
                pinMode( pin, OUTPUT );
20
17
                digitalWrite( pin, LOW );
21
 
        }
22
 
 
23
 
        // reset channel values
24
 
        for( int a = 0; a < NUM_MOTORS; a++ )
25
 
                channels_[ a ] = 0;
26
 
}
27
 
 
28
 
 
29
 
void Motors::set_values( int channel_values[] )
30
 
{
31
 
        for( int a = 0; a < NUM_MOTORS; a++ )
32
 
                channels_[ a ] = channel_values[ a ];
33
 
}
34
 
 
35
 
 
36
 
void Motors::update()
37
 
{
38
 
        static int event = NUM_MOTORS * 2;
 
18
        }       
 
19
}
 
20
 
 
21
 
 
22
static signed long calculate_duration( unsigned long then, unsigned long now )
 
23
{
 
24
        // does it look like now has overflowed (and wrapped)?
 
25
        if( now < then && now < ( ULONG_MAX / 2 ) && then > ( ULONG_MAX / 2 ) )
 
26
                return now + ( ULONG_MAX - then );
 
27
        
 
28
        // else, calculate duration
 
29
        else
 
30
                return now - then;
 
31
}
 
32
 
 
33
 
 
34
void Motors::update_channels( int channels[] )
 
35
{
 
36
        static int event = NUM_CHANNELS * 2;
39
37
        unsigned long now = micros();
40
38
        static unsigned long frame_start = now - FRAME_DURATION;
41
 
        static unsigned long next_event_at = 0;
 
39
        static unsigned long next_event_at;
42
40
 
43
41
        if( now >= next_event_at )
44
42
        {
45
43
                // action event
46
 
                if( event < NUM_MOTORS * 2 )
47
 
                        digitalWrite( FIRST_PIN + ( event / 2 ),
48
 
                                ( event & 1 )? LOW : HIGH );
 
44
                digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );
49
45
 
50
46
                // move to next event
51
 
                if( ++event >= NUM_MOTORS * 2 ) {
 
47
                if( ++event >= NUM_CHANNELS * 2 ) {
52
48
                        event = 0;
53
49
                        frame_start += FRAME_DURATION;
54
50
                }
56
52
                // calculate the time that the next event will occur
57
53
                next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;
58
54
                if( event & 1 )
59
 
                        next_event_at += MIN_PULSE_WIDTH + channels_[ event / 2 ] *
60
 
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
61
 
 
62
 
                // Serial.print( event / 2 );
63
 
                // Serial.print( ( event & 1 )? 'v' : '^' );
64
 
                // Serial.print( " " );
65
 
                // Serial.println( next_event_at );
66
 
 
67
 
                // if( event == 1 ) {
68
 
                //      unsigned long width = MIN_PULSE_WIDTH +
69
 
                //              (unsigned long)channels_[ event / 2 ] *
70
 
                //              ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
71
 
                //      Serial.print( channels_[ event / 2 ] );
72
 
                //      Serial.print( " " );
73
 
                //      Serial.println( width );
74
 
                // }
 
55
                        next_event_at += MIN_PULSE_WIDTH + channels[ event / 2 ] *
 
56
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
 
57
 
 
58
//              Serial.print( event / 2 );
 
59
//              Serial.print( ( event & 1 )? 'v' : '^' );
 
60
//              Serial.print( " " );
 
61
//              Serial.println( next_event_at );
 
62
 
 
63
                if( event == 1 ) {
 
64
                        unsigned long width = MIN_PULSE_WIDTH +
 
65
                                (unsigned long)channels[ event / 2 ] *
 
66
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
 
67
                        Serial.print( channels[ event / 2 ] );
 
68
                        Serial.print( " " );
 
69
                        Serial.println( width );
 
70
                }
75
71
        }
76
72
}