Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

Processes/Printing/Printing.cpp@12:d4b5851742a3, 2013-04-05 (annotated)

Committer:
twighk
Date:
Fri Apr 05 21:49:23 2013 +0000
Revision:
12:d4b5851742a3
Printing Thread prints stuff

Who changed what in which revision?

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