/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-11-28 22:56:14 UTC
  • Revision ID: tim@ed.am-20131128225614-016og5owu7b0qp3k
test/escs: added ESC (motor control) testing program

Show diffs side-by-side

added added

removed removed

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
}