extern "C" int MAMain() {
InitConsole();
gConsoleLogging = 1;
printf("Hello World!\n");
tryToRead();
FREEZE;
}
bool ForestSerialPort::setTimeslot(int boardId, int timeslot, bool set) {
unsigned char buff[] = {0xFF, 0x06, 0x01, 0x01, 0x00, 0x00, 0x00};
buff[3] = boardId;
int a = serial.available();
unsigned char s[256];
if(a) {
printf("Found stuff in the buffer (%d bytes)\n", a);
serial.read(s, MIN(a, 256));
a = serial.available();
}
ofSleepMillis(5);
serial.write(buff, 7);
ofSleepMillis(5);
RodIdentity ident;
// read the current identity packet of the rod
if(tryToRead((unsigned char *)&ident, sizeof(ident))) {
if(set) {
// now set its timeslot
buff[4] = timeslot;
serial.write(buff, 7);
// read the acknowledgement
if(tryToRead((unsigned char *)&ident, sizeof(ident))) {
rodInfos[ident.deviceId] = RodInfo(ident.deviceId, ident.timeslot);
allRodInfos[ident.deviceId] = &rodInfos[ident.deviceId];
return true;
} else {
printf("Didn't get a response from rod with id %d\n", boardId);
}
} else {
return true;
}
} else {
//printf("Couldn't find rod with id %d\n", boardId);
}
return false;
}
// sends out a packet requesting data from lasers
// and also asks each laser to be on or off according
// to a bitmap
void ForestSerialPort::retrieve() {
if(currentCommandType==RETURN_RAW_ACCELEROMETER) {
for(int i = 0; i < rodInfos.size(); i++) {
RawAccelerometerData accel;
if(tryToRead((unsigned char*)&accel, sizeof(accel), 100)) {
if(rodInfos.find(accel.id)!=rodInfos.end()) {
if(accel.x!=0x40) {
rodInfos[accel.id].setRawData(accel);
} else {
// printf("'%s' Rod id %d accelerometer is faulty\n", serialNo.c_str(), accel.id); // TODO: re-enable
}
}
} else {
for(map<int,RodInfo>::iterator it = rodInfos.begin(); it != rodInfos.end(); it++) {
if((*it).second.timeslot==i+1) {
(*it).second.notifyTimeout();
break;
}
}
}
}
} else if(currentCommandType==RETURN_PROCESSED_MOTION_DATA) {
for(int i = 0; i < rodInfos.size(); i++) {
ProcessedAccelerometerData accel;
if(tryToRead((unsigned char*)&accel, sizeof(accel))) {
if(rodInfos.find(accel.id)!=rodInfos.end()) {
rodInfos[accel.id].setProcessedData(accel);
} else {
printf("'%s' Can't find the rod with that id (%d)\n", serialNo.c_str(), accel.id);
}
} else {
printf("'%s' Can't read accelerometer - no data\n", serialNo.c_str());
}
}
}
}
