Colony - / - Diff - Robotics Club

Revision 1809

     #include <dragonfly_lib.h>
     #include <wl_basic.h>
     #include<encoders.h>
     #include "circle_spacing.h"
     enum role_e {MASTER,SLAVE};
     enum state_e {INIT, ID, MAPPING, PLANNING, WAIT, SENDING, RECIEVING, FINISH};
     enum role_e role = SLAVE;
     enum state_e current_state = INIT;
     enum state_e next_state = INIT;
     int used[17] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  // if a robot is here, the value of the indice equal to its id is 1, esle 0
     int map[17] = {20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20};  // Angular position of the robot which id equals the indice (20 for not here)
     int neighbour[17] = {20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20}; // clockwise neighbour
     int gap[17] = {20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20,20};  // gap to the left of the robot
     int stop = 0;
     struct vector slave_position;
     int desired_max_bom;
     int bom_max_counter;
     int robotid;
     int centerid = 0;
     int robotsRecieved=0;
     int target_slave = 0;
     void set_desired_max_bom(int desired_angle)
+    {
     	if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;
     	if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;
     	if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;
     	if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;
     	if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;
     	if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;
     	if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;
     	if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;
     	if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;
     	if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;
     	if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;
     	if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;
     	if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;
     	if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;
     	if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;
     	if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;
+    }
     // collects the angular position of the slave robots by making them switch their bom one by one.
     void map_slaves()
+    {
     	int bom_max;
     	for(int i=0; i<17; i++)
+    	{
     		if(used[i])
+    		{
     			send2(CIRCLE_ACTION_MAP,i);
     			delay_ms(1000);
     			do
+    			{
     				bom_refresh(BOM_ALL);
     				bom_max = bom_get_max();
+    			}
     			while(bom_max == -1);
     			map[i] = bom_max;
     			send2(CIRCLE_ACTION_DONE,i);
+    		}
+    	}
     	usb_puts("Map: \r\n");
     	for(int i=0; i<17; i++)
+    	{
     		usb_puti(map[i]);
     		usb_puts("\r\n");
+    	}
+    }
     void plan()
+    {
     	static int k = 0;
     	target_slave++;
     	if(target_slave>16)
+    	{
     		next_state = FINISH;
     		orb1_set_color(YELLOW);
+    	}
     	else if(used[target_slave])
+    	{
     		k++;
     		usb_puts("K: ");
     								usb_puti(k);
     								usb_puts("\r\n");
     		set_desired_max_bom(360/robotsRecieved*k);
     		send2(CIRCLE_ACTION_MOVE,target_slave);
     		delay_ms(500);
     		next_state = SENDING;
+    	}
+    }
     void plan2()
+    {
     	int order[robotsRecieved];
     	int check[17] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
     	for(int i=0; i<robotsRecieved; i++)
+    	{
     		int current;
     		int min = 20;
     		for(int j=0; j<17; j++)
+    		{
     			if(map[j]<min && !check[j])
+    			{
     				current = j;
     				min = map[j];
     				usb_puts("Test \r\n");
+    			}
+    		}
     		order[i]=current;
     		check[current]=1;
     			usb_puti(i);
     			usb_puts("  Current: ");
     			usb_puti(current);
     			usb_puts("\r\n");
+    	}
     	usb_puts("Order: \r\n");
     	for(int i=0; i<robotsRecieved; i++)
     	{	usb_puti(i);
     		usb_puts("   ");
     		usb_puti(order[i]);
     		usb_puts("\r\n");
+    	}
     	for(int i=0; i<robotsRecieved; i++)
+    	{
     		if(i==robotsRecieved-1) gap[order[i]]= map[order[0]] - map[order[i]] +16; // closes the loop
     		else gap[order[i]]= map[order[i+1]] - map[order[i]];
+    	}
     	usb_puts("Gap: \r\n");
     	for(int i=0; i<17; i++)
+    	{
     		usb_puti(gap[i]);
     		usb_puts("\r\n");
+    	}
     	for(int i=0; i<robotsRecieved; i++)
+    	{
     		if(i==0) neighbour[order[i]]=order[robotsRecieved-1];
     		else neighbour[order[i]]=order[i-1];
+    	}
     	usb_puts("Neighbour: \r\n");
     	for(int i=0; i<17; i++)
+    	{
     		usb_puti(neighbour[i]);
     		usb_puts("\r\n");
+    	}
+    }
     void plan3()
+    {
     	static int move_counter =0;
     	int max = 0;
     	int move_gap = 0;
     	for(int j=0; j<17; j++)
+    	{
     		if(gap[j]>max && gap[j]!=20)
+    		{
     			target_slave = j;
     			max = gap[j];
+    		}
+    	}
     	move_counter++;
     	if(move_counter==robotsRecieved) next_state = FINISH;
     	else
+    	{
     		move_gap = gap[target_slave]-16/robotsRecieved;
     		desired_max_bom = map[target_slave]+move_gap;
     		if(desired_max_bom>15) desired_max_bom-=16;
     		gap[target_slave]-=move_gap;
     		gap[neighbour[target_slave]] += move_gap;
     		usb_puts("Target:");
     		usb_puti(target_slave);
     		usb_puts("  Destination:");
     		usb_puti(desired_max_bom);
     		usb_puts("\n\r");
     		send2(CIRCLE_ACTION_MOVE,target_slave);
     		delay_ms(500);
     		next_state = SENDING;
+    	}
+    }
     void switch_state ()
+    {
     	switch(role)
+    	{
     		case MASTER:
     			switch(current_state)
+    			{
     				case PLANNING:
     					orb1_set_color(RED);
     					break;
     				case SENDING:
     					bom_off();
     					break;
+    			}
     			switch(next_state)
+    			{
     				case ID:
     					send2(CIRCLE_ACTION_EXIST,robotid);
     					break;
     				case MAPPING:
     					map_slaves();
     					plan2();
     					break;
     				case PLANNING:
     					orb1_set_color(PINK);
     					break;
     				case SENDING:
     					bom_on();
     					break;
     				case RECIEVING:
     					break;
+    			}
     			break;
     		case SLAVE:
     			switch(current_state)
+    			{
     				case SENDING:
     					bom_off();
     					break;
+    			}
     			switch(next_state)
+    			{
     				case ID:
     					break;
     				case MAPPING:
     					orb1_set_color(PINK);
     					break;
     				case SENDING:
     					bom_on();
     					break;
     				case RECIEVING:
     					bom_off();
     					break;
     				case FINISH:
     					motor_l_set(FORWARD,0);
     					motor_r_set(FORWARD,0);
     					orb1_set_color(YELLOW);
     					break;
+    			}
     			break;
+    	}
     	current_state = next_state;
+    }
     void send2(char arg0, char arg1)
+    {
     	char send_buffer[2];
     	send_buffer[0]=arg0;
     	send_buffer[1]=arg1;
     	wl_basic_send_global_packet(42,send_buffer,2);
+    }
     int main(void)
+    {
     	int sending_counter = 0;
     	int waitingCounter=0;
     	int max_bom;
         /* Initialize dragonfly board */
         dragonfly_init(ALL_ON);
     	xbee_init();
     	wl_basic_init_default();
     	wl_set_channel(12);
         int data_length;
     	unsigned char *packet_data;
     	char send_buffer[2];
     	robotid = get_robotid();
     	if(wheel()>100)
+    	{
     		role = MASTER;
+    	}
     	if(role == MASTER) {orb2_set_color(RED); set_desired_max_bom(30);}
     	else orb2_set_color(BLUE);
     	while(1)
+    	{
     		bom_refresh(BOM_ALL);
     		if(current_state != next_state) switch_state();
     		switch(role)
+    		{
     			case MASTER:
     				switch(current_state)
+    				{
     					case INIT:
     						next_state = ID;
     						break;
     					case ID:
     						waitingCounter++;
     						if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
+    						{
     							//only add to robots seen if you haven't gotten an ACK from this robot
     							if(used[packet_data[1]]==0)
+    							{
     								robotsRecieved++;
     								used[packet_data[1]] = 1;
     								usb_puts("Added: ");
     								usb_puti(packet_data[1]);
     								usb_puts("\r\n");
+    							}
     							// NEW: sends a packet to each robot it receives telling them to be done.
     							send2(CIRCLE_ACTION_GOTYOU,packet_data[1]);
+    						}
     						if(waitingCounter >= 300){
     							next_state = MAPPING;
+    						}
     						break;
     					case MAPPING:
     						next_state = PLANNING;
     						break;
     					case PLANNING:
     						plan3();
     						break;
     					case SENDING:
     						if(sending_counter++>4)	// clock for switching the BOMs between the master and slave
+    						{
     							next_state = RECIEVING;
     							sending_counter = 0;
     							send2(CIRCLE_ACTION_CHANGE,target_slave);
+    						}
     						break;
     					case RECIEVING:
     						if(sending_counter++>4)	// clock for switching the BOMs between the master and slave
+    						{
     							next_state = SENDING;
     							sending_counter = 0;
     							send2(CIRCLE_ACTION_CHANGE,target_slave);
+    						}
     						max_bom = bom_get_max();
     						//usb_puts("bom_get_max : ");
     						//usb_puti(max_bom);
     						//usb_puts("\n\r");
     						if(max_bom == desired_max_bom)
+    						{
     							bom_max_counter ++;
     							if(bom_max_counter>2)                 // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise.
+    							{
     							send2(CIRCLE_ACTION_STOP,target_slave);
     							next_state = PLANNING;
     							bom_max_counter = 0;
+    							}
+    						}
     						else bom_max_counter = 0;
     						break;
+    				}
     				break;
     			case SLAVE:
     				if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_STOP && packet_data[1] == robotid) next_state = FINISH;
     				switch(current_state)
+    				{
     					case INIT:
     						next_state = ID;
     						break;
     					case ID:
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
+    						{
     							centerid = packet_data[1];
     							send2(CIRCLE_ACTION_ACK,robotid);
     							next_state = MAPPING;
+    						}
     						break;
     					case MAPPING:
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_MAP && packet_data[1] == robotid)
+    						{
     							bom_on();
+    						}
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE && packet_data[1] == robotid)
+    						{
     							bom_off();
     							next_state = PLANNING;
+    						}
     					case PLANNING:
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_MOVE && packet_data[1] == robotid)
+    						{
     							next_state = RECIEVING;
+    						}
     						break;
     					case SENDING:
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_CHANGE && packet_data[1] == robotid)
     							next_state = RECIEVING;
     						break;
     					case RECIEVING:
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_CHANGE && packet_data[1] == robotid)
     							next_state = SENDING;
     						if(stop)
+    						{
     						motor_l_set(FORWARD,0);
     						motor_r_set(FORWARD,0);
     						orb1_set_color(GREEN);
+    						}
     						else
+    						{
     							max_bom = bom_get_max();
     							/*usb_puts("bom_get_max : ");
     							usb_puti(max_bom);
     							usb_puts("/n/r");*/
     						   if(max_bom == 8)
+    						   {
     							orb2_set_color(BLUE);
     							motor_r_set(FORWARD,180);
     							motor_l_set(FORWARD,180);
+    						   }
     						   else if(max_bom == 7 || max_bom == 6 || max_bom == 5)
+    						   {
     							motor_l_set(FORWARD,180);
     							motor_r_set(FORWARD,0);
+    							}
     							else if(max_bom == -1);
     						   else
+    						   {
     							orb2_set_color(GREEN);
     							motor_l_set(FORWARD,0);
     							motor_r_set(FORWARD,180);
+    							}
+    						}
     						break;
+    				}
     				break;
+    		}
     		packet_data = wl_basic_do_default(&data_length);
     		delay_ms(10);
+    	}
       return 0;
+    }
     /*
     //   Code working for a single slave orbiting around the master, with only one direction  (tested on 07.04.2010)
     #include <dragonfly_lib.h>
     #include <wl_basic.h>
     #include<encoders.h>
     int master = 0;
     int sending = 0;
     int stop = 0;
-...
     	int sending_counter = 0;
         /* Initialize dragonfly board */
        // Initialize dragonfly board
         dragonfly_init(ALL_ON);
     	xbee_init();
-...
     	char send_buffer[2];
     	if(wheel()>100)
+    	{
     		master = 1;
+    	}
     	if(master) {orb1_set_color(RED); set_desired_max_bom(30);}
     	else orb1_set_color(BLUE);
-...
     				else
+    				{
     					int max_bom = bom_get_max();
     					/*usb_puts("bom_get_max : ");
     					usb_puti(max_bom);
     					usb_puts("/n/r");*/
     				   if(max_bom == 8)
-...
     */

     USE_WIRELESS = 1
     # com1 = serial port. Use lpt1 to connect to parallel port.
     AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM7:'; else echo '/dev/ttyUSB0'; fi)
     AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
     else
     COLONYROOT := ../$(COLONYROOT)

Also available in: Unified diff