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
92
93
94
|
//
// comms.cc
//
#include "comms.h"
#include "config.h"
#include <Arduino.h>
// the width of the display of a single channel (in chars)
#define GRAPH_SIZE 7
void Comms::setup()
{
Serial.begin( 9600 );
}
static void draw_graph( int channel_values[] )
{
// init graph
static char graph[ GRAPH_SIZE + 2 ];
static char inited_graph = false;
if( !inited_graph ) {
for( int a = 1; a < GRAPH_SIZE + 1; a++ )
graph[ a ] = '_';
graph[ 0 ] = '|';
graph[ GRAPH_SIZE + 1 ] = 0;
inited_graph = true;
}
// draw channels
for( int a = 0; a < NUM_CHANNELS; a++ ) {
unsigned long value = min( channel_values[ a ], MAX_CHANNEL_VALUE );
int pos = ( GRAPH_SIZE ) * value / MAX_CHANNEL_VALUE;
graph[ pos + 1 ] = '^';
Serial.print( graph );
graph[ pos + 1 ] = '_';
}
Serial.println( "|" );
}
void Comms::write_channels( int channel_values[] )
{
// draw_graph( channel_values );
Serial.write( -1 );
Serial.write( -1 );
unsigned char *data = reinterpret_cast< unsigned char * >( channel_values );
Serial.write( data, sizeof( channel_values ) * NUM_CHANNELS );
}
bool Comms::read_channels( int channel_values[] )
{
static unsigned char buf[ sizeof( int ) * ( NUM_MOTORS + 1 ) ];
static unsigned int buflen = 0;
bool frame_complete = false;
while( buflen < sizeof( buf ) && Serial.available() )
{
// read byte
buf[ buflen++ ] = Serial.read();
// check if we've got a frame
if( buflen > 1 &&
buf[ buflen - 1 ] == 0xff && buf[ buflen - 2 ] == 0xff )
{
// got a complete frame?
if( buflen == sizeof( buf ) ) {
frame_complete = true;
// copy channel values
unsigned char *data =
reinterpret_cast< unsigned char * >( channel_values );
memcpy( data, buf, sizeof( int ) * NUM_MOTORS );
}
// reset read buffer
buflen = 0;
}
}
// reset read buffer in the case where the buffer is full, but there's been
// no end-of-frame marker (i.e., bad data)
if( buflen >= sizeof( buf ) )
buflen = 0;
return frame_complete;
}
|