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());
}
int main()
{
unsigned long usedchannel = RCSERVO_USECHANNEL0 +RCSERVO_USECHANNEL1 +RCSERVO_USECHANNEL2 +
RCSERVO_USECHANNEL4 +RCSERVO_USECHANNEL5 +RCSERVO_USECHANNEL6+
RCSERVO_USECHANNEL9 +RCSERVO_USECHANNEL10+RCSERVO_USECHANNEL11+
RCSERVO_USECHANNEL13+RCSERVO_USECHANNEL14+RCSERVO_USECHANNEL15+
RCSERVO_USECHANNEL16+RCSERVO_USECHANNEL17+RCSERVO_USECHANNEL18+
RCSERVO_USECHANNEL21+RCSERVO_USECHANNEL22+RCSERVO_USECHANNEL23; // for RB-100
if(allegro_init())
{
printf("error:initialize allegro library failed!!\n");
return -1;
}
if(install_keyboard())
{
printf("error:initialize keybaord failed!!\n");
return -1;
}
roboio_SetRBVer(RB_100);
if(rcservo_SetServos(usedchannel, RCSERVO_DMP_RS0263) == false)
{
printf("Set servo fails!%s\n",roboio_GetErrMsg());
return -1;
}
if(rcservo_Initialize(usedchannel) == false)
{
printf("RC servo initialize fails!%s\n",roboio_GetErrMsg());
return -1;
}
rcservo_SetFPS(500);
rcservo_EnterPlayMode_NOFB(NORMAL);
SetVal();
PRE_STATE = STATE = BALANCE;
ADJUST = 0.0;
BODY_YAW = 0.0;
BODY_PITCH = 0.0;
BODY_ROLL = 0.0;
POS_GAIN[0] = 0.0;
POS_GAIN[1] = 0.0;
POS_GAIN[2] = 0.0;
Prepare();
while(STATE != EXIT)
{
GetInput();
switch(STATE)
{
case FORWARD:
case BACKWARD:
case RIGHTWARD:
case LEFTWARD:
case FLWARD:
case FRWARD:
case BLWARD:
case BRWARD:
case LCIRCLE:
case RCIRCLE:
PlayMotion();
break;
case IDLE:
Idle();
break;
case BALANCE:
Stable();
break;
default:
break;
}
}
rcservo_Close();
i2c_Close();
remove_keyboard();
return 0;
}
