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