/ - Diff - Colony - Robotics Club

Revision 550

Added by Eugene Marinelli about 16 years ago

failed attempt to fix colonet robot code

trunk/code/projects/colonet/robot/robot_slave/Makefile
14	14	USE_WIRELESS = 1
15	15
16	16	# com1 = serial port. Use lpt1 to connect to parallel port.
17		AVRDUDE_PORT = /dev/ttyUSB1
	17	#AVRDUDE_PORT = /dev/ttyUSB1
18	18	#AVRDUDE_PORT = /dev/ttyUSB0
	19	AVRDUDE_PORT = /dev/ttyUSB2
19	20
20	21	#
21	22	#

     #include <string.h>
     #include <wireless.h>
     #define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (15)
     #define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (40.0)
     typedef struct {
       unsigned char msgId; //is this necessary?
-...
     /* Globals (internal) */
     static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
     static int robot_x, robot_y;
     static unsigned robot_x, robot_y;
     static volatile int updated_robot_pos_ready;
     #define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (80)
     #define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
     /* Internal function prototypes */
     static unsigned int two_bytes_to_int(char high, char low);
     static unsigned two_bytes_to_int(unsigned char high, unsigned char low);
     static void colonet_handle_receive(char type, int source, unsigned char* packet, int length);
     static void move_to_position_routine(int x, int y);
     static void move_to_position_routine(unsigned x, unsigned y);
     static PacketGroupHandler colonet_pgh;
     /* two_bytes_to_int(char a, char b)
      * Returns int of form [high][low]
      */
     static unsigned int two_bytes_to_int(unsigned char high, unsigned char low) {
       return (high<<8) | low;
+    }
     /* Public functions */
     int colonet_init() {
       colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;
-...
+    }
     void request_abs_position() {
       usb_puts("requesting_abs_position\n");
       //usb_puts("requesting_abs_position\n");
       ColonetRobotServerPacket pkt;
       pkt.client_id = -1;
-...
       wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), 0);
+    }
     static int distsquared(int x1, int y1,int x2, int y2) {
       return (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1);
     static float sqrt_approx(float x) {
       float x2 = x*x;
       float x3 = x*x2;
       return 0.00014*x3 - 0.0078*x2 + 0.29*x + 0.84;
+    }
     static float dist(float x1, float y1, float x2, float y2) {
       return sqrt_approx((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
+    }
     /* cubic approximation of arctan. */
     /*static void arctan(float t) {
       float t3 = t*t*t;
-...
       motor2_set(motor2_dir, ml);
+    }
     static float sqrt_approx(float x) {
       float x2 = x*x;
       float x3 = x*x2;
     static void move_to_position_routine(unsigned target_x, unsigned target_y) {
       usb_puts("move to absolute position routine!\n");
       updated_robot_pos_ready = 0;
       request_abs_position(); // While we're doing this computation, server can be reporting next position.
       return 0.00014*x3 - 0.0078*x2 + 0.29*x + 0.84;
+    }
       int count = 0;
       while (!updated_robot_pos_ready) {
         wl_do();
         if (count++ == 5000) { // in case the server missed it...
           request_abs_position();
           count = 0;
+        }
+      }
     static void move_to_position_routine(int target_x, int target_y) {
       int last_x = robot_x, last_y = robot_y;
     //  usb_puts("got past first loop.\n");
       unsigned last_x = robot_x, last_y = robot_y;
       char buf[40];
       sprintf(buf, "cur dist is %d\n", distsquared(robot_x, robot_y, target_x, target_y));
       usb_puts(buf);
       sprintf(buf, "radius squared is %d\n", TARGET_POSITION_STOP_DISTANCE_THRESHOLD*TARGET_POSITION_STOP_DISTANCE_THRESHOLD);
       usb_puts(buf);
       //sprintf(buf, "cur dist is %f\n", dist((float)robot_x, (float)robot_y, (float)target_x, (float)target_y));
       //usb_puts(buf);
       //sprintf(buf, "radius squared is %f\n", TARGET_POSITION_STOP_DISTANCE_THRESHOLD);
       //usb_puts(buf);
       while (distsquared(robot_x, robot_y, target_x, target_y) >
             TARGET_POSITION_STOP_DISTANCE_THRESHOLD*TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
     //  usb_puts("entering while loop.\n");
       while (dist(robot_x, robot_y, target_x, target_y) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
     //    usb_puts("inside while loop.\n");
         updated_robot_pos_ready = 0;
         request_abs_position(); // While we're doing this computation, server can be reporting next position.
         int count = 0;
         while (!updated_robot_pos_ready) { // ghetto condition variable
           wl_do();
     //    usb_puts("after request_abs_position.\n");
           if (count++ == 10000) { // in case the server missed it...
             request_abs_position();
             count = 0;
+          }
+        }
         int cur_robot_x = robot_x;
         int cur_robot_y = robot_y;
         //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 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);
         usb_puts(buf);
         int e_mag = e_x*e_x + e_y*e_y;
         int motor_differential = e_mag;// / 100;
     /*
         sprintf(buf, "Current position: %d %d\n", cur_robot_x, cur_robot_y);
         usb_puts(buf);
         sprintf(buf, "Target position: %d %d\n", target_x, target_y);
         usb_puts(buf);
         updated_robot_pos_ready = 0;
         request_abs_position(); // While we're doing this computation, server can be reporting next position.
         float r_x = (float)target_x - cur_robot_x;
         float r_y = (float)target_y - cur_robot_y;
         float r_mag = sqrt_approx(r_x * r_x + r_y * r_y);
-...
         sprintf(buf, "Error: <%f,%f>; mag: %f\n", e_x, e_y, e_mag);
         usb_puts(buf);
         /* Motor differential proportional to magnitude of directional error. */
         int motor_differential = (int)(e_mag * ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST);
         // Motor differential proportional to magnitude of directional error.
       //  int motor_differential = (int)(e_mag * ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST);
     */
         /* Determine left or right by transforming error vector to robot axes */
         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 = mat[0][0] * e_x + mat[0][1] * e_y;
     /*
         // Determine left or right by transforming error vector to robot axes
         // Perpendicular
         float p_x = v_y;
         float p_y = -v_x;
         /* Inverse matrix */
         // Inverse matrix
         float coeff = 1.0 / (p_x * v_y - v_x * p_y);
         float mat[2][2];
         mat[0][0] = v_y * coeff;
-...
         float e_trans_x = mat[0][0] * e_x + mat[0][1] * e_y;
         //float e_trans_y = mat[1][0] * e_x + mat[1][1] * e_y;  //Not needed for our purposes.
     */
         if (e_trans_x < 0) {
           motor_differential = -motor_differential;
+        }
-...
         last_y = cur_robot_y;
         set_motors_with_differential(motor_differential);
         count = 0;
         while (!updated_robot_pos_ready) { // ghetto condition variable
           wl_do();
           if (count++ == 5000) { // in case the server missed it...
             request_abs_position();
             count = 0;
+          }
+        }
+      }
       usb_puts("reached destination!\n");
+    }
-...
     static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) {
       ColonetRobotServerPacket* pkt= (ColonetRobotServerPacket*)packet;
       unsigned char* args; //up to 7 char args
       unsigned int int_args[3]; //up to 3 int (2-byte) args
       unsigned int_args[3]; //up to 3 int (2-byte) args
       char buf[40];
-...
     /*       break; */
         case SERVER_REPORT_POSITION_TO_ROBOT:
           robot_x = int_args[0];
           robot_y = int_args[1];
           robot_x = (unsigned)int_args[0];
           robot_y = (unsigned)int_args[1];
           updated_robot_pos_ready = 1;
           sprintf(buf, "pos is: %d %d\n", robot_x, robot_y);
           sprintf(buf, "pos is: %u %u\n", robot_x, robot_y);
           usb_puts(buf);
           break;
         case MOVE_TO_ABSOLUTE_POSITION:
           usb_puts("move to abs position2!\n");
           move_to_position_routine(int_args[0], int_args[1]);
           move_to_position_routine((unsigned)int_args[0], (unsigned)int_args[1]);
           break;
           //Buzzer
-...
         usb_puts(buf);
+      }
+    }
     /* two_bytes_to_int(char a, char b)
      * Returns int of form [high][low]
      */
     static unsigned int two_bytes_to_int(char high, char low) {
       return (((unsigned int)high)<<8) + (unsigned int)low;
+    }

trunk/code/projects/colonet/server/colonet_wireless.cpp
102	102	pkt.msg_code = msg_code;
103	103
104	104	for (int i = 0; i < PACKET_DATA_LEN; i++) {
	105	printf("data %i: %u\n", i, args[i]);
105	106	pkt.data[i] = args[i];
106	107	}
107	108

     #include <vision.h>
     using namespace std;
     #define MAX_POSITIONS 20
     #define SAME_ROBOT_DISTANCE_THRESHOLD 30
     #define ROBOT_DELETE_BUFFER 5
     using namespace std;
     class PositionMonitor {

     #include <stdlib.h>
     #define MINH 100 //min threshold level
     #define MAXH 160 //max threshold level
     #define MAXH 150 //max threshold level
     #define DEBUG 0 //Debug to find threshold level

      * @date 2/4/08
      */
     //TODO: make this file asynchronous
     #include <map>
     #include <PositionMonitor.h>
     #include <stdio.h>
     #include <stdlib.h>
     #include <vision.h>
     #include <stdio.h>
     #include <map>
     using namespace std;
     #define MAX_POSITIONS 20
     #define SAME_ROBOT_DISTANCE_THRESHOLD 110
     #define ROBOT_DELETE_BUFFER 10
     #define CAM_UPDATE_PERIOD 100000
     PositionMonitor::PositionMonitor() {
       //TODO: don't hardcode this file name
       //TODO: check for error returned from init
-...
     void PositionMonitor::run() {
       while (1) {
         updatePositions();
         usleep(500000);
         usleep(CAM_UPDATE_PERIOD);
+      }
+    }
-...
           if (isProbablySameRobot(newPos, oldPos)) {
             //TODO: is this the right use of an iterator?
             newPositionMap.insert(make_pair(iter->first, newPos));
     	deleteBufferMap.erase(iter->first);
     	deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER));
             deleteBufferMap.erase(iter->first);
             deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER));
             break;
+          }
+        }
-...
           int bufferValue = deleteBufferMap[iter2->first];
           bufferValue--;
           if (bufferValue > 0) {
     	newPositionMap.insert(make_pair(currId, iter2->second));
     	deleteBufferMap.erase(currId);
     	deleteBufferMap.insert(make_pair(currId, bufferValue));
             newPositionMap.insert(make_pair(currId, iter2->second));
             deleteBufferMap.erase(currId);
             deleteBufferMap.insert(make_pair(currId, bufferValue));
           } else {
     	deleteBufferMap.erase(currId);
             deleteBufferMap.erase(currId);
+          }
+        }
+      }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 550