void turnCallback( const servo_mast::mast_turn::ConstPtr& msg )
{
//change the motor position
//valid range is -23 to 232, but for most motors ~30-210
double pos = msg->position;
double acc = msg->acceleration;
double vel = msg->velocity;
bool engage = true;
if( (pos > 30.0) && (pos < 210.0) ) {
CPhidgetAdvancedServo_setPosition( servo, 0, pos );
}
else { engage = false; }
if( engage && (acc > minAcc) && (acc < maxAcc) ) {
CPhidgetAdvancedServo_setAcceleration( servo, 0, acc );
}
else { engage = false; }
if( engage && (vel > minVel) && (vel < maxVel) ) {
CPhidgetAdvancedServo_setVelocityLimit( servo, 0, vel );
}
else { engage = false; }
if( engage ) {
CPhidgetAdvancedServo_setEngaged( servo, 0, 1 );
}
}
void floatCallback( const std_msgs::Float64::ConstPtr& msg )
{
double pos;
pos = (double)msg->data;
serv_pos=pos;
printf( "Got %f \n", pos );;
if( (pos > -30.0) && (pos < 210.0) ) {
CPhidgetAdvancedServo_setPosition( servo, 0, pos );
CPhidgetAdvancedServo_setEngaged( servo, 0, 1 );
}
}
void Servo::run()
{
if(mode == Servo::JOG){
//ジョグモードの場合は単発で目的位置へ移動
CPhidgetAdvancedServo_setPosition (*servo, index, position);
}else if(mode == Servo::PREDEFINED){
//プリディファインドモードの場合は制御ベクトルに従ってそれぞれ移動
if(to == 0){
SetReady();
}else if(to == 1){
SetContact();
}else if(to == 2){
SetHome();
}
}
//// エンゲージ
/*
if(mode == Servo::JOG){
std::cout << "[ サーボモーター制御スレッド ] モータID : " << index << " > エンゲージ（ジョグ）" << std::endl;
Engage(true);
}else if(mode == Servo::KNOCK){
std::cout << "[ サーボモーター制御スレッド ] モータID : " << index << " > エンゲージ（電磁弁）" << std::endl;
CPhidgetAdvancedServo_setPosition (*servo, index, knockPosition);
}
*/
//// ディスエンゲージ動作は行わない
//電磁弁的動作処理時間（0.5秒）
//Sleep(500);
//
//ディスエンゲージ
//std::cout << "ディスエンゲージ" << std::endl;
//CPhidgetAdvancedServo_setEngaged(*servo, index, 0);
}
int teardown()
{
SetupLog("Closing...\n");
#ifndef NO_POWERLIB
power_button_reset();
#endif
CPhidgetAdvancedServo_setPosition(servo, 0, 120);
sleep(2);
// Close IFKIT
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
// close motoControl
CPhidget_close((CPhidgetHandle)motoControl);
CPhidget_delete((CPhidgetHandle)motoControl);
// disengage and close servo
CPhidgetAdvancedServo_setEngaged(servo, 0, 0);
CPhidget_close((CPhidgetHandle)servo);
CPhidget_delete((CPhidgetHandle)servo);
//all done, exit
return 0;
}
/*!
* \brief
* コンタクトポジションへ移動
*
*/
void Servo::SetContact()
{
CPhidgetAdvancedServo_setPosition (*servo, index, contactPosition);
}
/*!
* \brief
* レディポジションへ移動
*
*/
void Servo::SetReady()
{
CPhidgetAdvancedServo_setPosition (*servo, index, readyPosition);
}
/*!
* \brief
* ホームポジションへ移動
*
* \remarks
* 専用動作命令があるわけではないので注意
*
*/
void Servo::SetHome()
{
CPhidgetAdvancedServo_setPosition (*servo, index, homePosition);
curr = 0;
}
Controller::Controller()
: motoControl(0)
, speed(40)
, speedLeftFactor(1.5)
, speedRightFactor(1.5)
, accelLeftFactor(1.5)
, accelRightFactor(1.5)
, rotationOnSpotSpeed(100)
, accel(15)
, backwardTurnFastFactor(1.5)
, backwardTurnSlowFactor(-1.5)
, servo(0)
, servoOpen(35.00)
, servoClosed(150.00)
/*, speed(100)
, speedLeftFactor(1.0)
, speedRightFactor(1.0)
, accelLeftFactor(1.0)
, accelRightFactor(1.0)
, rotationOnSpotSpeed(40)
, accel(100)
, backwardTurnFastFactor(1)
, backwardTurnSlowFactor(-1)*/
{
//create the motor control object
CPhidgetMotorControl_create(&motoControl);
//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL);
ifKit = 0;
//create the InterfaceKit object
CPhidgetInterfaceKit_create(&ifKit);
//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);
//open the interfacekit for device connections
CPhidget_open((CPhidgetHandle)ifKit, -1);
CPhidgetAdvancedServo_create(&servo);
CPhidget_open((CPhidgetHandle)servo, -1);
int result;
const char *err;
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000)))
{
CPhidget_getErrorDescription(result, &err);
}
//Registers a callback that will run if an input changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, InputChangeHandler, NULL);
//Registers a callback that will run if a motor changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, VelocityChangeHandler, NULL);
//Registers a callback that will run if the current draw changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, CurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motoControl, -1);
//get the program to wait for a motor control device to be attached
CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000);
CPhidgetAdvancedServo_setEngaged(servo, 0, 1);
CPhidgetAdvancedServo_setPosition (servo, 0, 40.00);
}
void Controller::closeServo()
{
CPhidgetAdvancedServo_setPosition (servo, 0, servoClosed);
}
void Controller::openServo()
{
CPhidgetAdvancedServo_setPosition (servo, 0, servoOpen);
}
