void unsetOnChangeHandler(CPhidgetInterfaceKitHandle ifk, onChangeType t) {
switch (t) {
case inputChanged:
CPhidgetInterfaceKit_set_OnInputChange_Handler(ifk, NULL, NULL);
case outputChanged:
CPhidgetInterfaceKit_set_OnOutputChange_Handler(ifk, NULL, NULL);
case sensorChanged:
CPhidgetInterfaceKit_set_OnSensorChange_Handler(ifk, NULL, NULL);
}
}
int setOnChangeHandler(CPhidgetInterfaceKitHandle ifk, handler *h, onChangeType t) {
switch (t) {
case inputChanged:
return CPhidgetInterfaceKit_set_OnInputChange_Handler(ifk, &onChangeHandler, h);
case outputChanged:
return CPhidgetInterfaceKit_set_OnOutputChange_Handler(ifk, &onChangeHandler, h);
case sensorChanged:
return CPhidgetInterfaceKit_set_OnSensorChange_Handler(ifk, &onChangeHandler, h);
}
}
bool attach(
CPhidgetInterfaceKitHandle &phid,
int serial_number)
{
CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, "/home/motters/Desktop/phidgets.log");
//create the object
CPhidgetInterfaceKit_create(&phid);
//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)phid, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)phid, ErrorHandler, NULL);
//Registers a callback that will run if an input changes.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnInputChange_Handler (phid, InputChangeHandler, NULL);
//Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger.
//Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnSensorChange_Handler (phid, SensorChangeHandler, NULL);
//Registers a callback that will run if an output changes.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnOutputChange_Handler (phid, OutputChangeHandler, NULL);
//open the device for connections
CPhidget_open((CPhidgetHandle)phid, serial_number);
//get the program to wait for an interface kit device to be attached
if (serial_number == -1) {
ROS_INFO("Waiting for Interface Kit Phidget to be attached....");
}
else {
ROS_INFO("Waiting for Interface Kit Phidget %d to be attached....", serial_number);
}
int result;
if((result = CPhidget_waitForAttachment((CPhidgetHandle)phid, 10000)))
{
const char *err;
CPhidget_getErrorDescription(result, &err);
ROS_ERROR("Problem waiting for attachment: %s", err);
return false;
}
else return true;
}
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;
}
int interfacekit_simple()
{
int result, numSensors, i;
const char *err;
//Declare an InterfaceKit handle
CPhidgetInterfaceKitHandle 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);
//Registers a callback that will run if an input changes.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);
//Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger.
//Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL);
//Registers a callback that will run if an output changes.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL);
//open the interfacekit for device connections
CPhidget_open((CPhidgetHandle)ifKit, -1);
//get the program to wait for an interface kit device to be attached
printf("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
display_properties(ifKit);
//read interface kit event data
printf("Reading.....\n");
//keep displaying interface kit data until user input is read
printf("Press any key to go to next step\n");
getchar();
printf("Modifying sensor sensitivity triggers....\n");
//get the number of sensors available
CPhidgetInterfaceKit_getSensorCount(ifKit, &numSensors);
//Change the sensitivity trigger of the sensors
for(i = 0; i < numSensors; i++)
{
CPhidgetInterfaceKit_setSensorChangeTrigger(ifKit, i, 100); //we'll just use 10 for fun
}
//read interface kit event data
printf("Reading.....\n");
//keep displaying interface kit data until user input is read
printf("Press any key to go to next step\n");
getchar();
printf("Toggling Ratiometric....\n");
CPhidgetInterfaceKit_setRatiometric(ifKit, 0);
//read interface kit event data
printf("Reading.....\n");
//keep displaying interface kit data until user input is read
printf("Press any key to end\n");
getchar();
//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
printf("Closing...\n");
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
//all done, exit
return 0;
}
int test_interfacekit()
{
int numInputs, numOutputs, numSensors;
int err;
CPhidgetInterfaceKitHandle IFK = 0;
CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL);
CPhidgetInterfaceKit_create(&IFK);
CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnOutputChange_Handler(IFK, IFK_OutputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)IFK, IFK_AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)IFK, IFK_DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)IFK, IFK_ErrorHandler, NULL);
CPhidget_open((CPhidgetHandle)IFK, -1);
//wait 5 seconds for attachment
if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 0)) != EPHIDGET_OK )
{
const char *errStr;
CPhidget_getErrorDescription(err, &errStr);
printf("Error waiting for attachment: (%d): %s\n",err,errStr);
goto exit;
}
display_generic_properties((CPhidgetHandle)IFK);
CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle)IFK, &numOutputs);
CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle)IFK, &numInputs);
CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle)IFK, &numSensors);
CPhidgetInterfaceKit_setOutputState((CPhidgetInterfaceKitHandle)IFK, 0, 1);
printf("Sensors:%d Inputs:%d Outputs:%d\n", numSensors, numInputs, numOutputs);
//err = CPhidget_setDeviceLabel((CPhidgetHandle)IFK, "test");
while(1)
{
sleep(1);
}
while(1)
{
CPhidgetInterfaceKit_setOutputState(IFK, 7, 1);
CPhidgetInterfaceKit_setOutputState(IFK, 7, 0);
}
CPhidgetInterfaceKit_setOutputState(IFK, 0, 1);
sleep(1);
CPhidgetInterfaceKit_setOutputState(IFK, 0, 0);
sleep(1);
CPhidgetInterfaceKit_setOutputState(IFK, 0, 1);
sleep(1);
CPhidgetInterfaceKit_setOutputState(IFK, 0, 0);
sleep(5);
exit:
CPhidget_close((CPhidgetHandle)IFK);
CPhidget_delete((CPhidgetHandle)IFK);
return 0;
}
int interfacekit_simple()
{
int result, numSensors, i;
const char *err;
//Declare an InterfaceKit handle
CPhidgetInterfaceKitHandle 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);
//Registers a callback that will run if an input changes.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);
//Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger.
//Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL);
//Registers a callback that will run if an output changes.
//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL);
//open the interfacekit for device connections
CPhidget_open((CPhidgetHandle)ifKit, -1);
//get the program to wait for an interface kit device to be attached
printf("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;
}
int count = 0;
bool run = true;
power_button_reset();
while (run)
{
printf("Hello\n");
int lw = whleft.state();
int rw = whright.state();
int he = hall.state();
int sn = sonar.state();
if(power_button_get_value() >= 2){
power_button_reset();
}
if(he == 1 && motor.moveState() == 1){
count +=1;
}
if(he == 0){
count = 0;
}
if(power_button_get_value() < 1){
motor.stop();
count = 0;
}else if (sn == 1) {
motor.right();
}else if(lw == 1) {
motor.right();
}else if(rw == 1) {
motor.left();
}else if(he == 1 && motor.moveState() == 1 && count>5) {
motor.reverse();
sleep(5);
motor.right();
sleep(3);
count =0;
}else{
motor.forwards();
}
printf("Left Whisker: %d, Right Whisker: %d, Hall Effect: %d, Sonar: %d\n", lw, rw, he, sn);
sleep(1);
}
//Display the properties of the attached interface kit device
display_properties(ifKit);
//read interface kit event data
/*printf("Reading.....\n");
//keep displaying interface kit data until user input is read
printf("Press any key to go to next step\n");
getchar();
printf("Modifying sensor sensitivity triggers....\n");
//get the number of sensors available
CPhidgetInterfaceKit_getSensorCount(ifKit, &numSensors);
//Change the sensitivity trigger of the sensors
for(i = 0; i < numSensors; i++)
{
CPhidgetInterfaceKit_setSensorChangeTrigger(ifKit, i, 100); //we'll just use 10 for fun
}
//read interface kit event data
printf("Reading.....\n");
//keep displaying interface kit data until user input is read
printf("Press any key to go to next step\n");
getchar();
printf("Toggling Ratiometric....\n");
CPhidgetInterfaceKit_setRatiometric(ifKit, 0);
//read interface kit event data
printf("Reading.....\n");*/
//keep displaying interface kit data until user input is read
printf("Press any key to end\n");
getchar();
//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
printf("Closing...\n");
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
//all done, exit
return 0;
}
int interfacekit_simple()
{
int result, num_analog_inputs, num_digital_inputs;
const char *err;
//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_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL);
//For phidget spatial
//CPhidgetSpatialHandle spatial = 0;
CPhidgetSpatial_create(&spatial);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)spatial, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)spatial, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);
//open the interfacekit for device connections
CPhidget_open((CPhidgetHandle)ifKit, -1);
CPhidget_open((CPhidgetHandle)spatial, -1);
CPhidgetInterfaceKit_getInputCount(ifKit, &num_digital_inputs);
CPhidgetInterfaceKit_getSensorCount(ifKit, &num_analog_inputs);
printf("Waiting for spatial to be attached.... \n");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 1000)))
{
CPhidget_getErrorDescription(result, &err);
ROS_ERROR("Phidget Spatial: Problem waiting for attachment: %s\n", err);
spatialError = 1;
}
printf("Waiting for interface kit to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 1000)))
{
CPhidget_getErrorDescription(result, &err);
ROS_ERROR("Phidget IK: Problem waiting for attachment: %s\n", err);
phidget888_connected = false;
interfaceKitError = 1;
}
phidget888_connected = true;
CPhidgetInterfaceKit_setRatiometric(ifKit, 0);//
CPhidgetSpatial_setDataRate(spatial, 16);
CPhidgetEncoder_create(&m_leftEncoder);
CPhidget_set_OnAttach_Handler((CPhidgetHandle) m_leftEncoder,LeftEncoderAttach, NULL);
CPhidget_open((CPhidgetHandle) m_leftEncoder, -1);
if (m_encoder1Seen && m_encoder2Seen)
phidget_encoder_connected = true;
else phidget_encoder_connected = false;
//Initialize the sonars, if any are present
if(sonarsPresent)
{
CPhidgetInterfaceKit_setOutputState(ifKit, bwOutput, 1);
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
ros::Duration(0.250).sleep(); // sleep for 250ms
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
ros::Duration(0.002).sleep(); // sleep for 2ms
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
ros::Duration(0.150).sleep(); // sleep for 150ms
}
return 0;
}