/brmbot

To get this branch, use:
bzr branch /brmbot
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
1
//
2
// brmprotocol.h
3
// Copyright (c) 2011 Dominik Joe Pantucek <joe@joe.cz>
4
//
5
// Implements BRMProtocol (see http://brmlab.cz/brmbot_outdoor/protocol)
6
//
7
#ifndef __brmprotocol_h
8
#define __brmprotocol_h
9
10
#include "WProgram.h"
11
12
// Maximum message size
13
#define BRMPROTO_BUFFER_SIZE 50
14
15
// Maximum number of message parameters
16
#define BRMPROTO_MAX_FIELDS 10
17
18
// Maximum number of commands (uppercase letters)
19
#define BRMPROTO_MAX_COMMANDS 26
20
125 by Dominik Joe Pantucek
Add newest brmprotocol for arduino.
21
// Should we allow ending messages with ';'?
22
// #define BRMPROTO_MSG_SEMICOLON
23
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
24
// Should we keep the followin message flags for each message?
25
// #define BRMPROTO_MSGFLAGS
26
27
// Possible error states
28
#define BRMERR_TOO_LONG 1
29
#define BRMERR_MANY_FIELDS 2
30
#define BRMERR_INVALID_COMMAND 4
31
32
// Error codes for client
33
#define BRMPROTO_ERR_COUNT 1
34
#define BRMPROTO_ERR_FIELDS 2
35
#define BRMPROTO_ERR_COMMAND_FORMAT 3
36
#define BRMPROTO_ERR_COMMAND 4
106 by Dominik Joe Pantucek
Add user-defined error codes base and read serial line in a while loop.
37
#define BRMPROTO_ERR_USER 100
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
38
39
// Type used for registering functions
40
typedef void (*BRMProtoFunc)(int,char*[]);
41
42
//
43
// Represents protocol on port encapsulation
44
class BRMProtocol{
45
public:
46
47
  //
48
  // Initializes on default port
119 by Dominik Joe Pantucek
Implement BRMProtocol.begin() because Arduino cannot initialize serial port correctly in static object constructor.
49
  BRMProtocol(){
50
    BRMProtocol::BRMProtocol(Serial);
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
51
  }
52
  
53
  //
54
  // Initializes on specific port
119 by Dominik Joe Pantucek
Implement BRMProtocol.begin() because Arduino cannot initialize serial port correctly in static object constructor.
55
  BRMProtocol(HardwareSerial &port){
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
56
    serial=&port;
57
    bufsize=0;
108 by Dominik Joe Pantucek
Fix off-by-one missing handling of 0th field.
58
    numfields=1;
59
    fields[0]=buffer;
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
60
#ifdef BRMPROTO_MSGFLAGS
61
    msgflags=0;
62
#endif
63
    countvalid=0;
64
  }
65
  
66
  //
67
  // Release serial port
68
  ~BRMProtocol(){
69
    serial->end();
70
  }
71
  
72
  //
119 by Dominik Joe Pantucek
Implement BRMProtocol.begin() because Arduino cannot initialize serial port correctly in static object constructor.
73
  // Initialize the serial port - somehow this doesn't work in constructor of static object :(
74
  void begin(int _speed=9600) {
75
    serial->begin(_speed);
76
  }
77
  
78
  //
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
79
  // Single iteration of reading loop
80
  void read(){
106 by Dominik Joe Pantucek
Add user-defined error codes base and read serial line in a while loop.
81
    while (serial->available()) {
125 by Dominik Joe Pantucek
Add newest brmprotocol for arduino.
82
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
83
      // Data waiting, let's get 'em
84
      char c=serial->read();
125 by Dominik Joe Pantucek
Add newest brmprotocol for arduino.
85
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
86
      // Add it to buffer only if we have enough space or if it's the last character
125 by Dominik Joe Pantucek
Add newest brmprotocol for arduino.
87
      if ((bufsize<BRMPROTO_BUFFER_SIZE-1)||(c=='\n')
88
#ifdef BRMPROTO_MSG_SEMICOLON
89
            ||(c==';')
90
#endif
91
            ) {
92
        if ((c=='\n')
93
#ifdef BRMPROTO_MSG_SEMICOLON
94
            ||(c==';')
95
#endif
96
            ) {
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
97
          // If this is complete message, dispatch it
98
          buffer[bufsize++]=0;
99
          process_message();
100
          
101
          // Start over
102
          bufsize=0;
108 by Dominik Joe Pantucek
Fix off-by-one missing handling of 0th field.
103
          numfields=1;
104
          fields[0]=buffer;
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
105
#ifdef BRMPROTO_MSGFLAGS
106
          msgflags=0;
107
#endif
107 by Dominik Joe Pantucek
Make sure we do not get stuck if we're receiving data too fast.
108
          // Make sure we do not get stuck if we're receiving data too fast
109
          break;
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
110
        } else if (c==' ') {
111
          // If this is field delimiter, create new field
112
          buffer[bufsize++]=0;
113
          if (numfields<BRMPROTO_MAX_FIELDS)
114
            fields[numfields++]=buffer+bufsize;
115
#ifdef BRMPROTO_MSGFLAGS
116
          else
117
            msgflags|=BRMERR_MANY_FIELDS;
118
#endif
119
        } else {
108 by Dominik Joe Pantucek
Fix off-by-one missing handling of 0th field.
120
          // Just add character to the buffer
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
121
          buffer[bufsize++]=c;
122
        }
123
      }
124
#ifdef BRMPROTO_MSGFLAGS
125
      else
126
        msgflags|=BRMERR_TOO_LONG;
127
#endif
128
    }
129
  }
130
  
131
  //
132
  // Registers function in dispatch table
133
  int add(char c,BRMProtoFunc func) {
134
    int cmd=char2cmd(c);
135
    if (cmd<0)
136
      return BRMERR_INVALID_COMMAND;
137
    commands[cmd]=func;
138
    return 0;
139
  }
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
140
  
141
  //
142
  // Sends beginning of reply to serial port
138 by Dominik Joe Pantucek
Fix replies and add nice Motor representation.
143
  void reply(int space=0) {
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
144
    send_count();
145
    serial->print(fields[1]);
146
    if (space)
147
      serial->print(" ");
148
  }
149
  
150
  //
151
  // Sends error reply
152
  void error(int number,int args=0) {
153
    send_count();
154
    serial->print("R ");
155
    send_number(number,args);
156
  }
157
158
  //
159
  // Sends beginning of reply  
160
  void send_count() {
161
    if (countvalid)
162
      serial->print(fields[0]);
163
    else
164
      serial->print("-1");
165
    serial->print(" ");
166
  }
167
  
168
  //
169
  // Sends alert message
170
  void alert(int number,int args=0) {
116 by Dominik Joe Pantucek
Change alert id to -2 constant.
171
    serial->print("-2 A ");
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
172
    send_number(number,args);
173
  }
174
  
175
  //
176
  // Sends number and optional space
177
  void send_number(int number,int args=0) {
178
    if (args) {
179
      serial->print(number);
180
      serial->print(" ");
181
    } else
182
      serial->println(number);
183
  }
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
184
185
  // Holds specific serial port it uses
186
  HardwareSerial *serial;
187
  
137 by Dominik Joe Pantucek
Incremental brmdriver development.
188
private:
189
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
190
  // Message buffer
191
  char *fields[BRMPROTO_MAX_FIELDS];
192
  int numfields;
193
  char buffer[BRMPROTO_BUFFER_SIZE];
194
  int bufsize;
195
  int countvalid;
196
  
197
  // Message error flags
198
  int msgflags;
199
  
200
  // Function dispatch table
201
  BRMProtoFunc commands[BRMPROTO_MAX_COMMANDS];
202
  
203
  /*
204
  //
205
  // Quick character to decimal number converter
206
  inline int char2dec(char c) {
207
    if ((c>='0')&&(c<='9'))
208
      return c-'0';
209
    return -1;
210
  }
211
  
212
  //
213
  // Converts two-digit string to decimal number
214
  int str2count(char *str) {
215
    int count=(char2dec(str[0])*10)+char2dec(str[1]);
216
    if (count<0)
217
      count=-1;
218
    return count;
219
  }
220
  /**/
221
222
  //
223
  // Returns canonical command number
224
  int char2cmd(char c) {
225
    if ((c>='A')&&(c<='Z'))
226
      return c-'A';
227
    return -1;
228
  }
229
  
230
  //
231
  // Processes current message and dispatches it
232
  void process_message() {
233
    // Validate message
234
    countvalid=0;
235
    if (numfields>=1) {
236
      if (strlen(fields[0])==2) {
237
        // We have at least one field and its length is two
238
        countvalid=(fields[0][0]>='0')&&(fields[0][0]<='9')&&(fields[0][1]>='0')&&(fields[0][1]<='9');
239
      }
240
    }
241
    if (!countvalid) {
242
      // If we couldn't parse the count number, it's error
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
243
      error(BRMPROTO_ERR_COUNT);
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
244
      return;
245
    }
246
    if (numfields<2) {
247
      // If we have less than two fields, it's error
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
248
      error(BRMPROTO_ERR_FIELDS);
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
249
      return;
250
    }
251
    if (strlen(fields[1])!=1) {
252
      // If command letter isn't single letter, it's error
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
253
      error(BRMPROTO_ERR_COMMAND_FORMAT);
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
254
      return;
255
    }
256
    
257
    // Get command
258
    int cmd=char2cmd(fields[1][0]);
259
    if ((cmd<0)||(cmd>=BRMPROTO_MAX_COMMANDS))
105 by Dominik Joe Pantucek
Add alert(), error() and reply() functions.
260
      error(BRMPROTO_ERR_COMMAND);
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
261
    else
125 by Dominik Joe Pantucek
Add newest brmprotocol for arduino.
262
      if (commands[cmd])
263
        commands[cmd](numfields,fields);
264
      else
265
        error(BRMPROTO_ERR_COMMAND);
104 by Dominik Joe Pantucek
Split BRMProtocol as separate 'library'.
266
  }
267
  
268
}; // BRMProtocol
269
270
#endif // __brmprotocol_h
271