if (fabs(desired_angular_velocity) > .001) {    if (desired_angular_velocity > ORC_MAX_ANGULAR_VEL)      desired_angular_velocity = ORC_MAX_ANGULAR_VEL;    if (desired_angular_velocity < -ORC_MAX_ANGULAR_VEL)      desired_angular_velocity = -ORC_MAX_ANGULAR_VEL;    velError = (desired_angular_velocity - current_angular_velocity);    pTerm = velError * pGain;    if (!*ignore_d_term)      dTerm = (velError - *velErrorPrevPtr) * ORC_D_GAIN;    else {      dTerm = 0;      *ignore_d_term = 0;    }    new_pwm = current_pwm + carmen_round(pTerm + dTerm);    if(0)    carmen_warn("%s %f %f : %f %f : %d %d %d\n", 		(WHICH_MOTOR == ORC_LEFT_MOTOR ? "left" : "right"),		desired_angular_velocity,		current_angular_velocity, pTerm, dTerm,		current_pwm, carmen_round(pTerm+dTerm), new_pwm);    *velErrorPrevPtr = velError;    if (abs(new_pwm) > ORC_MAX_PWM)      new_pwm = (new_pwm > 0 ? ORC_MAX_PWM : -ORC_MAX_PWM);    if (WHICH_MOTOR == ORC_RIGHT_MOTOR) {      if (new_pwm < 0) 	dir = ORC_FORWARD;      else 	dir = ORC_BACKWARD;      //      carmen_warn("left %s\n", (dir == 1) ? "forward" : "backward");    } else {      if (new_pwm < 0) 	dir = ORC_BACKWARD;      else 	dir = ORC_FORWARD;      //      carmen_warn("right %s\n", (dir == 1) ? "forward" : "backward");    }    command_pwm = abs(new_pwm);  } else {    dir = ORC_FORWARD;    command_pwm = 0;  }  if (0)  carmen_warn("Tried to send %d %d %f %f %d\n", command_pwm, new_pwm,	      desired_velocity, current_velocity, WHICH_MOTOR);  buffer[0] = 0x4D;  buffer[1] = WHICH_MOTOR;  buffer[2] = dir;  buffer[3] = command_pwm;  send_packet_and_ack(buffer, 4, ORC_SLAVE);}static double delta_tick_to_metres(int delta_tick){  double revolutions = (double)delta_tick/    (double)(ORC_ENCODER_RESOLUTION*ORC_GEAR_RATIO);  double radians = revolutions*2*M_PI;  double metres = radians*(ORC_WHEEL_DIAMETER/2.0);  return metres;}static double voltage_to_current(unsigned short voltage){  double current;  current = voltage*5.0/65536.0;  // V=IR, R=0.18 ohm  current = current/0.18;  return current;}static int unpack_short (unsigned char *buffer, int offset){  return ((buffer[offset]&0x00ff)<<8)+(buffer[offset+1]&0x00ff);}static void unpack_master_packet(unsigned char *buffer){  int range;  short time_ticks, delta_master_ticks;  unsigned char command_buffer[2];  short digital_io_state;  unsigned short servo_pwm;  int i;  static double last_ping_time = 0;  static int last_ping = ORC_RIGHT_SONAR_PING;  //carmen_warn("Got master packet\n");  //printf("unpack_master_packet\n");  range = unpack_short(buffer, ORC_LEFT_SONAR_RANGE);  if (range == 0xffff)    left_range = 0;  else    left_range = range/1000000.0*331.46/2.0;  range = unpack_short(buffer, ORC_RIGHT_SONAR_RANGE);  if (range == 0xffff)    right_range = 0;  else    right_range = range/1000000.0*331.46/2.0;  time_ticks = buffer[ORC_MASTER_TIME];  delta_master_ticks = time_ticks - last_master_ticks;  if (delta_master_ticks > SHRT_MAX/2)    delta_master_ticks -= 2*SHRT_MAX;  if (delta_master_ticks < -SHRT_MAX/2)    delta_master_ticks += 2*SHRT_MAX;  last_master_ticks = time_ticks;  if (sonar_on && carmen_get_time() - last_ping_time > .05) {    command_buffer[0] = 'R';    if (last_ping == ORC_LEFT_SONAR_PING)      command_buffer[1] = ORC_RIGHT_SONAR_PING;    else      command_buffer[1] = ORC_LEFT_SONAR_PING;    send_packet_and_ack(command_buffer, 2, ORC_MASTER);    last_ping = command_buffer[1];    last_ping_time = carmen_get_time();  }  digital_io_state = unpack_short(buffer, ORC_DIGITAL_IN);  irs[0] = digital_io_state >> ORC_LEFT_IR_ECHO & 1;  irs[1] = digital_io_state >> ORC_RIGHT_IR_ECHO & 1; // 1 means there's a curb (beyond range of IR)  bumpers[0] = digital_io_state >> 8 & 1;  bumpers[1] = digital_io_state >> 9 & 1;  bumpers[2] = digital_io_state >> 10 & 1;  bumpers[3] = digital_io_state >> 11 & 1;  gripper_state = digital_io_state >> 12 & 1;   for (i = 0; i < 4; i++) {    servo_pwm = unpack_short(buffer, ORC_SERVO_PWM_STATE+i*2);    servo_state[i] = servo_pwm;  }}static void unpack_slave_packet(unsigned char *buffer){  short left_tick, right_tick, time_ticks, delta_slave_ticks;  int left_delta_tick, right_delta_tick;  int left_dir, right_dir;  unsigned char left_pinmode, right_pinmode;  unsigned short voltage;  static double start;  int left_orc_encoder, right_orc_encoder;  unsigned char left_slew, right_slew;  //carmen_warn("Got slave packet\n");  //printf("unpack_slave_packet\n");  if (!initialized) {    start = carmen_get_time();    initialized = 1;    x = 0;    y = 0;    theta = 0;    left_last_tick = unpack_short(buffer, ORC_LEFT_MOTOR_ENCODER);    right_last_tick = ((buffer[ORC_RIGHT_MOTOR_ENCODER]&0x00ff)<<8)+      (buffer[ORC_RIGHT_MOTOR_ENCODER+1]&0x00ff);    last_slave_ticks = ((buffer[ORC_SLAVE_TIME]&0x00ff)<<8)+      (buffer[ORC_SLAVE_TIME+1]&0x00ff);    left_pwm = buffer[ORC_LEFT_MOTOR_ACTUAL_PWM];    right_pwm = buffer[ORC_RIGHT_MOTOR_ACTUAL_PWM];    left_dir = ((buffer[ORC_LEFT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;    right_dir = ((buffer[ORC_RIGHT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;    if (left_dir == ORC_BACKWARD)      left_pwm = -left_pwm;    if (right_dir == ORC_FORWARD)      right_pwm = -right_pwm;    return;  }  left_orc_encoder = buffer[ORC_LEFT_ENCODER_STATE];  if (left_orc_encoder != ((0x0e << 4) + 0x0e))    carmen_warn("left encoder is in a bad state: %d %d\n", 		(left_orc_encoder >> 4) & 0xf, (left_orc_encoder & 0xf));  //  else  //    carmen_warn("left encoder is in a good state: %d %d\n",   //		(left_orc_encoder >> 4) & 0xf, (left_orc_encoder & 0xf));  right_orc_encoder = buffer[ORC_RIGHT_ENCODER_STATE];  if (right_orc_encoder != ((0x0e << 4) + 0x0e))    carmen_warn("right encoder is in a bad state: %d %d\n", 		(right_orc_encoder >> 4) & 0xf, right_orc_encoder & 0xf);  //  else  //    carmen_warn("right encoder is in a good state: %d %d\n",   //		(right_orc_encoder >> 4) & 0xf, right_orc_encoder & 0xf);  voltage = unpack_short(buffer, ORC_SERVO_CURRENT);  servo_current[0] = voltage_to_current(voltage);  voltage = unpack_short(buffer, ORC_SERVO_CURRENT+2);  servo_current[1] = voltage_to_current(voltage);  left_pinmode = buffer[ORC_LEFT_PINMODE];  right_pinmode = buffer[ORC_RIGHT_PINMODE];  left_pwm = buffer[ORC_LEFT_MOTOR_ACTUAL_PWM];  right_pwm = buffer[ORC_RIGHT_MOTOR_ACTUAL_PWM];  left_slew = buffer[ORC_LEFT_MOTOR_SLEW];  right_slew = buffer[ORC_RIGHT_MOTOR_SLEW];  if (0)  carmen_warn("left slew: %d right slew: %d\n", left_slew,	      right_slew);  left_dir = ((buffer[ORC_LEFT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;  right_dir = ((buffer[ORC_RIGHT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;  if (left_dir == ORC_BACKWARD)    left_pwm = -left_pwm;  if (right_dir == ORC_FORWARD)    right_pwm = -right_pwm;  left_tick = ((buffer[ORC_LEFT_MOTOR_ENCODER]&0x00ff)<<8)+    (buffer[ORC_LEFT_MOTOR_ENCODER+1]&0x00ff);  left_delta_tick = left_tick - left_last_tick;  if (left_delta_tick > SHRT_MAX/2)    left_delta_tick = left_delta_tick - 2*SHRT_MAX;  if (left_delta_tick < -SHRT_MAX/2)    left_delta_tick = left_delta_tick + 2*SHRT_MAX;  right_tick = ((buffer[ORC_RIGHT_MOTOR_ENCODER]&0x00ff)<<8)+    (buffer[ORC_RIGHT_MOTOR_ENCODER+1]&0x00ff);  right_delta_tick = right_tick - right_last_tick;  if (right_delta_tick > SHRT_MAX/2)    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;  left_delta_tick = -left_delta_tick;  left_displacement = delta_tick_to_metres(left_delta_tick);  right_displacement = delta_tick_to_metres(right_delta_tick);  if (0)  carmen_warn("left %d %f right %d %f\n", left_delta_tick, left_displacement,	      right_delta_tick, right_displacement);  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);  if (0) {    carmen_warn("Displacement: %f rotation: %f\n", displacement,		carmen_radians_to_degrees(rotation));    carmen_warn("before %f %f %f\n", x, y, carmen_radians_to_degrees(theta));  }  x = x+displacement*cos(theta);  y = y+displacement*sin(theta);  theta = carmen_normalize_theta(theta+rotation);  if(0)  carmen_warn("after %f %f %f (%f %f)\n", x, y, carmen_radians_to_degrees(theta),	      displacement, carmen_radians_to_degrees(rotation));  left_velocity = left_displacement/delta_slave_time;  right_velocity = right_displacement/delta_slave_time;  carmen_base_command_velocity(left_desired_velocity, left_velocity,       ORC_LEFT_MOTOR);  carmen_base_command_velocity(right_desired_velocity, right_velocity,       ORC_RIGHT_MOTOR);  time_since_last_command = carmen_get_time() - last_command_time;  last_command_time = carmen_get_time();}int carmen_base_direct_sonar_on(void){  printf("carmen_base_direct_sonar_on\n");  sonar_on = 1;  return 2;}int carmen_base_direct_sonar_off(void){  sonar_on = 0;  return 2;}int carmen_base_direct_reset(void){  printf("carmen_base_direct_reset\n");  x = 0;  y = 0;  theta = 0;  initialized = 0;  return 0;}static int orc_set_encoder_quad_phase_fast(int motor) {  unsigned char buffer[5];  buffer[0] = 'C';  buffer[1] = motor; // Motor Encoder 0  buffer[2] = ORC_QUAD_PHASE_FAST; // Quad Phase Fast  fprintf(stderr, "Setting motor channel %d into quad phase fast mode... ",	  motor);  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");  return 0;}static int orc_set_motor_slew(int motor, int slew) {  unsigned char buffer[5];  buffer[0] = 'W';  buffer[1] = motor;  buffer[2] = slew;  fprintf(stderr, "Setting %s motor slew to %d... ", 	  (motor == ORC_LEFT_MOTOR ? "left" : "right") , slew);  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");  return 0;}int orc_set_pin(int pin, char *description, int pin_mode) {  unsigned char buffer[5];  char *modes[15] = {"digital in", "digital in (pull-up)",		    "digital in (pull-down)", "digital out",		    "digital out (slow)", "servo", "sonar ping",		    "sonar echo", "analog in", "analog out",		    "clock generator out", "quadrature phase",		    "mono phase", "unknown", "quad-phase fast"};  buffer[0] = 'C';  buffer[1] = pin; // Servo0  buffer[2] = pin_mode; //Servo  fprintf(stderr, "Setting %s pin %d into %s mode... ", description, pin,	  modes[pin_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");  return 0;}int orc_set_pwm(int motor, int dir, int pwm){  unsigned char buffer[5];  buffer[0] = 0x4D;