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;
}
void initRCservo()
{
ServoLibrary::iterator i = servos_.begin();
for(; i != servos_.end(); ++i)
{
//std::string& joint_name = i->first;
Servo& servo = i->second;
if (servo.channel_ >= 1 && servo.channel_ <= 32 && servo.bus_ == Servo::PWM)
{
rcservo_SetServo(servo.channel_ - 1, servo.type_);
if (servo.min_pwm_ != 700 || servo.max_pwm_ != 2300)
rcservo_SetServoParams1(servo.channel_ - 1, 8000L, servo.min_pwm_, servo.max_pwm_);
}
}
if (rcservo_Initialize(servos_.getUsedPWMChannels()) == true)
{
rcservo_EnableMPOS();
rcservo_SetFPS(servo_fps_);
}
else
ROS_ERROR("RoBoIO: %s", roboio_GetErrMsg());
}
