/ - Diff - Colony - Robotics Club

     		exit(0);
+    	}
     	#endif
     	usb_puts("Entering command mode.\n");
     	xbee_enter_command_mode();
     	usb_puts("Entered command mode.\n");
     	xbee_enter_api_mode();
     	xbee_exit_command_mode();
     	xbee_send_read_at_command("MY");

     #ifdef STATION_DEBUG
     #define STATION_DEBUG_PRINT( s ) usb_puts( s )
     #define STATION_DEBUG_PUTI( i ) usb_puti( i )
     #else
     #define STATION_DEBUG_PRINT( s )
     #define STATION_DEBUG_PUTI( i )
     #endif
     /**

     AVRDUDE_PROGRAMMER = avrisp
     # com1 = serial port. Use lpt1 to connect to parallel port.
     AVRDUDE_PORT = /dev/ttyUSB0
     AVRDUDE_PORT = /dev/ttyUSB1
     # programmer connected to serial device
     AVRDUDE_WRITE_FLASH = -b 9600 -U flash:w:$(TARGET).hex

     #include "firefly+_lib.h"
     #include "bays.h"
     #define MAX(a,b) ((a) > (b) ? (a) : (b))
     #define MIN(a,b) ((a) < (b) ? (a) : (b))
     void bay_tester(void)
+    {
         bayStates[1] = 50;
         bayStates[2] = 51;
         bayStates[3] = 52;
+    }
     void print_bays_debug(void)
+    {
     	usb_puts("\n\r");
     	usb_puts("Bays: ");
     	for(int i = 0; i < NUM_BAYS; i++) {
     		usb_puti(bayStates[i]);
     		usb_putc(' ');
+    	}
     	usb_puts("\n\r");
     	usb_puts("Seeking robot: ");
     	usb_puti(robot_seeking);
     	usb_puts("\n\r");
+    }
     void do_consistency_checks(void)
+    {
         if(robot_seeking != 255 && robot_assignment(robot_seeking) == 255) // the only free bay from bay_tester()
+        {
           usb_puts("BAD STUFF HAPPENED...\n\r");
           usb_puts("robot assigned to BAY 1 does not equal robot_seeking\n\r");
           usb_puts("\n\rrobot_seeking: ");
           usb_puti(robot_seeking);
           usb_puts("Bay 1 contains:  ");
           usb_puti(bayStates[1]);
           usb_puts("\n\r");
           //while(1);
+        }
+    }
     // END DEBUGGING FUNCTIONS
     void bays_init(void)
+    {
     	int i;
     	for (i = 0; i < NUM_BAYS; i++)
     		bayStates[i] = 255;
     	robot_seeking = 255;
     	bay_tester();
     	digital_output(ENABLE_PIN, 0);
     	digital_output(BOM_PIN, 1);
     	digital_output(SELECT_0, 0);
-...
     	active_bay = -1;
+    }
     /*int assign_bay(int robot)
+    {
     	int max_bom = sensors[robot]; // where the station sees the robot (max bom)
     	int closest = 255;
     	int distance = 16;
     	int i;
     	for (i = 0; i < NUM_BAYS; i++)
+    	{
     		if (bayStates[i] != 255)
     			continue;
     		int d = max_bom - i * 4; // distance (BOM-wise) from bay
     		if (d < -8)
     			d += 16;
     		else
     			if (d > 8)
     				d -= 16;
     		if (d < 0)
     			d *= -1;
     		if (d < distance) // finds closest bay to robot's current position
+    		{
     			distance = d; // distance (BOM-wise)
     			closest = i; // actual bay number
+    		}
+    	}
     	if (closest == 255) // all full
     		return 255;
     	bayStates[closest] = robot;
     	return closest;
     }*/
     /*int assign_bay(int robot) // returns number of an empty bay (255 if all full)
+    {
         for(int i = 0; i < NUM_BAYS; i++) // right now, go thru linearly and allocate first available bay
+        {
             if(bayStates[i] == 255)
+            {
                 bayStates[i] = robot;
                 return i;
+            }
+        }
     }*/
     int left_index( int pos )
+    {
     	int i;
     	for(i = pos; i >= 0; i--)
+    	{
     		if(bayStates[i] != 255)
     			return i;
+    	}
     	return -1;
+    }
     int right_index( int pos )
+    {
     	int i;
     	for(i = pos; i < NUM_BAYS; i++)
+    	{
     		if(bayStates[i] != 255)
     			return i;
+    	}
     	return NUM_BAYS;
+    }
     /*int assign_bay( int robot ) // 'smart' assign_bay()
+    {
     	int ldist = 0;
     	int rdist = 0;
     	int score = 0;
     	int max = 0;
     	int bay = 255;
     	int count = 0;
     	int i;
     	for(i = 0; i < NUM_BAYS; i++)
+    	{
     		if(bayStates[i] == 255)
     			count++;
+    	}
     	if(count == NUM_BAYS)
+    	{
     		bayStates[0] = robot;
     		return 0;
+    	}
     	for(i = 0; i < NUM_BAYS; i++)
+    	{
     		if(bayStates[i] == 255)
+    		{
     			ldist = i - left_index(i);
     			rdist = right_index(i) - i;
     			if(ldist - i == 1)
     				score = rdist;
     			else if(i + rdist == NUM_BAYS)
     				score = ldist;
     			else
     				score = MIN(ldist,rdist);
     			if(score > max)
+    			{
     				max = score;
     				bay = i;
+    			}
+    		}
     		ldist = rdist = 0;
+    	}
     	bayStates[bay] = robot;
     	return bay;
     }*/
     int robot_assignment(int robot) //returns bay to which 'robot' has been assigned, 255 if not yet assigned
+    {
         for(int i = 0; i < NUM_BAYS; i++)
+        {
             if(bayStates[i] == robot)
                 return i;
+        }
         return 255;
+    }
     int is_empty(void) //returns 1 if there is an empty bay, 0 if not
+    {
         for(int i = 0; i < NUM_BAYS; i++)
+        {
             if(bayStates[i] == 255)
                 return 1;
+        }
         return 0;
+    }
     void lbom_on(void)
+    {
     	digital_output(BOM_PIN, 0);
-...
+    {
     	digital_output(ENABLE_PIN, 0);
+    }

     #define SELECT_1	_PIN_A3
     #define SELECT_2	_PIN_A4
     #define NUM_BAYS 4
     #define MAX_ROBOTS 20
     char sensors[MAX_ROBOTS]; //from wl_charging_station...probably should be there instead of here, but assign_bay needs for now
     int bayStates[NUM_BAYS]; //holds robot id of robot in each bay 0-3 (255 if empty)
     int robot_seeking; //robot if of the robot currently seeking (255 if none)
     int active_bay;
     void bay_tester(void);
     void print_bays_debug(void);
     void do_consistency_checks(void);
     // bom & beacon functions
     void bays_init(void);
     int assign_bay(int robot);
     int robot_assignment(int robot);
     int is_empty(void);
     // bom & beacon functions
     void lbom_on(void);
     void lbom_off(void);
     void bay_enable(int which);

     #include "charging.h"
     #include "charging_defs.h"
     #include "bays.h"
     #include <stdlib.h>
     #include <serial.h>
-...
     	for (i = 0; i < NUM_BAYS; i++)
     		bays[i] = -1;
     	bays_init();
     	// call our own special function to set the BOM
     	wl_token_ring_set_bom_functions(charging_bom_on,
     		charging_bom_off, charging_bom_read);
-...
     		return;
+    	}
     	STATION_DEBUG_PRINT("Cancelling charging for robot ");
     	STATION_DEBUG_PUTI(robot);
     	STATION_DEBUG_PRINT(".\n");
     	//allow other robots to seek
     	if (robots[robot]->state == STATE_SEEKING)
+    	{
     		//TODO : deactivate BOMs and homing beacons
     		bay_disable();
     		is_seeking = 0;
+    	}
-...
+    {
     	int bay = 0;
     	//TODO: implement
     	//TODO: use better algorithm based on distance
     	for (bay = 0; bay < NUM_BAYS; bay++)
     		if (bays[bay] == -1)
     			break;
     	if (bay == NUM_BAYS)
+    	{
     		STATION_DEBUG_PRINT("Attempted to assign a bay when all ");
     		STATION_DEBUG_PRINT("are full.\n");
+    	}
     	bays[bay] = robot;
     	return bay;
+    }
-...
      **/
     void charging_begin_seeking(int robot)
+    {
     	STATION_DEBUG_PRINT("Robot ");
     	STATION_DEBUG_PUTI(robot);
     	STATION_DEBUG_PRINT(" has begun seeking.\n");
     	if (robot > robotsSize)
+    	{
     		int nextSize = robot + 1;
-...
     				STATION_DEBUG_PRINT("seeking.\n");
     				break;
     			case STATE_DOCKED:
     				if (is_seeking)
+    				{
     					//we can only allow one robot to seek
     					robots[robot]->state = STATE_NOT_CHARGING;
     					bays[robots[robot]->bay] = -1;
     					robots[robot]->bay = -1;
     					break;
+    				}
     				robots[robot]->state = STATE_SEEKING;
     				is_seeking = 1;
     				//TODO: active robots[robot]->bay
     				bay_enable(robots[robot]->bay);
     				break;
     			default:
     				STATION_DEBUG_PRINT("Robot is in an ");
-...
     	robots[robot] = r;
     	numRobots++;
     	is_seeking = 1;
     	//TODO: activate r->bay
     	bay_enable(r->bay);
+    }
     /**
-...
      **/
     void charging_dock(int robot)
+    {
     	STATION_DEBUG_PRINT("Robot ");
     	STATION_DEBUG_PUTI(robot);
     	STATION_DEBUG_PRINT(" has docked with the station.\n");
     	if (robots[robot] == NULL)
+    	{
     		STATION_DEBUG_PRINT("Should not proceed from not ");
-...
     	//robot was seeking before
     	is_seeking = 0;
     	//TODO: deactivate boms and homing beacons
     	bay_disable();
     	robots[robot]->state = STATE_DOCKED;
+    }
-...
      **/
     void charging_bom_on(void)
+    {
     	//TODO: implement
     	lbom_on();
+    }
     /**
-...
      **/
     void charging_bom_off(void)
+    {
     	//TODO: implement
     	lbom_off();
+    }
     /**

     	//dragonfly_init(ALL_ON);
     	usb_puts("Initializing wireless.\n");
     	wl_init();
     	wl_token_ring_register();
     	charging_init();
     	usb_puts("Charging station initialized.\n");
     	while (1)
     		wl_do();

branches/autonomous_recharging/code/projects/autonomous_recharging/dragonfly/wl_recharge_group.c
2	2
3	3	#include <wireless.h>
4	4	#include <wl_defs.h>
5		#include <stdio.h>
6		#include <string.h>
7	5
	6	#include <serial.h>
8	7
	8	#include "recharge_defs.h"
	9
9	10	// Function prototypes
10	11	void wl_recharge_timeout_handler(void);
11	12	void wl_recharge_response_handler(int frame, int received);
...	...
59	60	int requestCount = 0;
60	61	int verifyCount = 0;
61	62
	63
62	64	/**
63	65	* Register this packet group with the wireless library.
64	66	* This function must be called before any other wl_recharge
...	...
111	113	{
112	114	if (recharge_state != SEEKING)
113	115	{
114		WL_DEBUG_PRINT("Unexpected docking occurred.\n");
	116	RECHARGE_DEBUG_PRINT("Unexpected docking occurred.\n");
115	117	return;
116	118	}
117	119
...	...
126	128	{
127	129	if (recharge_state != DOCKED)
128	130	{
129		WL_DEBUG_PRINT("Unexpected departure occurred.\n");
	131	RECHARGE_DEBUG_PRINT("Unexpected departure occurred.\n");
130	132	return;
131	133	}
132	134
...	...
163	165	void wl_recharge_send_poll(void)
164	166	{
165	167	wl_send_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_POLL_STATIONS,
166		NULL, 0, 0);
	168	0, 0, 0);
167	169	}
168	170
169	171	/**
...	...
174	176	void wl_recharge_send_request(int dest)
175	177	{
176	178	wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_REQUEST,
177		NULL, 0, dest, REQUEST_FRAME);
	179	0, 0, dest, REQUEST_FRAME);
178	180	}
179	181
180	182	/**
...	...
186	188	void wl_recharge_send_verify(int dest)
187	189	{
188	190	wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_VERIFY,
189		NULL, 0, dest, VERIFY_FRAME);
	191	0, 0, dest, VERIFY_FRAME);
190	192	}
191	193
192	194	/**
...	...
196	198	void wl_recharge_send_cancel(int dest)
197	199	{
198	200	wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_CANCEL,
199		NULL, 0, dest, CANCEL_FRAME);
	201	0, 0, dest, CANCEL_FRAME);
200	202	}
201	203
202	204	/**
...	...
208	210	void wl_recharge_send_seeking(int dest)
209	211	{
210	212	wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_SEEKING,
211		NULL, 0, dest, SEEKING_FRAME);
	213	0, 0, dest, SEEKING_FRAME);
212	214	}
213	215
214	216	/**
...	...
220	222	void wl_recharge_send_docked(int dest)
221	223	{
222	224	wl_send_robot_to_robot_global_packet(WL_RECHARGE_GROUP, WL_RECHARGE_DOCKED,
223		NULL, 0, dest, DOCKED_FRAME);
	225	0, 0, dest, DOCKED_FRAME);
224	226	}
225	227
226	228	/**
...	...
288	290	return;
289	291	if (!received)
290	292	{
291		WL_DEBUG_PRINT("Sent packet not recieved.\n");
	293	RECHARGE_DEBUG_PRINT("Sent packet not recieved.\n");
292	294	switch (frame)
293	295	{
294	296	case REQUEST_FRAME:
...	...
323	325	wl_recharge_available_station(source);
324	326	break;
325	327	case WL_RECHARGE_REQUEST:
326		WL_DEBUG_PRINT("Only stations should receive WL_RECHARGE_REQUEST.\n");
	328	RECHARGE_DEBUG_PRINT("Only stations should receive WL_RECHARGE_REQUEST.\n");
327	329	break;
328	330	case WL_RECHARGE_REQUEST_ACCEPT:
329	331	wl_recharge_request_accepted(source);
...	...
341	343	wl_recharge_docked(source);
342	344	break;
343	345	default:
344		WL_DEBUG_PRINT("Error packet of unknown type received.\n");
	346	RECHARGE_DEBUG_PRINT("Error packet of unknown type received.\n");
345	347	break;
346	348	}
347	349	}
...	...
372	374	{
373	375	if (recharge_state != REQUESTING)
374	376	{
375		WL_DEBUG_PRINT("Accepted when we weren't requesting.\n");
	377	RECHARGE_DEBUG_PRINT("Accepted when we weren't requesting.\n");
376	378	return;
377	379	}
378	380	if (station != source)
379	381	{
380		WL_DEBUG_PRINT("Accepted by a different station.\n");
	382	RECHARGE_DEBUG_PRINT("Accepted by a different station.\n");
381	383	return;
382	384	}
383	385
...	...
395	397	{
396	398	if (source != station)
397	399	{
398		WL_DEBUG_PRINT("Received verify from unassociated station.\n");
	400	RECHARGE_DEBUG_PRINT("Received verify from unassociated station.\n");
399	401	wl_recharge_send_cancel(source);
400	402	return;
401	403	}
...	...
409	411	wl_recharge_send_docked(station);
410	412	break;
411	413	default:
412		WL_DEBUG_PRINT("Cancelled docking procedure.\n");
	414	RECHARGE_DEBUG_PRINT("Cancelled docking procedure.\n");
413	415	wl_recharge_send_cancel(station);
414	416	break;
415	417	}
...	...
425	427	{
426	428	if (station != source)
427	429	{
428		WL_DEBUG_PRINT("Received cancel from station we weren't docking with.\n");
	430	RECHARGE_DEBUG_PRINT("Received cancel from station we weren't docking with.\n");
429	431	return;
430	432	}
431	433
...	...
441	443	recharge_state = POLLING;
442	444	break;
443	445	case DOCKED:
444		WL_DEBUG_PRINT("Charging station kicked us out.\n");
	446	RECHARGE_DEBUG_PRINT("Charging station kicked us out.\n");
445	447	wl_recharge_depart();
446	448	break;
447	449	case DEPARTING:
448	450	//ignore, since we are already departing
449	451	break;
450	452	default:
451		WL_DEBUG_PRINT("Received unexpected cancellation.\n");
	453	RECHARGE_DEBUG_PRINT("Received unexpected cancellation.\n");
452	454	break;
453	455	}
454	456	}
...	...
463	465	{
464	466	if (station != source)
465	467	{
466		WL_DEBUG_PRINT("Received seeking alert from station we aren't seeking.\n");
	468	RECHARGE_DEBUG_PRINT("Received seeking alert from station we aren't seeking.\n");
467	469	wl_recharge_send_cancel(source);
468	470	return;
469	471	}
...	...
478	480	wl_recharge_send_docked(station);
479	481	break;
480	482	default:
481		WL_DEBUG_PRINT("Received unexpected seeking confirmation.\n");
	483	RECHARGE_DEBUG_PRINT("Received unexpected seeking confirmation.\n");
482	484	break;
483	485	}
484	486	}
...	...
493	495	{
494	496	if (station != source)
495	497	{
496		WL_DEBUG_PRINT("Unknown station believes we are docked.\n");
	498	RECHARGE_DEBUG_PRINT("Unknown station believes we are docked.\n");
497	499	wl_recharge_send_cancel(source);
498	500	return;
499	501	}
...	...
507	509	case SEEKING:
508	510	//not good - we can't immediately proceed to seeking again
509	511	//we will leave the station and repoll it
510		WL_DEBUG_PRINT("Seeking, but station believes we are docked. ");
511		WL_DEBUG_PRINT("Leaving station and repolling.\n");
	512	RECHARGE_DEBUG_PRINT("Seeking, but station believes we are docked. ");
	513	RECHARGE_DEBUG_PRINT("Leaving station and repolling.\n");
512	514	wl_recharge_send_cancel(source);
513	515	wl_recharge_begin();
514	516	break;
515	517	default:
516		WL_DEBUG_PRINT("Unexpected docking verification received.\n");
	518	RECHARGE_DEBUG_PRINT("Unexpected docking verification received.\n");
517	519	break;
518	520	}
519	521	}

     #include <move.h>
     #include <battery.h>
     #include <dio.h>
     #include <lights.h>
     #include <wl_defs.h>
     #include "wl_recharge_group.h"
     #include "seeking.h"
     #include "departing.h"
     #include "recharge_i2c.h"
     #include "recharge_defs.h"
     // Function prototypes
     void recharge_check_low_battery(void);
-...
     void recharge_seek(void);
     void recharge_depart(void);
     //Global variables
     #ifdef RECHARGE_DEBUG
     int old_state = -1;
     #endif
     /**
      * Initializes recharging. Before this function may be called,
      * wl_init must be called.
-...
     	// show colors if we are debugging
     	#ifdef RECHARGE_DEBUG
     	switch (wl_recharge_get_state())
     	if (old_state != wl_recharge_get_state())
+    	{
     		old_state = wl_recharge_get_state();
     		RECHARGE_DEBUG_PUTI(old_state);
     		switch (wl_recharge_get_state())
+    		{
     			case NOT_RECHARGING:
     				orb_set_color(GREEN);
     				break;
     			case POLLING:
     			case REQUESTING:
     				orb_set_color(RED);
     				//we shouldn't be in this state
     				break;
     			case SEEKING:
     				orb_set_color(YELLOW);
     				break;
     			case DOCKED:
     				orb_set_color(BLUE);
     				break;
     			case DEPARTING:
     				orb_set_color(PURPLE);
     				break;
     			default:
     				WL_DEBUG_PRINT("Unexpected state.\n");
     				break;
+    		}
+    	}
     	#endif

     #include <dragonfly_lib.h>
     #include <wireless.h>
     #include <wl_token_ring.h>
     #include "recharge.h"
     int main(void)
+    {
     	dragonfly_init(ALL_ON);
     	usb_puts("Initializing wireless.\n");
     	wl_init();
     	wl_token_ring_register();
     	wl_token_ring_join();
     	usb_puts("Wireless initialized.\n");
     	recharge_init();
     	usb_puts("Recharging initialized.\n");
     	while (1)
+    	{
     		wl_do();
     		int charging = recharge_do();
     		if (!charging)
+    		{
     			//do stuff
+    		}
+    	}
     	return 0;
+    }

     # Relative path to the root directory (containing lib directory)
     ifndef COLONYROOT
     COLONYROOT = ../../../..
     #COLONYROOT = ../../../..
     COLONYROOT = ../../../../../../trunk
     endif
     # Target file name (without extension).
-...
      USE_WIRELESS = 1
     # com1 = serial port. Use lpt1 to connect to parallel port.
     AVRDUDE_PORT = com4
     AVRDUDE_PORT = /dev/ttyUSB0
+    #
+    #
     ###################################

Project

General

Profile

Colony

Revision 116