dTerm = (velError - *velErrorPrevPtr) * ORC_D_GAIN;    current_pwm = (int)(ffTerm + pTerm + *iTermPtr + dTerm);    *velErrorPrevPtr = velError;    if (abs(current_pwm) > ORC_MAX_PWM)      current_pwm = (current_pwm > 0 ? ORC_MAX_PWM : -ORC_MAX_PWM);    if (WHICH_MOTOR == ORC_LEFT_MOTOR)      current_pwm = -current_pwm;    if (current_pwm < 0) {      command_pwm = -current_pwm;      dir = ORC_BACKWARD;    } else {      command_pwm = current_pwm;      dir = ORC_FORWARD;    }  } else {    dir = ORC_FORWARD;    command_pwm = 0;    *iTermPtr = 0.0;  }  buffer[0] = 0x4D;  buffer[1] = WHICH_MOTOR;  buffer[2] = dir;  buffer[3] = command_pwm;  send_packet_and_ack(buffer, 4, ORC_SLAVE);}// velezj: rssII Armunsigned short gripper_theta_to_pwm( double theta ) {  if( theta < 0 ) {    carmen_warn( "gripper angles can ONLY be positive <= PI radians " );    return 0;  }    return ORC_ARM_GRIPPER_MIN_PWM + theta * ORC_ARM_GRIPPER_PWM_PER_RADIAN;}// velezj: rssII Armdouble gripper_pwm_to_theta( unsigned short pwm ) {  return (double)( pwm - ORC_ARM_GRIPPER_MIN_PWM ) / ORC_ARM_GRIPPER_PWM_PER_RADIAN;}// velezj: rssII Armvoid carmen_arm_reset() {  shoulder_iTerm = 0.0;  elbow_iTerm = 0.0;}// velezj: rssII Armvoid carmen_arm_direct_set( double shoulder_desired_angle, double elbow_desired_angle, double gripper_desired_angle ) {  // reset the iterms of the vel first  carmen_arm_reset();  shoulder_desired_theta = shoulder_desired_angle;  elbow_desired_theta = elbow_desired_angle;  gripper_desired_theta = gripper_desired_angle;  // send the gripper angle as a servo command to orc  unsigned char buffer[4];  unsigned short pwm = gripper_theta_to_pwm( gripper_desired_angle );  buffer[0] = 0x53;  buffer[1] = ORC_ARM_GRIPPER_SERVO;  buffer[2] = pwm >> 8;  buffer[3] = pwm & 0x00ff;  send_packet_and_ack(buffer, 4, ORC_MASTER);    printf( "\ncarmen_arm_direct_set: shoulder=%f  elbow=%f  gripper=%f \n", shoulder_desired_angle, elbow_desired_angle, gripper_desired_angle );}void carmen_arm_get_theta_state( double *shoulder_angle, double *elbow_angle, double *gripper_angle ) {  *shoulder_angle = shoulder_theta;  *elbow_angle = elbow_theta;  *gripper_angle = gripper_theta;}void carmen_arm_get_state( double *shoulder_avel, double *elbow_avel ) {  *shoulder_avel = shoulder_angular_velocity;  *elbow_avel = elbow_angular_velocity;}// velezj: rssII Armstatic void carmen_arm_command_angular_velocity( double desired_angular_velocity, double current_angular_velocity, int current_pwm, unsigned char WHICH_MOTOR ) {    double ffTerm = 0.0, pTerm = 0.0, dTerm = 0.0, velError = 0.0;  double * iTermPtr;  double * velErrorPrevPtr;;  unsigned char command_pwm;  unsigned char dir;  unsigned char buffer[4];  char which;  int motor_direction_sign = 1;  int max_pwm, min_pwm;  if (WHICH_MOTOR == ORC_SHOULDER_MOTOR) {    which = 'S';    iTermPtr = &shoulder_iTerm;    velErrorPrevPtr = &shoulder_error_prev;    motor_direction_sign = ORC_SHOULDER_MOTOR_DIRECTION_SIGN;    max_pwm = ORC_SHOULDER_MAX_PWM;    min_pwm = ORC_SHOULDER_MIN_PWM;  }  else {    which = 'E';    iTermPtr = &elbow_iTerm;    velErrorPrevPtr = &elbow_error_prev;    motor_direction_sign = ORC_ELBOW_MOTOR_DIRECTION_SIGN;    max_pwm = ORC_ELBOW_MAX_PWM;    min_pwm = ORC_ELBOW_MIN_PWM;  }  if (fabs(desired_angular_velocity) > .0005) {    if (desired_angular_velocity > ORC_ARM_MAX_ANGULAR_VEL)      desired_angular_velocity = ORC_ARM_MAX_ANGULAR_VEL;    if (desired_angular_velocity < -ORC_ARM_MAX_ANGULAR_VEL)      desired_angular_velocity = -ORC_ARM_MAX_ANGULAR_VEL;    velError = (desired_angular_velocity - current_angular_velocity);    ffTerm = desired_angular_velocity * ORC_ARM_FF_GAIN;    pTerm = velError * ORC_ARM_P_GAIN;    *iTermPtr += velError * ORC_ARM_I_GAIN;    dTerm = (velError - *velErrorPrevPtr) * ORC_ARM_D_GAIN;    current_pwm = (int)(ffTerm + pTerm + *iTermPtr + dTerm);    *velErrorPrevPtr = velError;    if (abs(current_pwm) > max_pwm)      current_pwm = (current_pwm > 0 ? max_pwm : -max_pwm);    if( abs( current_pwm) < min_pwm )      current_pwm = ( current_pwm > 0 ? min_pwm : -min_pwm );    if (WHICH_MOTOR == ORC_SHOULDER_MOTOR)      current_pwm = -current_pwm;    current_pwm *= motor_direction_sign;    if (current_pwm < 0) {      command_pwm = -current_pwm;      dir = ORC_BACKWARD;    } else {      command_pwm = current_pwm;      dir = ORC_FORWARD;    }  } else {    dir = ORC_FORWARD;    command_pwm = 0;    *iTermPtr = 0.0;  }  // debug  if( command_pwm != 0 ) {    printf( "[ %c ] p: %f  i: %f  d: %f   ve: %f\n", which, pTerm, *iTermPtr, dTerm, velError );      printf( "[ %c ] sending PWM: %d\n", which, ( dir == ORC_BACKWARD ? -command_pwm : command_pwm ) );  }  buffer[0] = 0x4D;  buffer[1] = WHICH_MOTOR;  buffer[2] = dir;  buffer[3] = command_pwm;  send_packet_and_ack(buffer, 4, ORC_SLAVE);}// velezj: rssII Armvoid bound_value( double *vPtr, double min, double max ) {  if( *vPtr < min )    *vPtr = min;  if( *vPtr > max )    *vPtr = max;}// velezj: rssII Armdouble sign( double v ) {  if( v < 0.0 )    return -1.0;  return 1.0;}// velezj: rssII Armvoid carmen_arm_control() {  double shoulder_theta_delta = shoulder_desired_theta - shoulder_theta;  double elbow_theta_delta = elbow_desired_theta - elbow_theta;    double pTerm = 0.0, dTerm = 0.0;  double *iTermPtr;  // shoulder  iTermPtr = &shoulder_theta_iTerm;  //printf( "control: shoulder diff: %f    ( %f  -  %f )\n", shoulder_theta_delta, shoulder_desired_theta, shoulder_theta );  if( fabs( shoulder_theta_delta ) > 0.01 ) {    pTerm = shoulder_theta_delta * ORC_ARM_THETA_P_GAIN;    *iTermPtr += ( shoulder_theta_delta * ORC_ARM_THETA_I_GAIN );    dTerm = ( shoulder_theta_delta - shoulder_theta_error_prev ) * ORC_ARM_THETA_D_GAIN;    shoulder_desired_angular_velocity = ( pTerm + *iTermPtr * dTerm );    bound_value( &shoulder_desired_angular_velocity, -ORC_ARM_MAX_ANGULAR_VEL, ORC_ARM_MAX_ANGULAR_VEL );    if( fabs( shoulder_desired_angular_velocity ) < ORC_ARM_MIN_ANGULAR_VEL ) {      shoulder_desired_angular_velocity = sign( shoulder_desired_angular_velocity ) * ORC_ARM_MIN_ANGULAR_VEL;    }    printf( "contorl: shoulder: p: %f   i: %f   d: %f    av: %f\n", pTerm, *iTermPtr, dTerm, shoulder_desired_angular_velocity );  } else {    shoulder_desired_angular_velocity = 0.0;    *iTermPtr = 0.0;    //printf( "control: shoulder: done!\n" );  }  //elbow  iTermPtr = &elbow_theta_iTerm;  //printf( "control: elbow diff: %f    ( %f  -  %f )\n", elbow_theta_delta, elbow_desired_theta, elbow_theta );  if( fabs( elbow_theta_delta ) > 0.01 ) {    pTerm = elbow_theta_delta * ORC_ARM_THETA_P_GAIN;    *iTermPtr += ( elbow_theta_delta * ORC_ARM_THETA_I_GAIN );    dTerm = ( elbow_theta_delta - elbow_theta_error_prev ) * ORC_ARM_THETA_D_GAIN;    elbow_desired_angular_velocity = ( pTerm + *iTermPtr * dTerm );    bound_value( &elbow_desired_angular_velocity, -ORC_ARM_MAX_ANGULAR_VEL, ORC_ARM_MAX_ANGULAR_VEL );    if( fabs( elbow_desired_angular_velocity ) < ORC_ARM_MIN_ANGULAR_VEL ) {      elbow_desired_angular_velocity = sign( elbow_desired_angular_velocity ) * ORC_ARM_MIN_ANGULAR_VEL;    }    printf( "contorl: elbow: p: %f   i: %f   d: %f    av: %f\n", pTerm, *iTermPtr, dTerm, elbow_desired_angular_velocity );  } else {    elbow_desired_angular_velocity = 0.0;    *iTermPtr = 0.0;    //printf( "control: elbow: done!\n" );  }  carmen_arm_command_angular_velocity( shoulder_desired_angular_velocity, shoulder_angular_velocity, shoulder_pwm, ORC_SHOULDER_MOTOR );  carmen_arm_command_angular_velocity( elbow_desired_angular_velocity, elbow_angular_velocity, elbow_pwm, ORC_ELBOW_MOTOR );  //command_velocity( shoulder_desired_angular_velocity * ( ORC_WHEEL_DIAMETER / 2.0 ), shoulder_angular_velocity * ( ORC_WHEEL_DIAMETER / 2.0 ), shoulder_pwm, ORC_LEFT_MOTOR );} 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;}// velezj: for RssII Armstatic double delta_ticks_to_angle( int delta_ticks ) {  double radians = (double)delta_ticks / (double)ORC_ARM_TICKS_PER_RADIAN;  return radians;}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;  char command_buffer[2];  short bumper_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_ECHO);  if (range == 0xffff)    left_range = 0;  else    left_range = range/1000000.0*331.46/2.0;  range = unpack_short(buffer, ORC_RIGHT_SONAR_ECHO);  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();  }  bumper_state = unpack_short(buffer, ORC_DIGITAL_IN);  bumpers[0] = bumper_state >> 8 & 1;  bumpers[1] = bumper_state >> 9 & 1;  bumpers[2] = bumper_state >> 10 & 1;  bumpers[3] = bumper_state >> 11 & 1;  gripper_state = bumper_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;  }  // velezj: rssII Arm  gripper_theta = gripper_pwm_to_theta( unpack_short( buffer, ORC_SERVO_PWM_STATE + ORC_ARM_GRIPPER_SERVO * 2 ) );}static void unpack_slave_packet(unsigned char *buffer){  short left_tick, right_tick, time_ticks, delta_slave_ticks;  double left_displacement, right_displacement;  int left_delta_tick, right_delta_tick;  double delta_slave_time;  int left_dir, right_dir;  unsigned char left_pinmode, right_pinmode;  unsigned short voltage;  static double start;  // velezj: RssII Arm  short shoulder_tick, elbow_tick;  int shoulder_delta_tick, elbow_delta_tick;  int shoulder_dir, elbow_dir;  double shoulder_angle_change, elbow_angle_change;  //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_FORWARD)      left_pwm = -left_pwm;    if (right_dir == ORC_BACKWARD)      right_pwm = -right_pwm;    // velezj: RssII Arm    shoulder_theta = 0.0;    elbow_theta = 0.0;    shoulder_last_tick = unpack_short( buffer, ORC_SHOULDER_MOTOR_ENCODER );    elbow_last_tick = unpack_short( buffer, ORC_ELBOW_MOTOR_ENCODER );    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;    return;  }  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_dir = ((buffer[ORC_LEFT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;  right_dir = ((buffer[ORC_RIGHT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;  if (left_dir == ORC_FORWARD)    left_pwm = -left_pwm;  if (right_dir == ORC_BACKWARD)    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)