/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: dan
  • Date: 2014-04-12 19:32:21 UTC
  • Revision ID: dan-20140412193221-75teiczn7unc5kcf
Ignore extention ".kicad_pcb-bak"

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/rc-interface/motors.cc
5
5
 
6
6
#include "motors.h"
7
7
#include "config.h"
 
8
#include "common.h"
8
9
#include <Arduino.h>
9
10
#include <limits.h>
10
11
 
11
12
 
 
13
static int channels_[ NUM_MOTORS ];
 
14
 
 
15
 
12
16
void Motors::setup()
13
17
{
14
18
        // setup outputs
15
 
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_CHANNELS; pin++ ) {
 
19
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_MOTORS; pin++ ) {
16
20
                pinMode( pin, OUTPUT );
17
21
                digitalWrite( pin, LOW );
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;
 
22
        }
 
23
 
 
24
        // reset channel values
 
25
        for( int a = 0; a < NUM_MOTORS; a++ )
 
26
                channels_[ a ] = 0;
 
27
}
 
28
 
 
29
 
 
30
void Motors::set_values( int channel_values[] )
 
31
{
 
32
        for( int a = 0; a < NUM_MOTORS; a++ )
 
33
                channels_[ a ] = channel_values[ a ];
 
34
}
 
35
 
 
36
 
 
37
void Motors::update()
 
38
{
 
39
        static int event = NUM_MOTORS * 2;
37
40
        unsigned long now = micros();
38
41
        static unsigned long frame_start = now - FRAME_DURATION;
39
 
        static unsigned long next_event_at;
 
42
        static unsigned long next_event_at = 0;
40
43
 
41
44
        if( now >= next_event_at )
42
45
        {
43
46
                // action event
44
 
                digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );
 
47
                if( event < NUM_MOTORS * 2 )
 
48
                        digitalWrite( FIRST_PIN + ( event / 2 ),
 
49
                                ( event & 1 )? LOW : HIGH );
45
50
 
46
51
                // move to next event
47
 
                if( ++event >= NUM_CHANNELS * 2 ) {
 
52
                if( ++event >= NUM_MOTORS * 2 ) {
48
53
                        event = 0;
49
54
                        frame_start += FRAME_DURATION;
50
55
                }
52
57
                // calculate the time that the next event will occur
53
58
                next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;
54
59
                if( event & 1 )
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 );
 
60
                        next_event_at += MIN_PULSE_WIDTH + channels_[ event / 2 ] *
 
61
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
 
62
 
 
63
                // Serial.print( event / 2 );
 
64
                // Serial.print( ( event & 1 )? 'v' : '^' );
 
65
                // Serial.print( " " );
 
66
                // Serial.println( next_event_at );
 
67
 
 
68
                // if( event == 1 ) {
 
69
                //      unsigned long width = MIN_PULSE_WIDTH +
 
70
                //              (unsigned long)channels_[ event / 2 ] *
 
71
                //              ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
 
72
                //      Serial.print( channels_[ event / 2 ] );
 
73
                //      Serial.print( " " );
 
74
                //      Serial.println( width );
 
75
                // }
 
76
        }
 
77
 
 
78
        // else, if an event is coming up soon, sleep and service the event to make
 
79
        // sure that no other code runs and blocks us!
 
80
        else
 
81
        {
 
82
                long delay = calculate_duration( now, next_event_at );
 
83
                if( delay < 250 )
 
84
                {
 
85
                        if( delay > 20 )
 
86
                                delayMicroseconds( delay - 20 );
 
87
                        update();
70
88
                }
71
89
        }
 
90
        
72
91
}