Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

Processes/Printing/Printing.cpp@20:70d651156779, 2013-04-09 (annotated)

Committer:
madcowswe
Date:
Tue Apr 09 15:33:36 2013 +0000
Revision:
20:70d651156779
Predict loop running, update loop not done.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 20:70d651156779 1 #include "Printing.h"
madcowswe 20:70d651156779 2 #ifdef PRINTINGOFF
madcowswe 20:70d651156779 3 void printingThread(void const*){Thread::wait(osWaitForever);}
madcowswe 20:70d651156779 4 bool registerID(char, size_t){return true;}
madcowswe 20:70d651156779 5 bool unregisterID(char) {return true;}
madcowswe 20:70d651156779 6 bool updateval(char, float*, size_t){return true;}
madcowswe 20:70d651156779 7 bool updateval(char id, float value){return true;}
madcowswe 20:70d651156779 8 #else
madcowswe 20:70d651156779 9 #include <iostream>
madcowswe 20:70d651156779 10 using namespace std;
madcowswe 20:70d651156779 11
madcowswe 20:70d651156779 12 size_t idlist[NUMIDS]; // Stores length of buffer 0 => unassigned
madcowswe 20:70d651156779 13 float* buffarr[NUMIDS];
madcowswe 20:70d651156779 14 volatile unsigned int newdataflags;
madcowswe 20:70d651156779 15
madcowswe 20:70d651156779 16 bool registerID(char id, size_t length) {
madcowswe 20:70d651156779 17 if (id < NUMIDS && !idlist[id]) {//check if the id is already taken
madcowswe 20:70d651156779 18 idlist[id] = length;
madcowswe 20:70d651156779 19 buffarr[id] = new float[length];
madcowswe 20:70d651156779 20 return true;
madcowswe 20:70d651156779 21 } else
madcowswe 20:70d651156779 22 return false;
madcowswe 20:70d651156779 23 }
madcowswe 20:70d651156779 24 bool unregisterID(char id) {
madcowswe 20:70d651156779 25 if (id < NUMIDS) {
madcowswe 20:70d651156779 26 idlist[id] = 0;
madcowswe 20:70d651156779 27 if (buffarr[id])
madcowswe 20:70d651156779 28 delete buffarr[id];
madcowswe 20:70d651156779 29 return true;
madcowswe 20:70d651156779 30 } else
madcowswe 20:70d651156779 31 return false;
madcowswe 20:70d651156779 32 }
madcowswe 20:70d651156779 33
madcowswe 20:70d651156779 34 bool updateval(char id, float* buffer, size_t length) {
madcowswe 20:70d651156779 35 //check if the id is registered, and has buffer of correct length
madcowswe 20:70d651156779 36 if (id < NUMIDS && idlist[id] == length && buffarr[id] && !(newdataflags & (1<<id))) {
madcowswe 20:70d651156779 37 for (size_t i = 0; i < length; i++)
madcowswe 20:70d651156779 38 buffarr[id][i] = buffer[i];
madcowswe 20:70d651156779 39 newdataflags |= (1<<id);
madcowswe 20:70d651156779 40 return true;
madcowswe 20:70d651156779 41 } else
madcowswe 20:70d651156779 42 return false;
madcowswe 20:70d651156779 43 }
madcowswe 20:70d651156779 44
madcowswe 20:70d651156779 45 bool updateval(char id, float value){
madcowswe 20:70d651156779 46 //check if the id is registered, and the old value has been written
madcowswe 20:70d651156779 47 if (id < NUMIDS && idlist[id] == 1 && buffarr[id] && !(newdataflags & (1<<id))) {
madcowswe 20:70d651156779 48 buffarr[id][0] = value;
madcowswe 20:70d651156779 49 newdataflags |= (1<<id);
madcowswe 20:70d651156779 50 return true;
madcowswe 20:70d651156779 51 } else
madcowswe 20:70d651156779 52 return false;
madcowswe 20:70d651156779 53 }
madcowswe 20:70d651156779 54
madcowswe 20:70d651156779 55 void printingThread(void const*){
madcowswe 20:70d651156779 56 newdataflags = 0;
madcowswe 20:70d651156779 57 for (int i = 0; i < NUMIDS; i++) {
madcowswe 20:70d651156779 58 idlist[i] = 0;
madcowswe 20:70d651156779 59 buffarr[i] = 0;
madcowswe 20:70d651156779 60 }
madcowswe 20:70d651156779 61
madcowswe 20:70d651156779 62
madcowswe 20:70d651156779 63 Thread::wait(3500);
madcowswe 20:70d651156779 64 while(true){
madcowswe 20:70d651156779 65 // Send number of packets
madcowswe 20:70d651156779 66 char numtosend = 0;
madcowswe 20:70d651156779 67 for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}
madcowswe 20:70d651156779 68 cout.put(numtosend);
madcowswe 20:70d651156779 69
madcowswe 20:70d651156779 70 // Send packets
madcowswe 20:70d651156779 71 for (char id = 0; id < NUMIDS; id++) {
madcowswe 20:70d651156779 72 if (newdataflags & (1<<id)) {
madcowswe 20:70d651156779 73 cout.put(id);
madcowswe 20:70d651156779 74 cout.write((char*)buffarr[id], idlist[id] * sizeof(float));
madcowswe 20:70d651156779 75 newdataflags &= ~(1<<id);
madcowswe 20:70d651156779 76 }
madcowswe 20:70d651156779 77 }
madcowswe 20:70d651156779 78 cout << endl;
madcowswe 20:70d651156779 79 Thread::wait(200);
madcowswe 20:70d651156779 80 }
madcowswe 20:70d651156779 81 }
madcowswe 20:70d651156779 82 #endif
madcowswe 20:70d651156779 83
madcowswe 20:70d651156779 84
madcowswe 20:70d651156779 85