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"

	static int state, sign, dataLength, pingWaitTime, turnDir;

	static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;

	static unsigned char *packet;

	void enterIntersectionQueue(int sign);

	void waitInIntersectionQueue();

	void driveThroughIntersection(int sign);

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(14);// Temporary, restore to 13

	initializeData();

	id = get_robotid();

	sign = 0;

	//Test code

	state = SINTERSECTION;

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

//			sign = lineFollow();

			//other road behaviors

				//tailgating?

			//read barcode

//			sign = doDrive(200);

			if(button1_click()){

				state = SINTERSECTION;

			}

			break;

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

			intersectionNum = 0;

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

			 */

			prevBot = 0;

			nextBot = 0;

			sign = 0; //Test code until barcodes integrated

			enterIntersectionQueue(sign);

			orb1_set_color(PURPLE);

			//waits for its turn

			waitInIntersectionQueue();

			orb1_set_color(RED);

			//Drives through intersection

			driveThroughIntersection(sign);

			//Exits intersection

			sendBuffer[0] = WINTERSECTIONEXIT;

			sendBuffer[2] = intersectionNum;//Intersection #

			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			orb1_set_color(ORANGE);

			while(1){

				stop();

//				sign = doDrive(200);

				if(button1_click()){

					start();

					state = SHIGHWAY;

					break;

				}

				if(button2_click()){

					start();

					state = SROAD;

					break;

				}

			}

			break;

		case SHIGHWAY:/*On highway*/

			orb1_set_color(CYAN);

			while(!button1_click()){

				highwayFSM();

			}

			state = SINTERSECTION;

			break;

		default:

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

			usb_puti(state);

		}

	}

}

void enterIntersectionQueue(int sign){

	//Choose turn direction

	intersectionNum = getIntersectType(sign);

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

	//Sends packet announcing its entry to the intersection

	sendBuffer[0] = WINTERSECTIONENTRY;

	sendBuffer[2] = intersectionNum;//Intersection #

	sendBuffer[3] = getIntersectPos(sign);

	sendBuffer[4] = turnDir;

	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

	orb1_set_color(BLUE);

	stop();

	doDrive(0);

	rtc_reset();

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

		dataLength = 0;

		packet = NULL;

		packet = wl_basic_do_default(&dataLength);

		if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){

			if(packet[0] == WINTERSECTIONREPLY){

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

					prevBot = packet[1];

					orb2_set_color(GREEN);

					break;

				}else if(packet[1] != id){//Someone else got here first, try again

					sendBuffer[0] = WINTERSECTIONENTRY;

					sendBuffer[2] = intersectionNum;

					sendBuffer[3] = getIntersectPos(sign);

					sendBuffer[4] = turnDir;

					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

					orb2_set_color(ORANGE);

					rtc_reset();

				}

			}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){

				sendBuffer[0] = WINTERSECTIONREPLY;

				sendBuffer[2] = intersectionNum;

				sendBuffer[3] = packet[1];

				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

				nextBot = packet[1];

				nextDir = packet[3];

				nextPath = packet[4];

				orb2_set_color(BLUE);

				//delay_ms(200);

			}

				delay_ms(0);

		}

	}

}

void waitInIntersectionQueue(){

	while(prevBot != 0){

		dataLength = 0;

		packet = NULL;

		packet = wl_basic_do_default(&dataLength);

		if(packet && dataLength==PACKET_LENGTH){

			if(packet[2] == intersectionNum){

				if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){

					prevBot = 0;

					orb2_set_color(PURPLE);

				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){

					sendBuffer[0] = WINTERSECTIONREPLY;

					sendBuffer[2] = intersectionNum;

					sendBuffer[3] = packet[1];

					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

					nextBot = packet[1];

					nextDir = packet[3];

					nextPath = packet[4];

					orb2_set_color(BLUE);

				}

			}

/*

			if(ISPING(packet)){

				sendBuffer[0] = WPINGREPLY;

				sendBuffer[2] = packet[1];

				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){

					nextBot = packet[1];

				}

				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			}

			if(packet[0]==WPINGREPLY && packet[2]==id){

				prevBot = packet[1];

				pingWaitTime = rtc_get();

			}

*/

		}

		if(prevBot && rtc_get() << 12 == 0){

			sendBuffer[0] = WPINGBOT;

			sendBuffer[2] = prevBot;

			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			pingWaitTime = rtc_get();

		}

		if(prevBot && pingWaitTime - rtc_get() > 4){

			sendBuffer[0] = WPINGBOT;

			if(pingWaitTime - rtc_get() > 8){

				sendBuffer[0] = WPINGQUEUE;

			}

			sendBuffer[2] = prevBot;

			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

		}

/*				if(prevBot && pingWaitTime - rtc_get() > 12){

			prevBot = 0;

		}

*/

	}

}

void driveThroughIntersection(int sign){

	//Code to choose path through intersection goes in this while loop

	//But here's the code to handle wireless while in the intersection...

	start();

	turn(getIntersectType(0), turnDir);

	while(doDrive(200) != FINISHED){

		dataLength = 0;

		packet = NULL;

		packet = wl_basic_do_default(&dataLength);		

		if(dataLength==PACKET_LENGTH){

			if(packet[2]==intersectionNum/*Intersection Num*/){

				if(packet[0]==WINTERSECTIONEXIT){

					prevBot = 0;

					orb2_set_color(RED);

				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){

					sendBuffer[0] = WINTERSECTIONREPLY;

					sendBuffer[2] = intersectionNum;//Intersection #

					sendBuffer[3] = packet[1];

					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

					nextBot = packet[1];

					nextDir = packet[3];

					nextPath = packet[4];

					orb2_set_color(YELLOW);

				}

			}

			if(ISPING(packet)){

				sendBuffer[0] = WPINGREPLY;

				sendBuffer[2] = packet[1];

				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){

					nextBot = packet[1];

				}

				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			}

		}

		if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time

			/*if(bots paths won't collide){

				sendBuffer[0] = WINTERSECTIONGO;

				sendBuffer[2] = 0;//Intersection #

				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

			*/

		}

	}

}