AnimatLab  2
Test
AnimatSerial.cpp
1 /*
2  AnimatSerial.cpp - Library for interfacing with Animat serial stream
3  Copyright (c) 2015 David Cofer, NeuroRobotic Technologies. All right reserved.
4 
5  This library is free software; you can redistribute it and/or
6  modify it under the terms of the GNU Lesser General Public
7  License as published by the Free Software Foundation; either
8  version 2.1 of the License, or (at your option) any later version.
9 
10  This library is distributed in the hope that it will be useful,
11  but WITHOUT ANY WARRANTY; without even the implied warranty of
12  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13  Lesser General Public License for more details.
14 
15  You should have received a copy of the GNU Lesser General Public
16  License along with this library; if not, write to the Free Software
17  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
18 */
19 
20 #include <Arduino.h>
21 #include "AnimatSerial.h"
22 
23 /* Constructor */
24 AnimatSerial::AnimatSerial(){
25  index = -1;
26  status = 0;
27  stream = NULL;
28 
29  inDataTotal = 0;
30  outDataTotal = 0;
31  inDataCount = 0;
32  outDataCount = 0;
33  inData = NULL;
34  outData = NULL;
35  changed = NULL;
36  dataChanged = false;
37 
38  messageID = -1;
39  packetSize = 0;
40 }
41 
42 AnimatSerial::AnimatSerial(HardwareSerial *ss, unsigned int inTotal, unsigned int outTotal){
43  index = -1;
44  status = 0;
45  stream = ss;
46 
47  inDataTotal = inTotal;
48  outDataTotal = outTotal;
49  inDataCount = 0;
50  outDataCount = 0;
51 
52  messageID = -1;
53  packetSize = 0;
54 
55  if(inDataTotal > 0)
56  {
57  inData = new AnimatData[inDataTotal];
58  changed = new bool[inDataTotal];
59  }
60 
61  if(outDataTotal > 0)
62  outData = new AnimatData[outDataTotal];
63 
64  clearInData();
65  clearChanged();
66  clearOutData();
67 }
68 
69 AnimatSerial::~AnimatSerial()
70 {
71  if(inData)
72  {
73  delete[] inData;
74  inData = NULL;
75  }
76 
77  if(outData)
78  {
79  delete[] outData;
80  outData = NULL;
81  }
82 }
83 
84 bool AnimatSerial::isChanged()
85 {
86  return dataChanged;
87 }
88 
89 void AnimatSerial::clearInData()
90 {
91  if(inData)
92  {
93  for(int i=0; i<inDataTotal; i++)
94  {
95  inData[i].value.fval = 0;
96  changed[i] = 0;
97  }
98  }
99 }
100 
101 void AnimatSerial::clearOutData()
102 {
103  if(outData)
104  {
105  for(int i=0; i<outDataTotal; i++)
106  outData[i].value.fval = 0;
107  }
108 }
109 
110 void AnimatSerial::clearChanged()
111 {
112  dataChanged = false;
113  if(changed)
114  {
115  for(int i=0; i<inDataTotal; i++)
116  changed[i] = false;
117  }
118 }
119 
120 bool AnimatSerial::isChanged(unsigned int index)
121 {
122  if(changed && index < inDataTotal)
123  return changed[index];
124  else
125  return false;
126 }
127 
128 bool AnimatSerial::getData(unsigned int index, AnimatData &data)
129 {
130  if(index < inDataTotal)
131  {
132  data = inData[index];
133  return true;
134  }
135  else
136  return false;
137 }
138 
139 bool AnimatSerial::addData(unsigned int id, float val)
140 {
141  if(outDataCount < outDataTotal)
142  {
143  outData[0].id.ival = id;
144  outData[0].value.fval = val;
145 
146  //Serial.print("Add Data: ");
147  //Serial.print(outData[0].id.ival, HEX);
148  //Serial.print(", ");
149  //Serial.print(outData[0].id.bval[0], HEX);
150  //Serial.print(", ");
151  //Serial.print(outData[0].id.bval[1], HEX);
152  //Serial.print(", ");
153  //Serial.print(outData[0].value.fval);
154  //Serial.println("");
155 
156  outDataCount++;
157  return true;
158  }
159  else
160  return false;
161 }
162 
163 void AnimatSerial::setInDataValue(int id, float val)
164 {
165  if(inData && changed && id < inDataTotal)
166  {
167  inData[id].value.fval = val;
168  changed[id] = true;
169  dataChanged = true;
170  }
171 }
172 
173 void AnimatSerial::begin(unsigned long baud){
174  if(stream != NULL)
175  stream->begin(baud);
176  else
177  {
178  stream = &Serial;
179  stream->begin(baud);
180  }
181 }
182 
183 /* process messages coming from CommanderHS
184  * format = 0xFF RIGHT_H RIGHT_V LEFT_H LEFT_V BUTTONS EXT CHECKSUM */
185 int AnimatSerial::readMsgs()
186 {
187  if(stream != NULL)
188  {
189  while(stream->available() > 0)
190  {
191  //Get first header byte
192  if(index == -1)
193  {
194  // looking for new packet
195  if(stream->read() == 0xff)
196  {
197  vals[0] = 0xff;
198  checksum = (int) vals[0];
199  index = 1;
200  messageID = -1;
201  packetSize = 0;
202  }
203  }
204  //Get second header byte
205  else if(index == 1)
206  {
207  vals[index] = (unsigned char) stream->read();
208  if(vals[index] == 0xff)
209  {
210  checksum += (int) vals[index];
211  index++;
212  }
213  else
214  index = -1; //Start over if the second byte is not 0xff
215  }
216  //Get the message ID
217  else if(index==2)
218  {
219  vals[index] = (unsigned char) stream->read();
220  messageID = vals[index];
221 
222  checksum += (int) vals[index];
223  index++;
224  }
225  //Get the message size byte 1
226  else if(index==3)
227  {
228  vals[index] = (unsigned char) stream->read();
229  packetSize = vals[index];
230 
231  checksum += (int) vals[index];
232  index++;
233  }
234  //Get the message size byte 2
235  else if(index==4)
236  {
237  vals[index] = (unsigned char) stream->read();
238  packetSize += (vals[index] << 8);
239 
240  //If the message size is greater than 128 then something is wrong. Start over.
241  if(packetSize > 128)
242  index = -1;
243  else
244  {
245  checksum += (int) vals[index];
246  index++;
247  }
248  }
249  else if(index > 4 && index <= packetSize)
250  {
251  vals[index] = (unsigned char) stream->read();
252 
253  if(index == (packetSize-1))
254  { // packet complete
255  //The transmitted checksum is the last entry
256  int iChecksum = vals[index];
257 
258  if(checksum%256 != iChecksum)
259  {
260  //Serial.println("Invalid Checksum. Sending resend msg.");
261  writeResendMsg();
262  // packet error!
263  index = -1;
264  }
265  else
266  {
267  //Serial.print("Found msg ID: ");
268  //Serial.println(messageID);
269  if(messageID == 2)
270  writeAllMsgs();
271  else if(messageID == 1)
272  {
273  int iStop = packetSize - 1; //Exclude the checksum at the end
274 
275  for(int iIdx=START_MESSAGE_INFO_BYTE; iIdx<iStop; iIdx+=DATA_SIZE)
276  {
277  id.bval[0] = vals[iIdx];
278  id.bval[1] = vals[iIdx+1];
279  value.bval[0] = vals[iIdx+2];
280  value.bval[1] = vals[iIdx+3];
281  value.bval[2] = vals[iIdx+4];
282  value.bval[3] = vals[iIdx+5];
283 
284  //Serial.print("A. Received Data ID: ");
285  //Serial.print(id.ival);
286  //Serial.print(", val: ");
287  //Serial.println(value.fval, 8);
288  //Serial.print(", Val: ");
289  //Serial.print(value.bval[0]);
290  //Serial.print(", ");
291  //Serial.print(value.bval[1]);
292  //Serial.print(", ");
293  //Serial.print(value.bval[2]);
294  //Serial.print(", ");
295  //Serial.println(value.bval[3]);
296  setInDataValue(id.ival, value.fval);
297  }
298  }
299  }
300 
301  index = -1;
302  stream->flush();
303  return 1;
304  }
305  else
306  {
307  checksum += (int) vals[index];
308  index++;
309  }
310 
311  if(index >= MAX_ANIMAT_BUFFER)
312  index = -1;
313  }
314 
315  }
316  }
317 
318  return 0;
319 }
320 
321 void AnimatSerial::writeMsgs(){
322  if(stream != NULL && outDataCount > 0) {
323  int checksum = 0xFF + 0xFF + 0x01;
324 
325  //First write the header
326  stream->write((byte) 0xFF);
327  stream->write((byte) 0xFF);
328  stream->write((byte) 0x01);
329  //Serial.print("0xFF, 0xFF, 0x01, ");
330 
331  size.ival = HEADER_SIZE + (DATA_SIZE * outDataCount) + FOOTER_SIZE;
332  stream->write((byte) size.bval[0]);
333  checksum += size.bval[0];
334  stream->write((byte) size.bval[1]);
335  checksum += size.bval[1];
336 
337  //Serial.print(size.bval[0], HEX);
338  //Serial.print(", ");
339  //Serial.print(size.bval[1], HEX);
340  //Serial.print(", ");
341 
342  for(int i=0; i<outDataCount; i++) {
343  stream->write((byte) outData[i].id.bval[0]); //
344  stream->write((byte) outData[i].id.bval[1]);
345  stream->write((byte) outData[i].value.bval[0]);
346  stream->write((byte) outData[i].value.bval[1]);
347  stream->write((byte) outData[i].value.bval[2]);
348  stream->write((byte) outData[i].value.bval[3]);
349 
350  checksum += outData[i].id.bval[0];
351  checksum += outData[i].id.bval[1];
352  checksum += outData[i].value.bval[0];
353  checksum += outData[i].value.bval[1];
354  checksum += outData[i].value.bval[2];
355  checksum += outData[i].value.bval[3];
356 
357  //Serial.print(outData[i].id.ival, HEX);
358  //Serial.print(", ");
359  //Serial.print(outData[i].id.bval[0], HEX);
360  //Serial.print(", ");
361  //Serial.print(outData[i].id.bval[1], HEX);
362  //Serial.print(", ");
363  //Serial.print(outData[i].value.bval[0], HEX);
364  //Serial.print(", ");
365  //Serial.print(outData[i].value.bval[1], HEX);
366  //Serial.print(", ");
367  //Serial.print(outData[i].value.bval[2], HEX);
368  //Serial.print(", ");
369  //Serial.print(outData[i].value.bval[3], HEX);
370  //Serial.print(", ");
371  }
372 
373  unsigned char bchecksum = (unsigned char) (checksum%256);
374  stream->write(bchecksum);
375 
376  //Serial.println(checksum, HEX);
377  //Serial.println("");
378  //Serial.println(bchecksum, HEX);
379  //Serial.println("");
380  //Serial.print("Size: ");
381  //Serial.println(size.ival);
382 
383  outDataCount = 0;
384  }
385 }
386 
387 void AnimatSerial::writeResendMsg(){
388  if(stream != NULL) {
389  int checksum = 0xFF + 0xFF + 0x02;
390 
391  //First write the header
392  stream->write((byte) 0xFF);
393  stream->write((byte) 0xFF);
394  stream->write((byte) 0x02);
395  //Serial.print("Writing Resend message 0xFF, 0xFF, 0x02, ");
396 
397  size.ival = HEADER_SIZE + FOOTER_SIZE;
398  stream->write((byte) size.bval[0]);
399  checksum += size.bval[0];
400  stream->write((byte) size.bval[1]);
401  checksum += size.bval[1];
402 
403  //Serial.print(size.bval[0], HEX);
404  //Serial.print(", ");
405  //Serial.print(size.bval[1], HEX);
406  //Serial.print(", ");
407 
408  unsigned char bchecksum = (unsigned char) (checksum%256);
409  stream->write(bchecksum);
410 
411  //Serial.println(checksum, HEX);
412  //Serial.println("");
413  //Serial.println(bchecksum, HEX);
414  //Serial.println("");
415  //Serial.print("Size: ");
416  //Serial.println(size.ival);
417  }
418 }
419 
420 void AnimatSerial::writeAllMsgs(){
421  //Serial.println("Writing resend message");
422  if(stream != NULL && outDataTotal > 0) {
423  for(int i=0; i<outDataTotal; i++)
424  addData(i, outData[i].value.fval);
425 
426  writeMsgs();
427  }
428 }