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 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_);
}
