/ - Diff - Colony - Robotics Club

         //usb_puts("after cur_robot_x/y = robot_x/y.\n");
         int r_x = target_x - cur_robot_x;
         int r_y = target_y - cur_robot_y;
         int e_x = target_x - cur_robot_x;
         int e_y = target_y - cur_robot_y;
         int v_x = cur_robot_x - last_x;
         int v_y = cur_robot_y - last_y;
         int e_x = r_x - v_x;
         int e_y = r_y - v_y;
         sprintf(buf, "vx:%d vy:%d rx:%d ry:%d ex:%d ey:%d\n", v_x, v_y, r_x, r_y, e_x, e_y);
         sprintf(buf, "vx:%d vy:%d rx:%d ry:%d ex:%d ey:%d\n", v_x, v_y, e_x, e_y, e_x, e_y);
         usb_puts(buf);
         int e_mag = e_x*e_x + e_y*e_y;
         int motor_differential = e_mag;// / 100;
         int motor_differential = e_mag >> 7;// / 128;
     /*
         sprintf(buf, "Current position: %d %d\n", cur_robot_x, cur_robot_y);
-...
         int p_x = v_y;
         int p_y = -v_x;
         int mat[2][2];
         mat[0][0] = v_y;
         mat[0][1] = -v_x;
         int e_trans_x = p_x * e_x + p_y * e_y;
         int e_trans_x = mat[0][0] * e_x + mat[0][1] * e_y;
     /*
         // Determine left or right by transforming error vector to robot axes
         // Perpendicular

     #include <stdio.h>
     #include <signal.h>
     #include <ColonetServer.h>
     #include <wireless.h>
     ColonetServer colonet_server;
     void cleanup(int a) {
       printf("TODO deinit wireless here\n");
       //wl_terminate();
       printf("todo - close wl port\n");
       exit(0);
+    }

       //TODO: check for error returned
       int numPositions = vision_get_robot_positions(&positions);
       printf("numPositions is %d\n", numPositions);
       /*
           printf("numPositions is %d\n", numPositions);
       for (int i = 0; i < numPositions; i++) {
         printf("{%d,%d} ", positions[i].x, positions[i].y);
+      }
       printf("\n");
       */
       map<int, VisionPosition> newPositionMap;
-...
       positionMap = newPositionMap;
       printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size());
       //  printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size());
       /*
       //TODO: remove this debug information
       map<int, VisionPosition>::iterator iter;
       for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {
         printf("%d has position (%d, %d) with delete buffer %d\n", iter->first, iter->second.x, iter->second.y, deleteBufferMap[iter->first]);
+      }
       */
       pthread_mutex_unlock(&position_map_lock);
       if (positions) {

trunk/code/projects/colonet/Makefile
11	11	clean: clean_server clean_client clean_robot
12	12
13	13	$(SERVER): $(SERVER_FILES)
	14	cd server/vision; make
14	15	cd server; make
15	16
16	17	$(CLIENT): $(CLIENT_FILES)

     #include <string.h>
     #define XBEE_FRAME_START 0x7E
     #define XBEE_GET_PACKET_TIMEOUT 1000
     /*Frame Types*/
     #define XBEE_FRAME_STATUS 0x8A
-...
     static int xbee_exit_command_mode(void);
     static int xbee_enter_api_mode(void);
     static int xbee_exit_api_mode(void);
     static void xbee_wait_for_string(char* s, int len);
     static void xbee_wait_for_ok(void);
     static int xbee_wait_for_string(char* s, int len);
     static int xbee_wait_for_ok(void);
     /*API Mode Functions*/
-...
     static unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;
     static int xbee_channel = XBEE_CHANNEL_DEFAULT;
     static int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;
     static unsigned int xbee_address = 0;
     static volatile unsigned int xbee_address = 0;
     /*Function Implementations*/
-...
     	arrival_buf[0] = 'A';
     	arrival_buf[1] = 'A';
     	arrival_buf[2] = 'A';
     	#ifdef ROBOT
     #ifdef ROBOT
     	//enable the receiving interrupt
     #ifdef FIREFLY
-...
+    	{
     		printf("Failed to open connection to XBee on port %s\r\n", xbee_com_port);
     		return -1;
     	} else {
     	  printf("Successfully opened connection to XBee on port %s\r\n", xbee_com_port);
+    	}
     	// set baud rate, etc. correctly
-...
+    	{
     		fprintf(stderr, "Error setting attributes.\n");
     		return -1;
     	} else {
     	  //printf("Successfully set termios attributes.\n");
+    	}
     	//lockf(xbee_stream, F_LOCK, 0);
-...
+    	{
     		fprintf(stderr, "Failed to create listener thread.\r\n");
     		return -1;
     	} else {
     	  //printf("Successfully created listener thread.\n");
+    	}
     #endif
     	if (xbee_enter_command_mode() != 0) {
     #ifndef ROBOT
     	  printf("Error returned from xbee_enter_command_mode\n");
     #endif
     		return -1;
+    	}
     	if (xbee_enter_api_mode() != 0) {
     #ifndef ROBOT
     	  printf("Error returned from xbee_enter_api_mode\n");
     #endif
     		return -1;
+    	}
     	if (xbee_exit_command_mode() != 0) {
     #ifndef ROBOT
     	  printf("Error returned from xbee_exit_command_mode\n");
     #endif
     		return -1;
+    	}
     	if (xbee_send_read_at_command("MY")) {
     #ifndef ROBOT
     	  printf("Error returned from xbee_send_read_at_command\n");
     #endif
     		return -1;
+    	}
     #ifndef ROBOT
     	//printf("About to enter while loop to get xbee_address.\n");
     	int i;
     	for (i = 0; xbee_address == 0 && i < XBEE_GET_PACKET_TIMEOUT; i++) {
     	  ret = xbee_get_packet(NULL);
     	  usleep(1000);
     	  if (ret == -1) {
     	    printf("xbee_get_packet(NULL) failed.\n");
     	    return -1;
+    	  }
+    	}
     	//	printf("After exiting while loop to get xbee_address.\n");
     #else
     	//wait to return until the address is set
     	while (xbee_address == 0) xbee_get_packet(NULL);
     	while (xbee_address == 0) {
     	  xbee_get_packet(NULL);
+    	}
     #endif
     #ifndef ROBOT
     	if (i == XBEE_GET_PACKET_TIMEOUT) { // We timed-out.
     	  printf("xbee_get_packet timed out.\n");
     	  return -1;
     	} else {
     	  return 0;
+    	}
     #else
     	return 0;
     #endif
+    }
     /**
-...
     		return -1;
+    	}
     	xbee_wait_for_ok();
     #ifndef ROBOT
     	//	printf("In xbee_enter_command_mode about to call xbee_wait_for_ok()\n");
     #endif
     	return 0;
     	if (xbee_wait_for_ok() != 0) {
     #ifndef ROBOT
     	  printf("xbee_wait_for_ok failed.\n");
     #endif
     	  return -1;
     	} else {
     	  return 0;
+    	}
+    }
     /**
-...
     /**
      * Wait until the string "OK\r" is received from the XBee.
      **/
     static void xbee_wait_for_ok()
     static int xbee_wait_for_ok()
+    {
     	xbee_wait_for_string("OK\r", 3);
     	return xbee_wait_for_string("OK\r", 3);
+    }
     /**
-...
      * @param s the string to receive
      * @param len the length of the string
      **/
     static void xbee_wait_for_string(char* s, int len)
     static int xbee_wait_for_string(char* s, int len)
+    {
     #ifndef ROBOT
       //printf("In xbee_wait_for_string.\n");
     #endif
     	char* curr = s;
     	while (curr - s < len) {
     		// check if buffer is empty
-...
     			if (c == *curr) {
     				curr++;
     			} else {
     				curr = s;
     #ifndef ROBOT
     			  return -1; // Computer is less forgiving.
     #else
     			  curr = s;
     #endif
+    			}
+    		}
     		} // else buffer is empty.
     #ifndef ROBOT
     		usleep(1000);
     		usleep(100);
     #endif
+    	}
     	return 0;
+    }
     /**
-...
     	if (xbee_send(prefix, 3) != 0) {
     		return -1;
+    	}
     	if (xbee_send(buf, len) != 0) {
     		return -1;
+    	}
-...
     			if (buffer_first == XBEE_BUFFER_SIZE)
     				buffer_first = 0;
     			// check if buffer is empty
     			if (buffer_first == buffer_last)
     				return -1;
     			if (buffer_first == buffer_last) {
     				return 0;
+    			}
     		} while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
     		if (buffer_first == XBEE_BUFFER_SIZE) {
-...
     		// check if buffer is empty
     		if (buffer_first == buffer_last) {
     			return -1;
     			return 0;
+    		}
     		xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
     		if (buffer_first == XBEE_BUFFER_SIZE) {
-...
+    	}
     	//we will take care of the packet
     	if (xbee_handle_packet(xbee_buf + 3, len)) {
     		return -1;
     	if (xbee_handle_packet(xbee_buf + 3, len) != 0) {
     		return 0;
+    	}
     	if (dest == NULL) {
     		return -1;
     		return 0;
+    	}
     	int i;
-...
     	if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
+    	{
     //							printf("reading xbee_address\n");
     		xbee_address = 0;
     		int i;
     		for (i = 0; i < extraLen; i++) {
-...
     	xbee_com_port = port;
+    }
     #endif

     #     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.
     #     AVR Studio 4.10 requires dwarf-2.
     #     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.
     DEBUG =
     DEBUG =
     # dwarf-2
     # List any extra directories to look for include files here.

     /*Function Prototypes*/
     void wl_do_timeout(void);
     static void wl_do_timeout(void);
     //Note: the actual frame sent has group as the first four bits and
     //frame as the last four.
     int wl_send_packet(char group, char type, char* data, int len, int dest, char options, char frame);
     static int wl_send_packet(char group, char type, char* data, int len, int dest, char options, char frame);
     /*Data Members*/
     //used to store incoming and outgoing packets
     unsigned char wl_buf[128];
     static unsigned char wl_buf[128];
     //1 if we have timed out since we last checked, 0 otherwise.
     int wl_timeout = 0;
     static int wl_timeout = 0;
     PacketGroupHandler* wl_packet_groups[WL_MAX_PACKET_GROUPS];
     static PacketGroupHandler* wl_packet_groups[WL_MAX_PACKET_GROUPS];
     #ifndef ROBOT
     //called when we time out, or receive interrupt
     void sig_handler(int signo)
     static void sig_handler(int signo)
+    {
     	switch (signo)
+    	{
-...
     #else
     //called when the timer ticks
     void timer_handler(void)
     static void timer_handler(void)
+    {
     	wl_timeout = 1;
+    }
-...
     		wl_packet_groups[i] = NULL;
     	if (xbee_lib_init() == -1) {
     	  return -1;
     		return -1;
+    	}
     	//begin timeout timer

Project

General

Profile

Colony

Revision 581