Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

Processes/Printing/Printing.cpp@32:ada943ecaceb, 2013-04-10 (annotated)

Committer:
madcowswe
Date:
Wed Apr 10 18:04:47 2013 +0000
Revision:
32:ada943ecaceb
Parent:
22:167dacfe0b14 
Made calibration more robust

Who changed what in which revision?

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