/*! * \brief * エンゲージ(サーボモーターに電力を供給する) * * \param flag * true : 電力供給開始 * false: 電力供給停止 * * \remarks * あまり長時間電力供給したままにしないこと. * */ void Servo::Engage(bool flag) { if(flag){ //通電せよ CPhidgetAdvancedServo_setEngaged(*servo, index, 1); engaged = true; }else{ //電流カットせよ CPhidgetAdvancedServo_setEngaged(*servo, index, 0); engaged = false; } }
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 ); } }
Controller::~Controller() { CPhidget_close((CPhidgetHandle)motoControl); CPhidget_delete((CPhidgetHandle)motoControl); CPhidgetAdvancedServo_setEngaged(servo, 0, 0); CPhidget_close((CPhidgetHandle)servo); CPhidget_delete((CPhidgetHandle)servo); }
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 ); } }
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; }
int setup() { init_debugging(); int result, numSensors, i; const char *err; //handles *Handles; //Declare an InterfaceKit handle // Setup the IFKit CPhidgetInterfaceKit_create(&ifKit); 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, IKInputChangeHandler, NULL); CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, IKSensorChangeHandler, NULL); CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, IKOutputChangeHandler, NULL); CPhidget_open((CPhidgetHandle)ifKit, -1); //get the program to wait for an interface kit device to be attached SetupLog("Waiting for interface kit to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached interface kit device IKDisplayProperties(ifKit); // Setup motoControl CPhidgetMotorControl_create(&motoControl); CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL); CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, MCInputChangeHandler, NULL); CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, MCVelocityChangeHandler, NULL); CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, MCCurrentChangeHandler, NULL); CPhidget_open((CPhidgetHandle)motoControl, -1); SetupLog("Waiting for MotorControl to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } MCDisplayProperties(motoControl); CPhidgetMotorControl_setAcceleration (motoControl, 0, 50.00); CPhidgetMotorControl_setAcceleration (motoControl, 1, 50.00); // Setup AdvancedServo CPhidgetAdvancedServo_create(&servo); CPhidget_set_OnAttach_Handler((CPhidgetHandle)servo, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)servo, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL); CPhidgetAdvancedServo_set_OnPositionChange_Handler(servo, ASPositionChangeHandler, NULL); CPhidget_open((CPhidgetHandle)servo, -1); SetupLog("Waiting for Phidget to be attached...."); if((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 10000))) { CPhidget_getErrorDescription(result, &err); printf("Problem waiting for attachment: %s\n", err); return 0; } //Display the properties of the attached device ASDisplayProperties(servo); CPhidgetAdvancedServo_setEngaged(servo, 0, 1); state.ServoPosition = 0; sensor.RightWhisker = 0; sensor.LeftWhisker = 0; sensor.FrontFacingIR = 0; sensor.TopIR = 0; state.AverageBaseLight = (float)10000; sensor.TopLeftLight = 0; sensor.TopRightLight = 0; state.flashWasOn = 0; state.wasOnBlackInLastIteration = 0; sensor.SpinSensor = 10.0; state.expectedMovement = None; state.expectedFor = 0; state.exitTrialCounter = 0; state.stuckCounter = 0; state.previousState = 2; gettimeofday(&state.lastFlashSighted, NULL); //#ifdef FREQUENCY //state.frequency = FREQUENCY; //#endif #ifndef NO_POWERLIB power_button_reset(); while(power_button_get_value()==0) { sleep(1); } #endif return 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 shutdownStuff() { CPhidgetAdvancedServo_setEngaged( servo, 0, 0 ); CPhidget_close( (CPhidgetHandle)servo ); CPhidget_delete( (CPhidgetHandle)servo ); }