right_delta_tick = right_delta_tick - 2*SHRT_MAX;  if (right_delta_tick < -SHRT_MAX/2)    right_delta_tick = right_delta_tick + 2*SHRT_MAX;  left_last_tick = left_tick;  right_last_tick = right_tick;  right_delta_tick = -right_delta_tick;  left_displacement = delta_tick_to_metres(left_delta_tick);  right_displacement = delta_tick_to_metres(right_delta_tick);  // velezj: For RssII Arm  shoulder_pwm = buffer[ORC_SHOULDER_MOTOR_ACTUAL_PWM];  elbow_pwm = buffer[ORC_ELBOW_MOTOR_ACTUAL_PWM];  shoulder_dir = ((buffer[ORC_SHOULDER_MOTOR_DIR] & 0x0f) ) & 0x03;  elbow_dir = ((buffer[ORC_ELBOW_MOTOR_DIR] & 0x0f) ) & 0x03;  if (shoulder_dir == ORC_BACKWARD)    shoulder_pwm = -shoulder_pwm;  if (elbow_dir == ORC_BACKWARD)    elbow_pwm = -elbow_pwm;  shoulder_tick = ((buffer[ORC_SHOULDER_MOTOR_ENCODER]&0x00ff)<<8)+    (buffer[ORC_SHOULDER_MOTOR_ENCODER+1]&0x00ff);  shoulder_delta_tick = shoulder_tick - shoulder_last_tick;  if (shoulder_delta_tick > SHRT_MAX/2)    shoulder_delta_tick = shoulder_delta_tick - 2*SHRT_MAX;  if (shoulder_delta_tick < -SHRT_MAX/2)    shoulder_delta_tick = shoulder_delta_tick + 2*SHRT_MAX;  elbow_tick = ((buffer[ORC_ELBOW_MOTOR_ENCODER]&0x00ff)<<8)+    (buffer[ORC_ELBOW_MOTOR_ENCODER+1]&0x00ff);  elbow_delta_tick = elbow_tick - elbow_last_tick;  if (elbow_delta_tick > SHRT_MAX/2)    elbow_delta_tick = elbow_delta_tick - 2*SHRT_MAX;  if (elbow_delta_tick < -SHRT_MAX/2)    elbow_delta_tick = elbow_delta_tick + 2*SHRT_MAX;  shoulder_last_tick = shoulder_tick;  elbow_last_tick = elbow_tick;  //elbow_delta_tick = -elbow_delta_tick;  shoulder_angle_change = delta_ticks_to_angle(shoulder_delta_tick);  elbow_angle_change = delta_ticks_to_angle(elbow_delta_tick);  if( fabs( shoulder_desired_theta - shoulder_theta ) > 0.01 ) {    printf( "shoulder_delta_theta: %f  (%d)\n", shoulder_angle_change, shoulder_delta_tick );  }  if( fabs( elbow_desired_theta - elbow_theta ) > 0.01 ) {    printf( "elbow_delta_theta: %f  (%d)\n", elbow_angle_change, elbow_delta_tick );  }  time_ticks = ((buffer[ORC_SLAVE_TIME]&0x00ff)<<8)+    (buffer[ORC_SLAVE_TIME+1]&0x00ff);  delta_slave_ticks = time_ticks - last_slave_ticks;  last_slave_ticks = time_ticks;  if (delta_slave_ticks > SHRT_MAX/2)    delta_slave_ticks -= 2*SHRT_MAX;  if (delta_slave_ticks < -SHRT_MAX/2)    delta_slave_ticks += 2*SHRT_MAX;  delta_slave_time = delta_slave_ticks/4000.0;  displacement = (left_displacement+right_displacement)/2;  rotation = atan2(right_displacement-left_displacement, ORC_WHEEL_BASE);  left_velocity = left_displacement/delta_slave_time;  right_velocity = right_displacement/delta_slave_time;  x = x+cos(theta);  y = y+sin(theta);  theta = carmen_normalize_theta(theta+rotation);  // velezj: For RssII Arm  shoulder_theta = shoulder_theta + shoulder_angle_change;  elbow_theta = elbow_theta + elbow_angle_change;  // since elbow relative to shoulder and when the shoulder moves to elbow angle  // in fact does NOT change with it, so we must update our elbow angle even when  // only the shoulder moves  //elbow_theta -= shoulder_angle_change;   shoulder_angular_velocity = shoulder_angle_change / delta_slave_time;  elbow_angular_velocity = elbow_angle_change / delta_slave_time;  if( fabs( shoulder_desired_theta - shoulder_theta ) > 0.01 || fabs( elbow_desired_theta - elbow_theta ) > 0.01 ) {    printf( "shoulder_theta: %f  (av = %f)\n", shoulder_theta, shoulder_angular_velocity );    printf( "elbow_theta:    %f  (av = %f)\n", elbow_theta, elbow_angular_velocity  );  }  time_since_last_command = carmen_get_time() - last_command_time;  last_command_time = carmen_get_time();  command_velocity(left_desired_velocity, left_velocity, left_pwm,       ORC_LEFT_MOTOR);  command_velocity(right_desired_velocity, right_velocity, right_pwm,       ORC_RIGHT_MOTOR);}int carmen_arm_direct_reset(void){  printf("carmen_arm_direct_reset\n");  initialized = 0;  return 0;}int carmen_arm_direct_initialize(char *model __attribute__ ((unused)), char *dev){  int result;  unsigned char buffer[5];  result = carmen_serial_connect(&serial_fd, dev);  if(result == -1) {    serial_fd = -1;    return -1;  }  if (strstr(dev, "USB") == NULL) {    carmen_warn("This doesn't look like a USB port. Setting linespeed to 115200\n");    carmen_serial_configure(serial_fd, 115200, "8N1");  } else {    carmen_warn("This looks like a USB port. Setting linespeed to 500000\n");    carmen_serial_configure(serial_fd, 500000, "8N1");  }  fprintf(stderr, "Clearing input buffer...");  carmen_serial_ClearInputBuffer(serial_fd);  carmen_arm_direct_update_status();  carmen_arm_direct_update_status();  carmen_serial_ClearInputBuffer(serial_fd);  fprintf(stderr, " ok.\n");  buffer[0] = 'C';  buffer[1] = 0; // Motor Encoder 0  buffer[2] = 14; // Quad Phase Fast  fprintf(stderr, "Setting motor encoder 0 into quad phase fast mode... ");  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok.\n");  buffer[1] = 1; // Motor Encoder 0  fprintf(stderr, "Setting motor encoder 1 into quad phase fast mode... ");  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 2; // Motor Encoder 1  fprintf(stderr, "Setting motor encoder 2 into quad phase fast mode... ");  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 3; // Motor Encoder 1  fprintf(stderr, "Setting motor encoder 3 into quad phase fast mode... ");  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[0] = 'W';  buffer[1] = ORC_LEFT_MOTOR;   buffer[2] = 10;             // Quad Phase Fast  fprintf(stderr, "Setting left motor slew to %d... ", buffer[2]);  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = ORC_RIGHT_MOTOR;   fprintf(stderr, "Setting right motor slew to %d... ", buffer[2]);  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");    //Edsinger: added servo   buffer[0] = 'C';  buffer[1] = 0; // Servo0  buffer[2] = ORC_SERVO_PIN; //Servo  fprintf(stderr, "Setting servo pin 0 into servo mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 1; // Servo1  buffer[2] = ORC_SERVO_PIN; //Servo  fprintf(stderr, "Setting servo pin 1 into servo mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 2; // Servo2  buffer[2] = ORC_SERVO_PIN; //Servo  fprintf(stderr, "Setting servo pin 2 into servo mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 3; // Servo3  buffer[2] = ORC_SERVO_PIN; //Servo  fprintf(stderr, "Setting servo pin 3 into servo mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 10; // Bumper  buffer[2] = ORC_DIGITAL_IN_PULL_UP;  fprintf(stderr, "Setting bumper 0 into digital pull-up mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 11; // Bumper  buffer[2] = ORC_DIGITAL_IN_PULL_UP;  fprintf(stderr, "Setting bumper 1 into digital pull-up mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 12; // Bumper  buffer[2] = ORC_DIGITAL_IN_PULL_UP;  fprintf(stderr, "Setting bumper 2 into digital pull-up mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = 13; // Bumper  buffer[2] = ORC_DIGITAL_IN_PULL_UP;  fprintf(stderr, "Setting bumper 3 into digital pull-up mode... ");  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  fprintf(stderr, "Setting gripper into digital pull-up mode... ");  buffer[1] = 12; // Gripper  buffer[2] = ORC_DIGITAL_IN_PULL_UP;  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[0] = 0x4D;  buffer[1] = ORC_LEFT_MOTOR;  buffer[2] = 0;  buffer[3] = 0;  fprintf(stderr, "Zeroing left motor velocity ... ");  if (send_packet_and_ack(buffer, 4, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  buffer[1] = ORC_RIGHT_MOTOR;  fprintf(stderr, "Zeroing right motor velocity... ");  if (send_packet_and_ack(buffer, 4, ORC_SLAVE) < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  fprintf(stderr, "Checking board status... ");  if (carmen_arm_direct_update_status() < 0) {    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);    return -1;  }  fprintf(stderr, "ok\n");  return 0;}int carmen_arm_direct_shutdown(void){  unsigned char buffer[5];  buffer[0] = 0x4D;  buffer[1] = ORC_LEFT_MOTOR;  buffer[2] = ORC_FORWARD;  buffer[3] = 0;  send_packet_and_ack(buffer, 4, ORC_SLAVE);  buffer[0] = 0x4D;  buffer[1] = ORC_RIGHT_MOTOR;  buffer[2] = ORC_FORWARD;  buffer[3] = 0;  send_packet_and_ack(buffer, 4, ORC_SLAVE);  close(serial_fd);  return 0;}static unsigned char *get_status_packet(unsigned char where){  unsigned char byte;  unsigned char *buffer = NULL;  unsigned char routing = 0;  int count;  byte = ORC_STATUS;  count = 0;  do {    send_packet(&byte, 1, where);    buffer = read_packet();    if (buffer != NULL) {      byte = buffer[2];      routing = byte >> 6;      if (buffer[3] == ORC_STATUS && routing == where) {	return buffer;      } else {	carmen_warn("Out of order packet %c %d %d %d\n", buffer[3],		    buffer[3], routing, where);	free(buffer);	buffer = NULL;      }    }    if (!buffer || buffer[0] != 0xED || buffer[3] != 0)      count++;    //    carmen_warn("Count %d\n", count);  } while (count < 10);    return buffer;}int carmen_arm_direct_update_status(void){  unsigned char *buffer;  double start_time = carmen_get_time();  buffer = get_status_packet(ORC_MASTER);  if (buffer == NULL)    return -1;  unpack_master_packet(buffer);  free(buffer);  if(0)    carmen_warn("time to update: %.2f\n", carmen_get_time() -		start_time);  buffer = get_status_packet(ORC_SLAVE);  if (buffer == NULL)    return -1;  unpack_slave_packet(buffer);  free(buffer);  if(0)  carmen_warn("time to update: %.2f\n", carmen_get_time() -	      start_time);  return 0;}/* void carmen_arm_direct_set(double servos[], int num_servos) *//* { *//*   unsigned char buffer[4]; *//*   unsigned short pwm; *//*   int i; */  /* if (num_servos > 4) { *//*     carmen_warn("orc_lib.c only supports 4 servos (%d sent)\n" *//*                 "Returning only 4\n", num_servos); *//*     num_servos = 4; *//*   } *//*  for (i=0;i<num_servos;i++) *//*    {  *//*      int nservo=(int)servos[i]; *//*     pwm = nservo; *//*     buffer[0] = 0x53; *//*     buffer[1] = i; *//*     buffer[2] = pwm >> 8; *//*     buffer[3] = pwm & 0x00ff; *//*     send_packet(buffer, 4, ORC_MASTER); *//*   } *//* } *//* void carmen_arm_direct_get(double servos[], int num_servos, *//* 			   double *currents, int *gripper) *//* { *//*   int i; */  /*   if (num_servos > 4) { *//*     carmen_warn("orc_lib.c only supports 4 servos (%d requested)\n" *//*                 "Returning only 4\n", num_servos); *//*     num_servos = 4; *//*   } */ /*   for (i = 0; i < num_servos; i++) { *//*     servos[i] = servo_state[i]; *//*   } *//*   if (currents && num_servos > 0) { *//*     currents[0] = servo_current[0]; *//*     if (num_servos > 1) *//*       currents[1] = servo_current[1]; *//*   } *//*   if (gripper) *//*     *gripper = gripper_state; *//* } */