add protocol.message 'sender' field (at the end .......) | fix 'lcd' + use it to debug | create main algorithm for 'node' | fix XBeeWrapper.receive()

node/main/main.ino

@@ -4,7 +4,7 @@
 #include "xbee_wrapper.h" // WBeeWrapper
 
 // Peripherals
-LiquidCrystal_I2C screen(0x27, 16, 2);
+LiquidCrystal_I2C screen(0x20, 16, 2);
 XBeeWrapper xbee = XBeeWrapper();
 
 // ACTUAL DATA
@@ -14,19 +14,21 @@ Packet recv;
 Packet send;
 
 void setup() {
-  Serial.begin(38400);
+	Serial.begin(38400);
-  Serial.println("+ ready");
+	Serial.println("+ ready");
 
-  screen.begin();
+	screen.begin();
-  screen.backlight();
+	screen.backlight();
+	screen.home();
+	screen.setCursor(0, 0);
+	screen.print("ready");
+
+	xbee.begin(38400);
 
-  xbee.begin(38400);
 }
 
 void loop() {
 
-	delay(500);
-
 	// 1. ignore if no received data
 	if( xbee.receive(recv) != XBWRECV_OK )
 		return;
@@ -36,47 +38,58 @@ void loop() {
 
 	// DISCOVER REQUEST
 	if( opcode == 0 ) {
-		Serial.print("+ discover(");
+
-		Serial.print(recv.getSender());
+		// security
-		Serial.print(", ");
+		if( recv.getDist() == 0 && recv.getSender() != 255 )
-		Serial.print(recv.getWave());
+			return;
-		Serial.print(", ");
+
-		Serial.print(recv.getDist());
+
-		Serial.println(")");
+		status(wave_id, dist);
+		screen.print("dsc");
+		screen.print("[");
+		screen.print(recv.getSender());
+		screen.print("] -> ");
+		screen.print(recv.getDist());
+		screen.print("/");
+		screen.print(recv.getWave());
 
 		if( recv.getWave() != wave_id ){
 			wave_id = recv.getWave();
 			dist    = recv.getDist()+1;
-			recv.setDist(dist);
+
-			Serial.print("  + wave = ");
+			status(wave_id, dist);
-			Serial.println(wave_id);
+			screen.print("propagated");
-			Serial.print("  + dist = ");
+			delay(100);
-			Serial.println(dist);
 
 			send.setOpcode(0);
 			send.setWave(wave_id);
 			send.setDist(dist);
 			send.setSender(42);
 			xbee.broadcast(send);
-			Serial.print("  + send discover(");
+			Serial.print("  + send discover[");
 			Serial.print(send.getSender());
-			Serial.print(", ");
+			Serial.print("] -> ");
-			Serial.print(send.getWave());
-			Serial.print(", ");
 			Serial.print(send.getDist());
-			Serial.println(")");
+			Serial.print(" / ");
+			Serial.println(send.getWave());
+
 		}
 
 	// MESSAGE DATA
 	} else {
 
-		Serial.print("+ message(");
+		screen.clear();
-		Serial.print(recv.getDist());
+		screen.setCursor(0,0);
-		Serial.print(", ");
+		screen.print("message(");
-		Serial.print(recv.getTTL());
+		screen.print(recv.getDist());
-		Serial.print(", ");
+		screen.print(",");
-		Serial.print(recv.getSize());
+		screen.print(recv.getTTL());
-		Serial.println(")");
+		screen.print(",");
+		screen.print(recv.getSize());
+		screen.print(")");
+		screen.setCursor(0,1);
+		screen.print( (char*)recv.getData());
+		delay(600);
 
 
 		// propagation
@@ -98,3 +111,14 @@ void loop() {
 	}
 
 }
+
+
+
+void status(uint8_t wave, uint8_t dist){
+	screen.clear();
+	screen.setCursor(0,0);
+	screen.print(dist);
+	screen.print(" / ");
+	screen.print(wave);
+	screen.setCursor(0,1);
+}

packet.cpp

@@ -33,8 +33,8 @@ void Packet::setOpcode(uint8_t value) { opcode = value; dsc.opcode = value; msg.
 uint8_t Packet::getWave()           { return dsc.wave; }
 void Packet::setWave(uint8_t value) { dsc.wave = value; }
 
-uint8_t Packet::getSender()           { return dsc.sender; }
+uint8_t Packet::getSender()           { return (opcode == 0) ? dsc.sender : msg.sender; }
-void Packet::setSender(uint8_t value) { dsc.sender = value; }
+void Packet::setSender(uint8_t value) { if(opcode == 0) dsc.sender = value; else msg.sender = value; }
 
 uint8_t Packet::getDist()           { return (opcode == 0) ? dsc.dist : msg.dist; }
 void Packet::setDist(uint8_t value) { if(opcode == 0) dsc.dist = value; else msg.dist = value; }
@@ -75,12 +75,15 @@ uint8_t Packet::read_message(uint8_t buf[], size_t size){
 	msg.size   = buf[3];
 
 	// 3. check message size
-	if( size - 4 - 1 != msg.size )
+	if( size - 5 - 1 != msg.size )
 		return PKTREAD_INVALID_MESSAGE_FORMAT;
 
 	// 4. extract message
 	memcpy(msg.data, buf+4, msg.size);
 
+	// 5. add sender id
+	msg.sender = buf[msg.size+4+1];
+
 	return PKTREAD_OK;
 };
 
@@ -101,5 +104,7 @@ size_t Packet::write_message(uint8_t buf[]){
 
 	memcpy( buf+4, msg.data, msg.size );
 
+	buf[msg.size+5] = msg.sender;
+
 	return 4 + msg.size;
 };

protocol.h

@@ -15,14 +15,15 @@
 	};
 
 
-	#define MESSAGE_MIN_SIZE sizeof(uint8_t)*4
+	#define MESSAGE_MIN_SIZE sizeof(uint8_t)*5
-	#define MESSAGE_MAX_SIZE sizeof(uint8_t)*4+sizeof(uint8_t)*255
+	#define MESSAGE_MAX_SIZE sizeof(uint8_t)*5+sizeof(uint8_t)*255
 	struct message {
 		uint8_t opcode; // opcode = 1
 		uint8_t dist;   // distance of the last sender
 		uint8_t ttl;    // time to live default = 10
 		uint8_t size;   // size of message in bytes
-		uint8_t *data; // actual message
+		uint8_t *data;  // actual message
+		uint8_t sender; // sender id
 	};
 
 

well/main/main.ino

@@ -6,7 +6,7 @@
 #define WAVE_TIMEOUT 5000
 
 // Peripherals
-LiquidCrystal_I2C screen(0x27, 16, 2);
+LiquidCrystal_I2C screen(0x20, 16, 2);
 XBeeWrapper xbee = XBeeWrapper();
 
 // ACTUAL DATA
@@ -19,6 +19,9 @@ void setup() {
 
 	screen.begin();
 	screen.backlight();
+	screen.home();
+	screen.setCursor(0,0);
+	screen.print("ready");
 
 	xbee.begin(38400);
 }

xbee_wrapper.cpp

@@ -22,20 +22,20 @@ uint8_t XBeeWrapper::receive(Packet& pkt){
 
 	if( apiId == RX_16_RESPONSE ){
 
-		Rx16Response res;
+		Rx16Response res = Rx16Response();
 		xbee.getResponse().getRx16Response(res);
 
 		// extract data and propagate error
-		if( !pkt.read(res.getData(), res.getDataLength()) )
+		if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
 			return XBWRECV_ERROR;
 
 	} else {
 
-		Rx64Response res;
+		Rx64Response res = Rx64Response();
 		xbee.getResponse().getRx64Response(res);
 
 		// extract data and propagate error
-		if( !pkt.read(res.getData(), res.getDataLength()) )
+		if( pkt.read(res.getData(), res.getDataLength()) != PKTREAD_OK)
 			return XBWRECV_ERROR;
 
 	}