/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-02-05 19:19:00 UTC
  • Revision ID: tim@ed.am-20140205191900-vsc9juuji1zjn7tc
added project dir

Show diffs side-by-side

added added

removed removed

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