reb.cc

机器人仿真平台,和stage配合运行

	// reset the odometry
	if (config_size != sizeof(player_position_resetodom_config_t)) {
	  fprintf(stderr, "REB: pos reset odom req got wrong size (%d)\n", config_size);
	  if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}

#ifdef DEBUG_CONFIG
	printf("REB: RESET_ODOM_REQ\n");
#endif
	
	SetOdometry(0,0,0);
	
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;
      // END POSITION IOCTLS ////////////

      case PLAYER_POSITION_POSITION_MODE_REQ: {
	// select velocity or position mode
	// 0 for velocity mode
	// 1 for position mode
	if (config_size != sizeof(player_position_position_mode_req_t)) {
	  fprintf(stderr, "REB: pos vel mode req got wrong size (%d)\n", config_size);
	  if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}

	player_position_position_mode_req_t *posmode = 
                (player_position_position_mode_req_t *)config_buffer;
#ifdef DEBUG_CONFIG
	printf("REB: POSITION_MODE_REQ %d\n", posmode->state);
#endif
	
	if (posmode->state) {
	  this->velocity_mode = false;
	  this->pos_update_period = REB_POS_UPDATE_PERIOD_POS;
	} else {
	  this->velocity_mode = true;
	  this->pos_update_period = REB_POS_UPDATE_PERIOD_VEL;
	}
	
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;
      
      case PLAYER_POSITION_SET_ODOM_REQ: {
	// set the odometry to a given position
	if (config_size != sizeof(player_position_set_odom_req_t)) {
	  fprintf(stderr, "REB: pos set odom req got wrong size (%d)\n",
		  config_size);
	  if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}

	player_position_set_odom_req_t *req = 
                (player_position_set_odom_req_t *)config_buffer;

#ifdef DEBUG_CONFIG
	int x,y;
	short theta;
	x = ntohl(req->x);
	y = ntohl(req->y);
	theta = ntohs(req->theta);

	printf("REB: SET_ODOM_REQ x=%d y=%d theta=%d\n", x, y, theta);
#endif
	SetOdometry(req->x, req->y, req->theta);

	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;

      case PLAYER_POSITION_SPEED_PID_REQ: {
	// set up the velocity PID on the REB
	// kp, ki, kd are used
	if (config_size != sizeof(player_position_speed_pid_req_t)) {
	  fprintf(stderr, "REB: pos speed PID req got wrong size (%d)\n", config_size);
	  if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}

	player_position_speed_pid_req_t *pid = 
                (player_position_speed_pid_req_t *)config_buffer;
	int kp = ntohl(pid->kp);
	int ki = ntohl(pid->ki);
	int kd = ntohl(pid->kd);

#ifdef DEBUG_CONFIG
	printf("REB: SPEED_PID_REQ kp=%d ki=%d kd=%d\n", kp, ki, kd);
#endif
	
	ConfigSpeedPID(REB_MOTOR_LEFT, kp, ki, kd);
	ConfigSpeedPID(REB_MOTOR_RIGHT, kp, ki, kd);
	
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;
      
      case PLAYER_POSITION_POSITION_PID_REQ: {
	// set up the position PID on the REB
	// kp, ki, kd are used
	if (config_size != sizeof(player_position_position_pid_req_t)) {
	  fprintf(stderr, "REB: pos pos PID req got wrong size (%d)\n", config_size);
	  if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}
	
	player_position_position_pid_req_t *pid = 
                (player_position_position_pid_req_t *)config_buffer;
	int kp = ntohl(pid->kp);
	int ki = ntohl(pid->ki);
	int kd = ntohl(pid->kd);

#ifdef DEBUG_CONFIG
	printf("REB: POS_PID_REQ kp=%d ki=%d kd=%d\n", kp, ki, kd);
#endif
	
	ConfigPosPID(REB_MOTOR_LEFT, kp, ki, kd);
	ConfigPosPID(REB_MOTOR_RIGHT, kp, ki, kd);
	
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;
      
      case PLAYER_POSITION_SPEED_PROF_REQ: {
	// set the speed profile for position mode 
	// speed is max speed
	// acc is max acceleration
	if (config_size != sizeof(player_position_speed_prof_req_t)) {
	  fprintf(stderr, "REB: pos speed prof req got wrong size (%d)\n", config_size);
	  if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}

	player_position_speed_prof_req_t *prof = 
                (player_position_speed_prof_req_t *)config_buffer;	
	int spd = ntohs(prof->speed);
	int acc = ntohs(prof->acc);

#ifdef DEBUG_CONFIG	
	printf("REB: SPEED_PROF_REQ: spd=%d acc=%d  spdu=%g accu=%g\n", spd, acc,
	       spd*PlayerUBotRobotParams[this->param_index].PulsesPerMMMS,
	       acc*PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);
#endif
	// have to convert spd from mm/s to pulse/10ms
	spd = (int) rint((double)spd * 
			 PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);
	
	// have to convert acc from mm/s^2 to pulses/256/(10ms^2)
	acc = (int) rint((double)acc * 
			 PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);
	// now have to turn into pulse/256
	//	acc *= 256;
	
	if (acc > REB_MAX_ACC) {
	  acc = REB_MAX_ACC;
	} else if (acc == 0) {
	  acc = REB_MIN_ACC;
	}
	
#ifdef DEBUG_CONFIG
	printf("REB: SPEED_PROF_REQ: SPD=%d  ACC=%d\n", spd, acc);
	ConfigSpeedProfile(REB_MOTOR_LEFT, spd, acc);
	ConfigSpeedProfile(REB_MOTOR_RIGHT, spd, acc);
#endif
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;
      
      default:
	fprintf(stderr, "REB: got unknown position config command\n");
	break;
      }
      break;
      // END REB_POSITION IOCTLS ////////////
      
    default:
      printf("REB: unknown config code %d\n", id.code);
    }
  }
}


/* this will update the data that is sent to clients
 * just call separate functions to take care of it
 *
 * returns:
 */
void
REB::UpdateData()
{
  player_reb_data_t d;
    
  // struct timeval curr, end;
  //  int timems;

  Lock();
  memcpy(&d, this->data, sizeof(player_reb_data_t));
  Unlock();

  // get time since last ir update in ms
  //timems = (curr.tv_sec - last_ir_update.tv_sec)*1000 +
  //    (curr.tv_usec - last_ir_update.tv_usec)/1000;
  
  // we dont want to update IR during position mode moves
  // because it uses a lot of b/w on the serial port... FIX

  //  if (this->ir_subscriptions && timems >= REB_IR_UPDATE_PERIOD) {

  Lock();
  //  GlobalTime->GetTime(&curr);
  UpdateIRData(&d);
  //  GlobalTime->GetTime(&end);
  Unlock();
  //  printf("REB: IR PER=%d us\n", (end.tv_sec - curr.tv_sec)*1000000 +
  //	 (end.tv_usec - curr.tv_usec));

  //    last_ir_update = curr;
    //  }

  //    timems = (curr.tv_sec - last_power_update.tv_sec)*1000 +
  //    (curr.tv_usec - last_power_update.tv_usec)/1000;
    //    printf("REB: power=%d timems=%d\n", power_subscriptions, timems);
  //   if (this->power_subscriptions && timems >= REB_POWER_UPDATE_PERIOD) {
  Lock();
  UpdatePowerData(&d);
  Unlock();

    //  last_power_update = curr;
    //  }

    //  timems = (curr.tv_sec - last_pos_update.tv_sec)*1000 +
    //    (curr.tv_usec - last_pos_update.tv_usec)/1000;


    //  if (this->pos_subscriptions && timems >= this->pos_update_period) {
  Lock();
  //  GlobalTime->GetTime(&curr);
  UpdatePosData(&d);
  //  GlobalTime->GetTime(&end);
  Unlock();
  //  printf("REB: POS PER=%d us\n", (end.tv_sec - curr.tv_sec)*1000000 +
  //	 (end.tv_usec - curr.tv_usec));
  
    //    last_pos_update = curr;
    //  }
  
  PutData((unsigned char *)&d, sizeof(d), 0 ,0);
}

/* this will update the IR part of the client data
 * it entails reading the currently active IR sensors
 * and then changing their state to off and turning on
 * 2 new IRs.  
 * NOTE: assumes calling function already called Lock()
 *
 * returns:
 */
void
REB::UpdateIRData(player_reb_data_t * d)
{
  // then we can take a reading
  uint16_t volts[PLAYER_IR_MAX_SAMPLES];
  // struct timeval curr;
  // char buf[64];

  ReadAllIR(volts);
  
  for (int i =0; i < PLAYER_IR_MAX_SAMPLES; i++) {
    // these are in units of 4 mV
    //    volts[i] = ReadAD(i);
    
    // now turn into mV units
    volts[i] *= 4;
    d->ir.voltages[i] = htons(volts[i]);
    //    printf("REB: IR%d=%d\n", i, volts[i]);
  }
  
}

/* this will update the POWER data.. basically just the battery level for now
 * NOTE: assumes calling function already called Lock()
 * returns:
 */
void
REB::UpdatePowerData(player_reb_data_t *d)
{
  // read voltage 
  uint16_t volt = (uint16_t)ReadAD(REB_BATTERY_CHANNEL);
  
  // this is in units of 20mV.. change to mV
  volt *= 20;
  d->power.charge = htons(volt);
}
  
/* this will update the position data.  this entails odometry, etc
 * assumes caller already called Lock()
 * It is in the midst of being converted from floating to fixed point...
 * returns:
 */ 
void
REB::UpdatePosData(player_reb_data_t *d)
{
  // change to fixed point FIX
  double x=0, y=0, theta;
  long x_f, y_f;
  unsigned char target_status =0;
  int lreading=0, rreading=0;
  long mmpp_f = PlayerUBotRobotParams[this->param_index].MMPerPulsesF;

  // check if we have to get a baseline time first
  if (this->refresh_last_position) {
    GlobalTime->GetTime(&(this->last_position));
    this->refresh_last_position = false;
  }

  // get the previous odometry values
  // we know this is from last time, cuz this function
  // is only place to change them
  theta = (double) ((int)ntohs(this->data->position.yaw));

  //covert theta to rad
  theta = DEG2RAD(theta);

  x_f = ntohl(this->data->position.xpos)*REB_FIXED_FACTOR;
  y_f = ntohl(this->data->position.ypos)*REB_FIXED_FACTOR;

  // get the time
  struct timeval curr;
  GlobalTime->GetTime(&curr);

  double v, theta_dot;
  long v_f=0;

  if (this->velocity_mode) {
    int lvel=0,rvel=0;
    lvel = ReadSpeed(REB_MOTOR_LEFT);
    // negate because motor's are facing opposite directions
    rvel = -ReadSpeed(REB_MOTOR_RIGHT); 

    lreading = lvel;
    rreading = rvel;

    // calc time in  sec
    long t_f = (curr.tv_sec - this->last_position.tv_sec)*100 +
      (curr.tv_usec - this->last_position.tv_usec)/10000;

    // this is pulse/10ms
    v_f = (rvel+lvel)/2;
    v_f *= REB_FIXED_FACTOR;

    // rad/pulse
    theta_dot = (rvel- lvel) /
      (PlayerUBotRobotParams[this->param_index].RobotAxleLength *
       PlayerUBotRobotParams[this->param_index].PulsesPerMM);

    theta += theta_dot *t_f;

    // convert from rad/10ms -> rad/s -> deg/s
    theta_dot = theta_dot * 100.0;
    
    // this is pulse/10ms
    long x_dot_f = (long) (v_f * cos(theta));
    long y_dot_f = (long) (v_f * sin(theta));

    // change to deltas mm and add integrate over time
    x_f += (x_dot_f/REB_FIXED_FACTOR) * mmpp_f * t_f;
    y_f += (y_dot_f/REB_FIXED_FACTOR) * mmpp_f * t_f;
    
    x_f /= REB_FIXED_FACTOR;
    y_f /= REB_FIXED_FACTOR;
  } else {
    // in position mode
    int rpos=0, lpos=0;
    uint8_t rtar;
    int mode, error;
    
    v = theta_dot= 0.0;

    // now we read the status of the motion controller
    // DONT ASK ME -- but calling ReadStatus on the LEFT
    // motor seems to cause the REB (the kameleon itself!)
    // to freeze some time after issuing a position mode
    // command -- happens for RIGHT motor too but
    // maybe not as much???
    //  ltar = ReadStatus(REB_MOTOR_LEFT, &mode, &error);
    
    rtar = ReadStatus(REB_MOTOR_RIGHT, &mode, &error);

    target_status = rtar;
    // check for on target so we know to update
    if (!d->position.stall && target_status) {
      // then this is a new one, so do an update

      lpos = ReadPos(REB_MOTOR_LEFT);
      rpos = -ReadPos(REB_MOTOR_RIGHT);

      lreading = lpos;
      rreading = rpos;
      double p;

      // take average pos
      p = (lpos + rpos) /2.0;
      
      // now convert to mm
      p *= PlayerUBotRobotParams[this->param_index].MMPerPulses;
      
      // this should be change in theta in rad
      theta_dot = (rpos - lpos) *
	PlayerUBotRobotParams[this->param_index].MMPerPulses / 
	PlayerUBotRobotParams[this->param_index].RobotAxleLength;
      
      // update our theta
      theta += theta_dot;
      
      // update x & y positions
      x += p * cos(theta);
      y += p * sin(theta);

      x_f = (long) rint(x);
      y_f = (long) rint(y);

      printf("REB: pos mode x=%g y=%g theta=%g\n", x,y, theta);
    } 

  }

  // get integer rounded x,y and theta
  int rx = x_f;
  int ry = y_f;

  int rtheta = (int) rint(RAD2DEG(theta));
  
  // get int rounded angular velocity
  int rtd = (int) rint(RAD2DEG(theta_dot));

  // get int rounded trans velocity (in convert from pulses/10ms -> mm/s)
  
  // need to add the RFF/2 for rounding
  long rv  = (v_f/REB_FIXED_FACTOR) *100 * mmpp_f + (REB_FIXED_FACTOR/2);
  rv/= REB_FIXED_FACTOR;

  // normalize theta
  rtheta %= 360;

  // now make theta positive
  if (rtheta < 0) {
    rtheta += 360;
  }

#ifdef DEBUG_POS
  printf("REB: l%s=%d r%s=%d x=%d y=%d theta=%d trans=%d rot=%d target=%02x\n",
	 this->velocity_mode ? "vel" : "pos", lreading,
	 this->velocity_mode ? "vel" : "pos", rreading, 
	 rx, ry, rtheta,  rv, rtd, target_status);
#endif

  // now write data
  d->position.xpos = htonl(rx);