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