/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 test/escs/main.ino

  • Committer: Tim Marston
  • Date: 2013-01-16 23:02:32 UTC
  • Revision ID: tim@ed.am-20130116230232-d3od7r9g99n0ra0w
added test programs for all 4 IMU sensors

Show diffs side-by-side

added added

removed removed

Lines of Context:
test/escs/main.ino
1
 
//
2
 
// main.ino
3
 
//
4
 
// Testing controlling ESCs (and motors).
5
 
 
6
 
 
7
 
#include <limits.h>
8
 
 
9
 
 
10
 
// number of ESCs
11
 
#define NUM_CHANNELS 4
12
 
 
13
 
// pin number of first channel
14
 
#define FIRST_PIN 4
15
 
 
16
 
// 0 <= channel value <= this constant
17
 
#define MAX_CHANNEL_VALUE 1000UL
18
 
 
19
 
// pulse width when channel value is 0 and MAX_CHANNEL_VALUE in microseconds
20
 
#define MIN_PULSE_WIDTH 1150UL
21
 
#define MAX_PULSE_WIDTH 1750UL
22
 
 
23
 
// interval between rising edges of successinve channels in microseconds
24
 
#define CHANNEL_INTERVAL ( MAX_PULSE_WIDTH + 500UL )
25
 
 
26
 
// duration of a whole frame (including the pulse) in microseconds
27
 
#define FRAME_DURATION 20000UL
28
 
 
29
 
 
30
 
// test ramping
31
 
#define RAMP_START 0
32
 
#define RAMP_MAX 900
33
 
 
34
 
// deltas (applied every 100 milliseconds)
35
 
#define RAMP_UP_DELTA 4
36
 
#define RAMP_DOWN_DELTA -10
37
 
 
38
 
 
39
 
void setup()
40
 
{
41
 
        // setup outputs
42
 
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_CHANNELS; pin++ ) {
43
 
                pinMode( pin, OUTPUT );
44
 
                digitalWrite( pin, LOW );
45
 
        }       
46
 
 
47
 
        Serial.begin( 9600 );
48
 
}
49
 
 
50
 
 
51
 
signed long calculate_duration( unsigned long then, unsigned long now )
52
 
{
53
 
        // does it look like now has overflowed (and wrapped)?
54
 
        if( now < then && now < ( ULONG_MAX / 2 ) && then > ( ULONG_MAX / 2 ) )
55
 
                return now + ( ULONG_MAX - then );
56
 
        
57
 
        // else, calculate duration
58
 
        else
59
 
                return now - then;
60
 
}
61
 
 
62
 
 
63
 
void update_channels( int channels[] )
64
 
{
65
 
        static int event = NUM_CHANNELS * 2;
66
 
        unsigned long now = micros();
67
 
        static unsigned long frame_start = now - FRAME_DURATION;
68
 
        static unsigned long next_event_at;
69
 
 
70
 
        if( now >= next_event_at )
71
 
        {
72
 
                // action event
73
 
                digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );
74
 
 
75
 
                // move to next event
76
 
                if( ++event >= NUM_CHANNELS * 2 ) {
77
 
                        event = 0;
78
 
                        frame_start += FRAME_DURATION;
79
 
                }
80
 
 
81
 
                // calculate the time that the next event will occur
82
 
                next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;
83
 
                if( event & 1 )
84
 
                        next_event_at += MIN_PULSE_WIDTH + channels[ event / 2 ] *
85
 
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
86
 
 
87
 
//              Serial.print( event / 2 );
88
 
//              Serial.print( ( event & 1 )? 'v' : '^' );
89
 
//              Serial.print( " " );
90
 
//              Serial.println( next_event_at );
91
 
 
92
 
                if( event == 1 ) {
93
 
                        unsigned long width = MIN_PULSE_WIDTH +
94
 
                                (unsigned long)channels[ event / 2 ] *
95
 
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
96
 
                        Serial.print( channels[ event / 2 ] );
97
 
                        Serial.print( " " );
98
 
                        Serial.println( width );
99
 
                }
100
 
        }
101
 
}
102
 
 
103
 
 
104
 
void loop()
105
 
{
106
 
        int channels[ NUM_CHANNELS ];
107
 
        for( int a = 0; a < NUM_CHANNELS; a++ )
108
 
                channels[ a ] = 0;
109
 
 
110
 
        // test ramping state
111
 
        unsigned long then = millis();
112
 
        int val = RAMP_START, delta = RAMP_UP_DELTA;
113
 
 
114
 
        while( true )
115
 
        {
116
 
                update_channels( channels );
117
 
 
118
 
                // update ramp value and set channel 0 value
119
 
                unsigned long now = millis();
120
 
                if( now > then + 100 ) {
121
 
                        then = now;
122
 
                        val += delta;
123
 
                        if( val >= RAMP_MAX ) {
124
 
                                val = RAMP_MAX;
125
 
                                delta = RAMP_DOWN_DELTA;
126
 
                        }
127
 
                        if( val <= 0 ) {
128
 
                                val = 0;
129
 
                                delta = 0;
130
 
                        }
131
 
                        channels[ 0 ] = val;
132
 
                }
133
 
        }
134
 
}