Colony » Colony Scout

/**

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

#include "trafficNavigation.h"

#include "../linefollowing/lineDrive.h"

#define DEBUG_INTERSECTION

#ifdef MAIN_NEW

#ifdef DEBUG_INTERSECTION

  #define DBG_USBS(str) usb_puts(str)

  #define DBG_USBI(x) usb_puti(x)

#else

  #define DBG_USBS(str)

  #define DBG_USBI(x)

#endif

static int state, sign, turnDir;

static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;

unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;

bool done;

//TODO: classes for lineDrive::doDrive and other functions in line_drive.h, clock functions, sending/receiving packets, encoder data, initializations, get ids, etc.

void trafficNavigation::run()

{

	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */

	id = get_robotid();

	sign = 0;

	delay_ms(500);

	//Test code

	state = SROAD;

	sendBuffer[1] = id;

	while (ok()) {

		/*DTN Finite State Machine*/

		switch(state){

		case SROAD:/*Following a normal road*/

			start();

			done = false;

			//finishes when reads a barcode: TODO: how do we detect intersections now?

			while(!done){

				sign = lineDrive::doDrive(180);

				switch(sign){

					case NORMAL:

					case FINISHED:

					case LOST:

					case ERROR:

						break;

					default:

						//we have a barcode!

						state = SINTERSECTION_ENTER;

						DBG_USBS("Read Barcode #:");

						DBG_USBI(sign);

						DBG_USBS("\n");

						done = true;

						break;

				}

			}

			break;

		case SINTERSECTION_ENTER:/*Entering, and in intersection*/

			stop();

			lineDrive::doDrive(0);

			DBG_USBS("STATE: SINTERSECTION_ENTER\n");

			/*Intersection queue:

			 *Each robot when entering the intersection will check for other robots

			 *in the intersection, and insert itself in a queue to go through.

			 */

			queuePrevBot = -1; //the bot that will drive before this bot

			queueNextBot = -1; //the bot that will drive after this bot

			resolvPrevBotID = -3; //in the case of a race, the bot that is

			                      //to enter the queue before this bot

			resolvPrevBotDraw = 0; //the random priority number that bot has

			resolvDraw = 0; //my random priority number

			intersectionNum = getIntersectNum(sign);

			if(intersectionNum == (char) -1){ //invalid

				state = SROAD;

				DBG_USBS("Barcode has invalid intersectionNum\n");

				break;

			}

			turnDir = validateTurn(sign, getTurnType(sign));

			if(turnDir == (char) -1){ //invalid

				state = SROAD;

				DBG_USBS("Barcode has invalid turn\n");

				break;

			}

			trafficNavigation::enterIntersection(); //sends wireless packet for entry

			state = SINTERSECTION_WAIT;

			rtc_reset(); //reset rtc for timeout wait for reply

			done = false;

			char retried = 0;

			while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue

				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);

				if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier...

					trafficNavigation::enterIntersection();

					retried = 1;

				}

				switch (ret) {

					case KPLACEDINQUEUE:

						done = true;

						break;

					case KFAILEDTOQUEUE:

						DBG_USBS("Failed to queue\n");

						trafficNavigation::enterIntersection();

						rtc_reset();

						break;

					case KRESOLVINGENTER:

						state = SINTERSECTION_ENTER_RESOLV;

						done = true;

						break;

				}

			}

			break;

		case SINTERSECTION_ENTER_RESOLV:

			DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n");

			rtc_reset();

			done = false;

			retried = 0;

			while(rtc_get() < 4 && !done){

				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);

				switch (ret) {

					case KRESOLVINGENTER:

						break;

					case KPLACEDINQUEUE:

						done = true;

						break;

					case KFAILEDTOQUEUE:

						DBG_USBS("Failed to queue\n");

						trafficNavigation::enterIntersection();

						rtc_reset();

						break;

				}

				//if resolvPrevBotID == -1, this indicates that

				//there was a prevbot before, but it has entered

				//the queue so it's our turn.

				if(!done && resolvPrevBotID == (char) -1 && !retried){

					trafficNavigation::enterIntersection();

					rtc_reset();

					retried = 1;

				//if resolvPrevBotID == -2, we have been

				//resolving but never have seen a bot with lower

				//priority than us. after the 6/16ths sec

				//timeout, assume we are first.

				} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){

					//send a intersection reply to myself to

					//trigger other bots to enter queue.

					sendBuffer[0] = WINTERSECTIONREPLY;

					sendBuffer[2] = intersectionNum;

					sendBuffer[3] = id;

					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

					done = true;

					break;

				}

			}

			state = SINTERSECTION_WAIT;

			break;

		case SINTERSECTION_WAIT:/*Waiting in intersection */

			DBG_USBS("STATE: SINTERSECTION_WAIT\n");

			done = false;

			while(queuePrevBot != (char) -1){

				done = true;

				int ret = wirelessPacketHandle(state);

				if (ret==KFIRSTINQUEUE) state = SINTERSECTION_DRIVE;

			}

			//hack to make sure bot that just left intersection is

			//really out of the intersection.

			rtc_reset();

			while(rtc_get() < 2 && done){//wait one second

				wirelessPacketHandle(state);

			}

			state = SINTERSECTION_DRIVE;

			break;

		case SINTERSECTION_DRIVE:

			DBG_USBS("STATE: SINTERSECTION_DRIVE\n");

			start();

			turn(getIntersectType(sign), turnDir);

			//Exits intersection

			sendBuffer[0] = WINTERSECTIONEXIT;

			sendBuffer[2] = intersectionNum;//Intersection #

			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			state = SROAD;

			break;

		default:

			DBG_USBS("I got stuck in an unknown state (Likely highway) My state is ");

			DBG_USBI(state);

		}

	}

}

int trafficNavigation::wirelessPacketHandle(int state){

	int dataLength = 0;

	unsigned char *packet = NULL;

	packet = wl_basic_do_default(&dataLength);

	/* sanity check */

	if(dataLength == 0 || packet == NULL) //no packet

		return ENOPACKET;

	if(dataLength != PACKET_LENGTH)

		return EPACKETLEN;

	DBG_USBS("Recieved Wireless Packet: ");

	for(int i = 0; i < dataLength; i++){

		DBG_USBI(packet[i]);

		DBG_USBS(" ");

	}

	DBG_USBS("\n");

	switch (packet[0]){

		case WROADENTRY: //[type, bot, road]

			return ENOACTION;

			break;

		case WROADEXIT: //[type, bot, road]

			return ENOACTION;

			break;

		case WROADSTOP: //[type, bot, road]

			return ENOACTION;

			break;

		case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

						trafficNavigation::sendResolv(false);

						resolvPrevBotID = -2;

						return KRESOLVINGENTER;

						break;

					case SINTERSECTION_ENTER_RESOLV:

						return ENOACTION;

					case SINTERSECTION_WAIT:

					case SINTERSECTION_DRIVE:

						if(queueNextBot == (char) -1){

							sendBuffer[0] = WINTERSECTIONREPLY;

							sendBuffer[2] = intersectionNum;

							sendBuffer[3] = packet[1];

							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

							queueNextBot = packet[1];

							return KREPLIEDTOENTER;

						}

						break;

				}

			}

			break;

		case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

					case SINTERSECTION_ENTER_RESOLV:

						if(packet[3] == id){ //Reply for me

							queuePrevBot = packet[1];

							return KPLACEDINQUEUE;

						} else {

							if(packet[3] == resolvPrevBotID)

								resolvPrevBotID = -1;

							return KFAILEDTOQUEUE;

						}

						break;

					default:

						return ENOACTION;

				}

			}

			break;

		case WINTERSECTIONEXIT: //[type, bot, intersection]

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_WAIT:

						if(packet[1]==queuePrevBot){

							queuePrevBot=-1;

							return KFIRSTINQUEUE;

						}

				}

			}

			break;

		case WINTERSECTIONGO: //[type, bot, intersection]

			break;

		case WINTERSECTIONPOLICEENTRY: //[?]

			return ENOACTION;

			break;

		case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]

			//in the case robots draw the same number these will be

			//arbitrated using the sending robot's id, prefering

			//lower ids

			DBG_USBS("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");

			DBG_USBI((int) resolvPrevBotID);

			DBG_USBS("\n");

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

					case SINTERSECTION_ENTER_RESOLV:

						if(resolvPrevBotID == (char) -3){

							DBG_USBS("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");

							trafficNavigation::sendResolv(false);

							resolvPrevBotID = -2;

						}

						if((unsigned char) packet[3] == resolvDraw && id > packet[1]){

							//other bot drew same number as me,

							//and i have a higher id, so it goes first.

							DBG_USBS("bot ");

							DBG_USBI(packet[1]);

							DBG_USBS(" Drew the same number as me and I have a higher id, so it goes first\n");

							resolvPrevBotID = packet[1];

						} else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){

							//other bot drew same number as the bot before me,

							//so if it has a higher id than the bot before me,

							//it is going to go in between that one and me.

							DBG_USBS("bot ");

							DBG_USBI(packet[1]);

							DBG_USBS(" Drew the same number as the bot before me, and the bot before me has have a higher id, so this goes first\n");

							resolvPrevBotID = packet[1];

						} else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){

							//found a bot that goes in between the bot before me

							//and me, so now it is before me.

							DBG_USBS("bot ");

							DBG_USBI(packet[1]);

							DBG_USBS(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n");

							resolvPrevBotDraw = packet[3];

							resolvPrevBotID = packet[1];

						}

						return KRESOLVINGENTER;

						break;

					case SINTERSECTION_WAIT:

					case SINTERSECTION_DRIVE:

						DBG_USBS("Trying to resolv in non resolv case...queueNextbot = "); DBG_USBI(queueNextBot); DBG_USBS("\n");

						if(queueNextBot == (char) -1){

							trafficNavigation::sendResolv(true);

						}

						return KRESOLVINGENTER;

						break;

				}

			}

			break;

		case WHIGHWAYENTRY: //[type, bot, highway]

			return ENOACTION;

			break;

		case WHIGHWAYREPLY: //[type, fromBot, highway, toBot]

			return ENOACTION;

			break;

		case WHIGHWAYEXIT: //[type, bot, highway]

			return ENOACTION;

			break;

		case WPINGGLOBAL: //[type, bot]

			return ENOACTION;

			break;

		case WPINGBOT: //[type, fromBot, toBot]

			return ENOACTION;

			break;

		case WPINGQUEUE: //[type, fromBot, toBot]

			return ENOACTION;

			break;

		case WPINGREPLY: //[type, fromBot, toBot]

			return ENOACTION;

			break;

		case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces

			return ENOACTION;

			break;

		default:

			return ENOACTION;

			break;

	}

}

unsigned char trafficNavigation::resolvRandomNumberGen(){

	if ((resolvSeed *= (rtc_get() + encoder_read(LEFT))%9) == 0){

		return resolvSeed + 1; //0 is a reseved priority value for the last

		                 //bot that is already in the queue.

	}

	return resolvSeed;

}

void trafficNavigation::sendResolv(bool override){

	if(!override)

		resolvDraw = trafficNavigation::resolvRandomNumberGen();

	else

		resolvDraw = 0;

	sendBuffer[0] = WINTERSECTIONRESOLVERACE;

	sendBuffer[2] = intersectionNum;

	sendBuffer[3] = resolvDraw;

	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

}

void trafficNavigation::enterIntersection(void){

	//Sends packet announcing its entry to the intersection

	sendBuffer[0] = WINTERSECTIONENTRY;

	sendBuffer[2] = intersectionNum;//Intersection #

	sendBuffer[3] = 0; //getIntersectPos(sign);

	sendBuffer[4] = turnDir;

	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

}

/**

 * Copyright (c) 2011 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 */

#ifndef _trafficNavigation_H_

#define _trafficNavigation_H_

#include "../Behavior.h"

//from old trafficNavigation.h

#ifndef trafficNavigation_H

/* Debug Options - These must go before includes */

#define MAIN_NEW

//#define SENDGRAPH

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include "../linefollowing/lineDrive.h"

#include "highways.h"

#include "intersectData.h"

#include "validTurns.h"

/*States*/

#define SROAD 0

#define SINTERSECTION 9 /* for old version */

#define SINTERSECTION_ENTER 10

#define SINTERSECTION_ENTER_RESOLV 11

#define SINTERSECTION_WAIT 12

#define SINTERSECTION_DRIVE 13

#define SHIGHWAY 20

#define SCLEARPACKET 30

/*Sign Codes

 * bitwise OR labels to create a barcode or read one

 * There should be macros to extract these probably

 * The bits will be stored in some variable (char or short)

 * Bits if road: ? ? ? NAME NAME NAME TYPE CROAD

 * Bits if intersection: ? ? ? ? DIR DIR #WAYS CINTERSECTION

 */

#define CROAD 0x0 //0b

#define CINTERSECTION 0x1 //1b

#define CNORMALROAD 0x0 //00b

#define CHIGHWAYROAD 0x2 //10b

#define C4WAY 0x0 //00b

#define C3WAY 0x2 //10b

#define CNORTH 0x0 //0000b

#define CEAST 0x4 //0100b

#define CSOUTH 0x8 //1000b

#define CWEST 0x12 //1100b

/*Wireless Packet Types

 * The first byte of any wireless packet should be one of these types.

 * Each type will have its own structure

 * The second byte should be the id of the bot sending the packet

 * The third byte should be the number of the intersection or road that

 *   the packet pertains to

 */

#define PACKET_LENGTH 5

#define WROADENTRY 0 //[type, bot, road]

#define WROADREPLY 1 //[type, fromBot, road, toBot]

#define WROADEXIT 2 //[type, bot, road]

#define WROADSTOP 3 //[type, bot, road]

#define WINTERSECTIONENTRY 10   //[type, bot, intersection, fromDir, toDir]

#define WINTERSECTIONREPLY 11   //[type, fromBot, intersection, toBot]

#define WINTERSECTIONEXIT 12    //[type, bot, intersection]

#define WINTERSECTIONGO 13      //[type, bot, intersection]

#define WINTERSECTIONPOLICEENTRY 14

#define WINTERSECTIONRESOLVERACE 15   //[type, bot, intersection, num]

#define WHIGHWAYENTRY 20 //[type, bot, highway]

#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]

#define WHIGHWAYEXIT 22 //[type, bot, highway]

#define WPINGGLOBAL 30 //[type, bot]

#define WPINGBOT 31 //[type, fromBot, toBot]

#define WPINGQUEUE 32 //[type, fromBot, toBot]

#define WPINGREPLY 33 //[type, fromBot, toBot]

#define WCOLLISIONAVOID 41 //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces

#define WGRAPHDATA 80

/*Wireless Parsing Status

 * For wireless parsing - status codes describing various errors/statuses with wireless

 * parsing. 

 * Errors begin with E - Note that all of these errors are actually returned as negative.

 *                     - values of >= 100 indicate a serious error

 * Non-Errors begin with K - returned as positive. (don't start with S because S

 *                           is for state)

 */

#define ENOPACKET -10 //The received packet doesn't exist.

#define ENOACTION -15 //The received packet has no defined action, and this is OK

#define ENOIMPLEMENT -20 //The received packet has no defined action and needs to be implemented.

#define EPACKETLEN -100 //The received packet was the wrong length

#define KOK 1

#define KPLACEDINQUEUE 20

#define KFAILEDTOQUEUE 21

#define KFIRSTINQUEUE 22

#define KREPLIEDTOENTER 23

#define KRESOLVINGENTER 24 //resolving a race between bots trying to enter intersection at the same time.

/*Macros

 */

#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE)

#endif

//

#ifdef DEBUG_INTERSECTION

  #define DBG_USBS(str) usb_puts(str)

  #define DBG_USBI(x) usb_puti(x)

#else

  #define DBG_USBS(str)

  #define DBG_USBI(x)

#endif

class trafficNavigation : Behavior

{

    public:

        trafficNavigation(std::string scoutname) : Behavior(scoutname) {};

        void run();

		int wirelessPacketHandle(int state);

		void enterIntersection(void);

		void sendResolv(bool override);

		unsigned char resolvRandomNumberGen();

};

#endif