Ejemplo n.º 1
0
Archivo: servo.cpp Proyecto: Neobot/PC
int main(void) {
    unsigned long home[32]  = {0L};
    unsigned long frame[32] = {0L};
    
    // first set the correct RoBoard version
    roboio_SetRBVer(RB_100);
    //roboio_SetRBVer(RB_100RD);  // if your RoBoard is RB-100RD
    //roboio_SetRBVer(RB_110);    // if your RoBoard is RB-110
    //roboio_SetRBVer(RB_050);    // if your RoBoard is RB-050

    rcservo_SetServo(RCSERVO_PINS1, RCSERVO_SERVO_DEFAULT_NOFB);     // select the servo model on pin S1 as non-feedback servo
    rcservo_SetServo(RCSERVO_PINS2, RCSERVO_SERVO_DEFAULT_NOFB);     // select the servo model on pin S2 as non-feedback servo
    if (rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2) == false)  // set PWM/GPIO pins S1 & S2 as Servo mode
    {
        printf("ERROR: fail to init RC Servo lib (%s)!\n", roboio_GetErrMsg());
        return -1;
    }

    home[0] = home[1] = 1500L;  // set the initial home position of all servos as 1500us
    rcservo_EnterPlayMode_HOME(home);  // enter Action Playing Mode for moving servos

    printf("press ENTER to move servo on pins S1 & S2.\n"); getchar();
    frame[0] = 900L;   // move the servo on pin S1 to position 900us
    frame[1] = 1800L;  // move the servo on pin S2 to position 1800us
    rcservo_MoveTo(frame, 2000L);  // move servos to the target positions in 2000ms

    printf("press ENTER to move only the servo on pin S1.\n"); getchar();
    rcservo_MoveOne(RCSERVO_PINS1, 1800L, 500L);  // move the servo to position 1800us in 300ms

    printf("press ENTER to move servo on pins S1 & S2.\n"); getchar();
    frame[0] = 900L;   // move the servo on pin S1 to position 900us
    frame[1] = 900L;   // move the servo on pin S2 to position 900us
    rcservo_MoveTo(frame, 3000L);  // move servos to the target positions in 3000ms

    printf("Complete.\n");

    rcservo_Close();  // close RC Servo lib
    return 0;
}
Ejemplo n.º 2
0
  void executeJointState(const sensor_msgs::JointStateConstPtr& msg, bool alternate=false)
  {
    // TODO: something better than this for velocity and duration.
    //       it could be possible to work out a scheme where each servo can get
    //       an individual duration.  but what about the velocity interpretation?
    //       the concept of velocity is completely unaccounted for in the roboio.
    uint32_t duration;
    if (msg->velocity.size())
      duration = msg->velocity[0];
    else
      duration = 300;
    
    std::map<int, unsigned short> icsframe;
    for (size_t i=0; i < msg->name.size(); i++)
    {
      Servo& servo = servos_[msg->name[i]];
      if (servo.channel_ < 0 || servo.channel_ > 32)
        continue;

      float rot_range = servo.max_rot_ - servo.min_rot_;
      float pwm_range = servo.max_pwm_ - servo.min_pwm_; 
      int   pwm_center = (servo.max_pwm_ + servo.min_pwm_) / 2;
      float pwm_per_rot = pwm_range / rot_range;
      float f_pwm_val = (float)msg->position[i] * pwm_per_rot;
      unsigned int i_pwm_val = (int)round(f_pwm_val) + (int)servo.trim_pwm_ + (int)pwm_center;    
      
      if (servo.bus_ == Servo::COM4)
      	icsframe[servo.channel_] = i_pwm_val;
      else 
      	playframe_[servo.channel_-1] = i_pwm_val;
    }

    // OPTIONAL?: wait for previous movement to finish:
    //  while (rcservo_PlayAction() != RCSERVO_PLAYEND) { }
    //  or rcservo_MoveTo(playframe_, duration);
    
    if (alternate)
    {
    	pthread_mutex_lock(&playframe_mutex_);
    		rcservo_MoveTo(playframe_, duration);
      pthread_mutex_unlock(&playframe_mutex_);  
    }
    else
    {
      pthread_mutex_lock(&playframe_mutex_);
        rcservo_SetAction(playframe_, duration);
      pthread_mutex_unlock(&playframe_mutex_);
    }

		// TODO: no concept of duration here, could use the speed parameters, 
    //       or could write a separate thread that interpolates for the serial
    pthread_mutex_lock(&com4_mutex_);     
      std::map<int, unsigned short>::iterator i = icsframe.begin();
      for(; i != icsframe.end(); ++i)
      {
        int channel = i->first;
        unsigned short pos = i->second;
        com4_ics_pos(channel, pos);
        usleep(1);
      }   	
    pthread_mutex_unlock(&com4_mutex_);
  }