/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: 2013-11-20 23:58:23 UTC
  • Revision ID: tim@ed.am-20131120235823-q9xheffre1y00lhd
fixed pulse-width test

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/rc-interface/motors.cc
1
 
//
2
 
// motor.cc
3
 
//
4
 
 
5
 
 
6
 
#include "motors.h"
7
 
#include "config.h"
8
 
#include "common.h"
9
 
#include <Arduino.h>
10
 
#include <limits.h>
11
 
 
12
 
 
13
 
static int channels_[ NUM_MOTORS ];
14
 
 
15
 
 
16
 
void Motors::setup()
17
 
{
18
 
        // setup outputs
19
 
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_MOTORS; pin++ ) {
20
 
                pinMode( pin, OUTPUT );
21
 
                digitalWrite( pin, LOW );
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;
40
 
        unsigned long now = micros();
41
 
        static unsigned long frame_start = now - FRAME_DURATION;
42
 
        static unsigned long next_event_at = 0;
43
 
 
44
 
        if( now >= next_event_at )
45
 
        {
46
 
                // action event
47
 
                if( event < NUM_MOTORS * 2 )
48
 
                        digitalWrite( FIRST_PIN + ( event / 2 ),
49
 
                                ( event & 1 )? LOW : HIGH );
50
 
 
51
 
                // move to next event
52
 
                if( ++event >= NUM_MOTORS * 2 ) {
53
 
                        event = 0;
54
 
                        frame_start += FRAME_DURATION;
55
 
                }
56
 
 
57
 
                // calculate the time that the next event will occur
58
 
                next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;
59
 
                if( event & 1 )
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();
88
 
                }
89
 
        }
90
 
        
91
 
}