/ - Diff - Colony - Robotics Club

Revision 644

Changed move-to-point colonet behavior. Need to test.

     #include <serial.h>
     #include <stdio.h>
     int behavior_status;
     int main(void) {
       dragonfly_init(ALL_ON);

     #include <string.h>
     #include <wireless.h>
     #define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (40.0)
     #define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (40)
     typedef struct {
       unsigned char msgId; //is this necessary?
-...
     static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL];
     static unsigned robot_x, robot_y;
     static unsigned last_x, last_y;
     static volatile int updated_robot_pos_ready;
     static volatile int new_movement_command_received;
     #define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
-...
     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(unsigned x, unsigned y);
     static int dist_squared(int x1, int y1, int x2, int y2);
     static PacketGroupHandler colonet_pgh;
-...
       // TODO this should return an error if wl_init has not been called yet.
       wl_register_packet_group(&colonet_pgh);
       robot_x = 0;
       robot_y = 0;
       robot_x = last_x = 0;
       robot_y = last_y = 0;
       updated_robot_pos_ready = 0;
       new_movement_command_received = 0;
-...
     void request_abs_position() {
       //usb_puts("requesting_abs_position\n");
       ColonetRobotServerPacket pkt;
       pkt.client_id = -1;
       pkt.msg_code = ROBOT_REQUEST_POSITION_FROM_SERVER;
       wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(ColonetRobotServerPacket), 0);
+    }
     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 int dist_squared(int x1, int y1, int x2, int y2) {
       int dx = x1 - x2;
       int dy = y1 - y2;
       return dx*dx + dy*dy;
+    }
     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;
       return -0.039 * t3 + 0.74 * t + 0.00011;
       }*/
     /** Differential is the intended left motor speed - right motor speed. */
     static void set_motors_with_differential(int differential) {
       int ml, mr;
-...
+        }
+      }
     //  usb_puts("got past first loop.\n");
       unsigned last_x = robot_x, last_y = robot_y;
       //  char buf[40];
       //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 (!new_movement_command_received &&
              dist(robot_x, robot_y, target_x, target_y) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
              dist_squared(robot_x, robot_y, target_x, target_y) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
         updated_robot_pos_ready = 0;
         request_abs_position(); // While we're doing this computation, server can be reporting next position.
         request_abs_position();   // While we're doing this computation, server can be reporting next position.
     //    usb_puts("after request_abs_position.\n");
         // e is the error vector (where we want to go)
         int e_x = target_x - robot_x;
         int e_y = target_y - robot_y;
         int cur_robot_x = robot_x;
         int cur_robot_y = robot_y;
         // v is the velocity vector (wehere we just went)
         int v_x = robot_x - last_x;
         int v_y = robot_y - last_y;
         if (abs(v_x) < 2 && abs(v_x) < 2) {
           // if we didn't move since the last check, just go forward.
           motor1_set(0, 240);
           motor2_set(0, 240);
         } else {
           // move according to algortihm
           int e_mag = e_x*e_x + e_y*e_y;
           int motor_differential = e_mag >> 7;   // 128;
         //usb_puts("after cur_robot_x/y = robot_x/y.\n");
           // p is perpendicular to v, directed to the "left" of the robot.
           int p_x = v_y;
           int p_y = -v_x;
           int dot_product_pe = p_x * e_x + p_y * e_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;
         //    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 >> 7;// / 128;
     /*
         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);
         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);
         r_x /= r_mag;
         r_y /= r_mag;
         float v_x = (float)cur_robot_x - last_x;
         float v_y = robot_y - last_y;
         float v_mag = sqrt_approx(v_x*v_x + v_y*v_y);
         v_x /= v_mag;
         v_y /= v_mag;
         float e_x = r_x - v_x;
         float e_y = r_y - v_y;
         float e_mag = sqrt_approx(e_x * e_x + e_y * e_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);
     */
         int p_x = v_y;
         int p_y = -v_x;
         int e_trans_x = p_x * e_x + p_y * 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
         float coeff = 1.0 / (p_x * v_y - v_x * p_y);
         float mat[2][2];
         mat[0][0] = v_y * coeff;
         mat[0][1] = -v_x * coeff;
         //mat[1][0] = -p_y * coeff;     //Not needed for our purposes.
         //mat[1][1] = p_x * 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;
           // if the dot product of p and e is negative, we should turn right. otherwise turn left.
           if (dot_product_pe < 0) {
             motor_differential = -motor_differential;
+          }
           set_motors_with_differential(motor_differential);
+        }
         // sprintf(buf, "motor_diff: %d\n", motor_differential);
         //    usb_puts(buf);
         last_x = cur_robot_x;
         last_y = cur_robot_y;
         set_motors_with_differential(motor_differential);
         // once we move, ask for the position again.
         count = 0;
         while (!updated_robot_pos_ready) { // ghetto condition variable
         while (!updated_robot_pos_ready) {
           wl_do();
           if (count++ == 5000) { // in case the server missed it...
             request_abs_position();
-...
+        }
+      }
       //  usb_puts("reached destination!\n");
       //  Once this function terminates, the robot be at its destination.
+    }
     /* Private functions */
-...
         case BATTERY:
           sprintf((char*)pkt->data, "%d", (int)battery8());
           usb_puts("battery:");
           usb_puts((char*)pkt->data);
           usb_puts("\n");
           // NOTE: wl_send_robot_to_robot_global_packet does not work here!
           wl_send_global_packet(COLONET_PACKET_GROUP_ID, 0, (char*)pkt, sizeof(ColonetRobotServerPacket), 0);
           break;
-...
     /*       break; */
         case SERVER_REPORT_POSITION_TO_ROBOT:
           last_x = robot_x;
           last_y = robot_y;
           robot_x = (unsigned)int_args[0];
           robot_y = (unsigned)int_args[1];
-...
         case MOVE:
           new_movement_command_received = 1;
           /* format for move: left direction, left velocity, right direction, right velocity */
           sprintf(buf, "calling motor1 set with: %d, %d\ncalling motor2 set with: %d %d\n",
             args[0], args[1], args[2], args[3]);
           sprintf(buf, "calling move\n");
           usb_puts(buf);
     	  motor1_set(args[0], args[1]);
     	  motor2_set(args[2], args[3]);

+    	}
     	private synchronized void createAndShowGUI () {
     		paintThread = new Thread(this, "PaintThread");
     		paintThread.start();
     		//paintThread = new Thread(this, "PaintThread");
     		//paintThread.start();
     		// Webcam
     		gc = GraphicsEnvironment.getLocalGraphicsEnvironment().getDefaultScreenDevice().getDefaultConfiguration();

Also available in: Unified diff

Project

General

Profile

Colony

Revision 644