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
|
//
// motor.cc
//
#include "motors.h"
#include "config.h"
#include <Arduino.h>
#include <limits.h>
void Motors::setup()
{
// setup outputs
for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_CHANNELS; pin++ ) {
pinMode( pin, OUTPUT );
digitalWrite( pin, LOW );
}
}
static signed long calculate_duration( unsigned long then, unsigned long now )
{
// does it look like now has overflowed (and wrapped)?
if( now < then && now < ( ULONG_MAX / 2 ) && then > ( ULONG_MAX / 2 ) )
return now + ( ULONG_MAX - then );
// else, calculate duration
else
return now - then;
}
void Motors::update_channels( int channels[] )
{
static int event = NUM_CHANNELS * 2;
unsigned long now = micros();
static unsigned long frame_start = now - FRAME_DURATION;
static unsigned long next_event_at;
if( now >= next_event_at )
{
// action event
digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );
// move to next event
if( ++event >= NUM_CHANNELS * 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 );
}
}
}
|