/ - Diff - Colony - Robotics Club

Revision 624

Added by Eugene Marinelli about 16 years ago

robot now abandons seeking if receives a new command

     static unsigned robot_x, robot_y;
     static volatile int updated_robot_pos_ready;
     static volatile int new_movement_command_received;
     #define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
     /* Internal function prototypes */
-...
       robot_x = 0;
       robot_y = 0;
       updated_robot_pos_ready = 0;
       new_movement_command_received = 0;
       return 0;
+    }
-...
+    }
     static void move_to_position_routine(unsigned target_x, unsigned target_y) {
       new_movement_command_received = 0;
       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.
-...
       unsigned last_x = robot_x, last_y = robot_y;
       char buf[40];
       //  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);
     //  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");
       while (!new_movement_command_received &&
              dist(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.
-...
         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);
         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;
-...
         // 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;
     /*
-...
           motor_differential = -motor_differential;
+        }
         sprintf(buf, "motor_diff: %d\n", motor_differential);
         usb_puts(buf);
         // sprintf(buf, "motor_diff: %d\n", motor_differential);
         //    usb_puts(buf);
         last_x = cur_robot_x;
         last_y = cur_robot_y;
-...
+        }
+      }
       usb_puts("reached destination!\n");
       //  usb_puts("reached destination!\n");
+    }
     /* Private functions */
-...
      */
     static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) {
       length = length;
       wl_source = wl_source;
       ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)packet;
       unsigned char* args; //up to 7 char args
-...
       int i;
       for (i = 0; i < length; i++) {
         sprintf(buf, "%d: %d ", i, packet[i]);
         usb_puts(buf);
         usb_puts(buf);
+     }
       usb_puts("\n");
       */
-...
           //Sharp
         case READ_DISTANCE:
           break;
         case LINEARIZE_DISTANCE:
           break;
         case LOG_DISTANCE:
           break;
           //Analog
         case CALL_ANALOG8:
-...
         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);
           sprintf(buf, "it's a battery request\nsent battery value: %s\n", pkt->data);
           usb_puts(buf);
           break;
           //BOM
-...
           break;
+        }
       } else if (type == COLONET_COMMAND) {
         sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
         usb_puts(buf);
         // sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
         // usb_puts(buf);
     /* TODO uncomment this stuff */
     /*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
-...
           break;
         case MOVE_TO_ABSOLUTE_POSITION:
           usb_puts("move to abs position2!\n");
           new_movement_command_received = 1;
           usb_puts("move to abs position!\n");
           move_to_position_routine((unsigned)int_args[0], (unsigned)int_args[1]);
           break;
-...
         case BUZZER_OFF:
           buzzer_off();
           break;
           //Orb
         case ORB_INIT:
           orb_init();
           break;
         //Orb
         case ORB_SET:
           orb_set(args[0], args[1], args[2]);
           break;
-...
         case ORB_ENABLE:
           orb_enable();
           break;
         case LED_INIT:
           break;
         case LED_USER:
           break;
           //Motors
         case MOTORS_INIT:
           usb_puts("calling motors_init\n");
           motors_init();
           break;
         case MOTOR1_SET:
           new_movement_command_received = 1;
           sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
           usb_puts(buf);
           motor1_set(args[0], args[1]);
           break;
         case MOTOR2_SET:
           new_movement_command_received = 1;
           sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
           usb_puts(buf);
           motor2_set(args[0], args[1]);
           break;
         case MOTORS_OFF:
           new_movement_command_received = 1;
           usb_puts("calling motors_off\n");
           motors_off();
           break;
         case MOVE:
           new_movement_command_received = 1;
           sprintf(buf, "calling move with: %d, %d\n", args[0], args[1]);
           usb_puts(buf);
           move(args[0], args[1]);
           break;
         case USB_INIT:
           usb_init();
           break;
         case USB_PUTC:
           usb_putc((char)args[0]);
           break;
         case PRINTF:
           printf("%s", pkt->data);
           usb_puts((char*)pkt->data);
           break;
         case KILL_ROBOT:
           new_movement_command_received = 1;
           motors_off();
           buzzer_off();
           exit(0); //kill the robot

trunk/code/projects/colonet/robot/colonet_dragonfly/Makefile
21	21	cp libcolonet_dragonfly.a $(COLONYROOT)/code/lib/bin/
22	22
23	23	clean:
24		rm -rf *.o
	24	rm -rf .o .a

Also available in: Unified diff

Project

General

Profile

Colony

Revision 624