Colony

/*

 * main.c for Traffic Navigation

 * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG

 *

 * Author: Colony Project, CMU Robotics Club

 */

#include "traffic_navigation.h"

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

#ifdef MAIN_NEW

	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;

	int wirelessPacketHandle(int state);

	void enterIntersection(void);

	void sendResolv(bool override);

	unsigned char resolvRandomNumberGen();

int main (void) {

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

	dragonfly_init(ALL_ON);

	xbee_init();

	encoders_init();

	lineDrive_init();

	rtc_init(SIXTEENTH_SECOND, NULL);	

	wl_basic_init_default();

	wl_set_channel(13);

	initializeData();

	id = get_robotid();

	sign = 0;

	orb1_set_color(GREEN);

	delay_ms(1000);

	orb1_set_color(ORB_OFF);

	//Test code

	state = SROAD;

	sendBuffer[1] = id;

	/*

	doDrive(200);

	turn(DOUBLE, ILEFT);

	*/

	/*

	while(1)

		doDrive(255);

	*/

	while (1) {

		/*DTM Finite State Machine*/

		switch(state){

		case SROAD:/*Following a normal road*/

			/* implement other road behaviors?

			 *    -tailgating

			 */

#ifdef ORB_INTERSECTION

			orb1_set_color(WHITE);

			orb2_set_color(ORB_OFF);

#endif

			start();

			done = false;

			while(!done){

				sign = doDrive(200);

				switch(sign){

					case NORMAL:

					case FINISHED:

					case LOST:

					case ERROR:

						break;

					default:

						//we have a barcode!

						state = SINTERSECTION_ENTER;

						done = true;

						break;

				}

			}

			break;

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

			stop();

			doDrive(0);

#ifdef ORB_INTERSECTION

			orb1_set_color(RED);

			orb2_set_color(ORB_OFF);

#endif

#ifdef DEBUG_INTERSECTION

			usb_puts("STATE: SINTERSECTION_ENTER\n");

#endif

			/*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;

				break;

			}

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

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

				state = SROAD;

				break;

			}

			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() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue

				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);

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

#ifdef ORB_INTERSECTION

					orb2_set_color(PURPLE);

#endif

					enterIntersection();

					retried = 1;

				}

				switch (ret) {

					case KPLACEDINQUEUE:

#ifdef ORB_INTERSECTION

						orb2_set_color(GREEN);

#endif

						done = true;

						break;

					case KFAILEDTOQUEUE:

#ifdef DEBUG_INTERSECTION

						usb_puts("Failed to queue\n");

#endif

#ifdef ORB_INTERSECTION

						orb2_set_color(RED);

#endif

						enterIntersection();

						rtc_reset();

						break;

					case KRESOLVINGENTER:

#ifdef ORB_INTERSECTION

						orb2_set_color(ORANGE);

#endif

						state = SINTERSECTION_ENTER_RESOLV;

						done = true;

						break;

				}

			}

			break;

		case SINTERSECTION_ENTER_RESOLV:

#ifdef ORB_INTERSECTION

			orb1_set_color(PURPLE);

			orb2_set_color(ORB_OFF);

#endif

#ifdef DEBUG_INTERSECTION

			usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n");

#endif

			rtc_reset();

			done = false;

			retried = 0;

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

				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);

				switch (ret) {

					case KRESOLVINGENTER:

#ifdef ORB_INTERSECTION

						orb2_set_color(YELLOW);

#endif

						break;

					case KPLACEDINQUEUE:

#ifdef ORB_INTERSECTION

						orb2_set_color(GREEN);

#endif

						done = true;

						break;

					case KFAILEDTOQUEUE:

						usb_puts("Failed to queue\n");

						orb2_set_color(RED);

						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){

					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() > 6){

					//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 */

#ifdef ORB_INTERSECTION

			orb1_set_color(YELLOW);

			orb2_set_color(ORB_OFF);

#endif

#ifdef DEBUG_INTERSECTION

			usb_puts("STATE: SINTERSECTION_WAIT\n");

#endif

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

				int ret = wirelessPacketHandle(state);

				switch (ret){

					case KFIRSTINQUEUE:

#ifdef ORB_INTERSECTION

						orb2_set_color(GREEN);

#endif

						state = SINTERSECTION_DRIVE;

						break;

					case KREPLIEDTOENTER:

#ifdef ORB_INTERSECTION

						orb2_set_color(BLUE);

#endif

						break;

					case KRESOLVINGENTER:

#ifdef ORB_INTERSECTION

						orb2_set_color(ORANGE);

#endif

						break;

				}

			}

			state = SINTERSECTION_DRIVE;

			break;

		case SINTERSECTION_DRIVE:

#ifdef DEBUG_INTERSECTION

			usb_puts("STATE: SINTERSECTION_DRIVE\n");

#endif

#ifdef ORB_INTERSECTION

			orb1_set_color(GREEN);

			orb2_set_color(ORB_OFF);

#endif

			start();

			turn(getIntersectType(sign), turnDir);

			while(doDrive(200) != FINISHED){

			//while(!button2_click()){

				 int ret = wirelessPacketHandle(state);

				 switch (ret){

					 case KREPLIEDTOENTER:

#ifdef ORB_INTERSECTION

						orb2_set_color(BLUE);

#endif

						break;

					 case KRESOLVINGENTER:

#ifdef ORB_INTERSECTION

					 	orb2_set_color(ORANGE);

#endif

						break;

				 }

			 }

			//Exits intersection

#ifdef ORB_INTERSECTION

			orb1_set_color(WHITE);

#endif

			sendBuffer[0] = WINTERSECTIONEXIT;

			sendBuffer[2] = intersectionNum;//Intersection #

			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			//Exits intersection

			/*

			while(1){

				if(button1_click()){

					start();

					state = SHIGHWAY;

					break;

				}

				if(button2_click()){

					start();

					state = SROAD;

					break;

				}

			}*/

			state = SROAD;

			break;

		case SHIGHWAY:/*On highway*/

#ifdef ORB_INTERSECTION

			orb1_set_color(CYAN);

#endif

			while(!button1_click()){

				highwayFSM();

			}

			state = SINTERSECTION_ENTER;

			break;

		default:

			usb_puts("I got stuck in an unknown state! My state is ");

			usb_puti(state);

		}

	}

}

int 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;

	usb_puts("Recieved Wireless Packet: ");

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

		usb_puti(packet[i]);

		usb_putc(' ');

	}

	usb_putc('\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:

						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

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

			usb_puti((int) resolvPrevBotID);

			usb_putc('\n');

			if (packet[2] == intersectionNum){

				switch (state){

					case SINTERSECTION_ENTER:

					case SINTERSECTION_ENTER_RESOLV:

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

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

							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.

							usb_puts("bot ");

							usb_puti(packet[1]);

							usb_puts(" 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.

							usb_puts("bot ");

							usb_puti(packet[1]);

							usb_puts(" 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.

							usb_puts("bot ");

							usb_puti(packet[1]);

							usb_puts(" 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:

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

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

							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 resolvRandomNumberGen(){

	if ((resolvSeed *= (rtc_get())%9) == 0){

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

		                 //bot that is already in the queue.

	}

	return resolvSeed;

}

void sendResolv(bool override){

	if(!override)

		resolvDraw = resolvRandomNumberGen();

	else

		resolvDraw = 0;

	sendBuffer[0] = WINTERSECTIONRESOLVERACE;

	sendBuffer[2] = intersectionNum;

	sendBuffer[3] = resolvDraw;

	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

}

void 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);

}

#endif