std::string Phidget::getErrorDescription(int errorCode)
{
char a[1000];
const char * errorPtr = a;
CPhidget_getErrorDescription(errorCode, &errorPtr);
return std::string(errorPtr);
}
int Create_Text_LCD()
/*-----------------
creation du handler pour le display du kit LCD
------------------*/
{
int err2;
const char *errStr;
CPhidgetTextLCD_create(&txt_lcd);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)txt_lcd,
LCD_AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)txt_lcd,
LCD_DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)txt_lcd,
LCD_ErrorHandler, NULL);
//open the TextLCD for device connections
CPhidget_open((CPhidgetHandle)txt_lcd,lcd_display_number);
//get the program to wait for an TextLCD device to be attached
if((err2 = CPhidget_waitForAttachment((CPhidgetHandle)txt_lcd,
10000)))
{
CPhidget_getErrorDescription(err2, &errStr);
printf("Problem waiting for attachment Texte LCD: %s\n", errStr);
return 0;
}
return 1;
}
int Create_KIT2()
/*-----------------
creation du handler pour le kit interface 2
------------------*/
{
int err1;
const char *errStr;
CPhidgetInterfaceKit_create(&IFK2);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK2,
IFK2_SensorChangeHandler,
NULL);
CPhidget_open((CPhidgetHandle)IFK2,kit_number2);
//wait 5 seconds for attachment
if((err1 = CPhidget_waitForAttachment((CPhidgetHandle)IFK2, 5000))
!= EPHIDGET_OK)
{
CPhidget_getErrorDescription(err1, &errStr);
printf("Error waiting for attachment IFK2: (%d): %s\n",err1,errStr);
return 0;
}
CPhidgetInterfaceKit_setSensorChangeTrigger(
(CPhidgetInterfaceKitHandle)IFK2,
IR_sensor_rear_left, 0);
CPhidgetInterfaceKit_setSensorChangeTrigger(
(CPhidgetInterfaceKitHandle)IFK2,
IR_sensor_rear_right, 0);
return 1;
}
int Create_KIT1()
/*-----------------
creation du handler pour le kit interface 1
------------------*/
{
int err;
const char *errStr;
CPhidgetInterfaceKit_create(&IFK);
CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler,
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);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK,IFK_SensorChangeHandler,
NULL);
//ouverture du kit interface
CPhidget_open((CPhidgetHandle)IFK,kit_number);
//wait 5 seconds for attachment
if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 5000))
!= EPHIDGET_OK)
{
CPhidget_getErrorDescription(err, &errStr);
printf("Error waiting for attachment IFK1: (%d): %s\n",err,errStr);
return 0;
}
//pour faire marcher les boutons stop et inhibit
CPhidgetInterfaceKit_setOutputState(IFK,true_inhibit_number, 1);
return 1;
}
void ioPhidget::connect(int timeout = 10000){
const char *err;
int result;
// this starts reading in data.
//sets a bridge data event handler called by rate set by 'datarate'
CPhidgetBridge_set_OnBridgeData_Handler(bridge, &data, &dat);
// open bridge for device connections
CPhidget_open((CPhidgetHandle)bridge, -1);
//Wait for 10 seconds, otherwise exit
printf("Waiting for attachment...");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)bridge, timeout)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return;
}
display_generic_properties((CPhidgetHandle)bridge);
// return if the device is attached or not (int)
CPhidget_getDeviceStatus((CPhidgetHandle)bridge, &isAttached);
return;
}
int main(int argc, char *argv[])
{
int result;
CPhidgetSBCManagerHandle sbcm;
CPhidgetSBCManager_create(&sbcm);
CPhidgetSBCManager_set_OnAttach_Handler(sbcm, attach, NULL);
CPhidgetSBCManager_set_OnDetach_Handler(sbcm, detach, NULL);
CPhidgetSBCManager_set_OnError_Handler(sbcm, error, NULL);
result = CPhidgetSBCManager_start(sbcm);
if(result != EPHIDGET_OK)
{
const char *err;
CPhidget_getErrorDescription(result, &err);
printf("Error: (%d) %s\n", result, err);
return 1;
}
printf("Press any key to end\n");
getchar();
printf("Closing...\n");
CPhidgetSBCManager_stop(sbcm);
CPhidgetSBCManager_delete(sbcm);
return 0;
}
bool attach(
CPhidgetPHSensorHandle &phid,
int serial_number)
{
int result;
const char *err;
//create the PH Sensor object
CPhidgetPHSensor_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 the PH changes by more than the PH trigger.
//Requires the handle for the PHSensor, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetPHSensor_set_OnPHChange_Handler(phid, PHChangeHandler, NULL);
//open the PH Sensor for device connections
CPhidget_open((CPhidgetHandle)phid, -1);
//get the program to wait for an PH Sensor device to be attached
ROS_INFO("Waiting for PH Sensor to be attached....");
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)phid, 10000))) {
CPhidget_getErrorDescription(result, &err);
ROS_INFO("Problem waiting for PH Sensor attachment: %s\n", err);
return false;
}
else return true;
}
// connect to a device. Use -1 for first available device
void PhidgetConnector::connect(int serial_in, int _timeOut) {
cout << "attempting connection to board #" << serial_in << "... " ;
CPhidgetInterfaceKitHandle * thisIFKIT = new CPhidgetInterfaceKitHandle();
ifkits.push_back(thisIFKIT);
CPhidgetInterfaceKit_create(thisIFKIT);
if(bUseEvents) {
cout << "using events..." << endl;
CPhidget_set_OnError_Handler((CPhidgetHandle)*thisIFKIT, ErrorHandler, this);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(*thisIFKIT, SensorChangeHandler, this);
} else {
cout << "not using events..." << endl;
}
CPhidget_open((CPhidgetHandle)*thisIFKIT, serial_in);
printf("Waiting for interface kit to be attached....");
int result;
const char *err;
int timeOut = 30000;
if(_timeOut>0) timeOut = _timeOut * 1000;
if((result = CPhidget_waitForAttachment((CPhidgetHandle)*thisIFKIT, timeOut))) {
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment %s\n", err);
} else {
printf("sensor opened okay.");
display_properties(*thisIFKIT);
}
}
void PhidgetHelper::Initialize()
{
CPhidgetSpatial_create(&handle);
std::cout << "Handle: " << handle << std::endl;
CPhidgetSpatial_set_OnSpatialData_Handler(handle, PhidgetHelper::SpatialDataHandler, NULL);
std::cout << "Initialized spatial callback" << std::endl;
CPhidget_open((CPhidgetHandle)handle, -1);
std::cout << "Handle opened" << std::endl;
int result = 0;
if(result = CPhidget_waitForAttachment((CPhidgetHandle)handle, 1500))
{
const char* err;
CPhidget_getErrorDescription(result, &err);
std::cerr << "PHIDGET ERROR: " << err << std::endl;
raise(SIGTERM);
return;
}
std::cout << "Attached" << std::endl;
// double mag[3];
// CPhidgetSpatial_getMagneticField(handle, 0, &mag[0]);
// CPhidgetSpatial_getMagneticField(handle, 1, &mag[1]);
// CPhidgetSpatial_getMagneticField(handle, 2, &mag[2]);
// std::cout << "Magnetic Field " << "x: " << mag[0] << "; y: " << mag[1] << "; z: " << mag[2] << std::endl;
// double mag_angle = atan(mag[0] / mag[1]);
CPhidgetSpatial_setDataRate(handle, 16);
std::cout << "Datarate set" << std::endl;
}
CPhidgetInterfaceKitHandle InitializeTempSensors(int n_tempsensors, int* sensor_channels, int rate_tempsensors) {
int result;
const char *err;
/* If ifKit is already defined, return. */
if (ifKit != NULL) {
return ifKit;
}
/* 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);
/* Open the interfacekit for device connections */
CPhidget_open((CPhidgetHandle)ifKit, -1);
LOG(logINFO) << "Waiting for interface kit to be attached...";
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 5000))) {
CPhidget_getErrorDescription(result, &err);
LOG(logERROR) << "Problem waiting for attachment:" << err;
return 0;
}
/* Set the data input rates of the interface kit potentiometers */
for (int i = 0; i < n_tempsensors; i++) {
CPhidgetInterfaceKit_setDataRate(ifKit, sensor_channels[i], rate_tempsensors);
}
return ifKit;
}
void ImuRosI::initDevice()
{
ROS_INFO("Opening device");
open(-1);
ROS_INFO("Waiting for IMU to be attached...");
int result = waitForAttachment(10000);
if(result)
{
is_connected_ = false;
error_number_ = result;
diag_updater_.force_update();
const char *err;
CPhidget_getErrorDescription(result, &err);
ROS_FATAL("Problem waiting for IMU attachment: %s Make sure the USB cable is connected and you have executed the phidgets_api/share/setup-udev.sh script.", err);
}
// calibrate on startup
calibrate();
// set the hardware id for diagnostics
diag_updater_.setHardwareIDf("%s-%d",
getDeviceName().c_str(),
getDeviceSerialNumber());
}
int encoder_simple()
{
int result;
const char *err;
file=fopen("/home/dune/dataSaved/essai1/encoder","w");
//Declare an encoder handle
CPhidgetEncoderHandle encoder = 0;
//create the encoder object
CPhidgetEncoder_create(&encoder);
//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)encoder, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)encoder, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)encoder, 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).
CPhidgetEncoder_set_OnInputChange_Handler(encoder, InputChangeHandler, NULL);
//Registers a callback that will run if the encoder changes.
//Requires the handle for the Encoder, the function that will be called,
//and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetEncoder_set_OnPositionChange_Handler (encoder, PositionChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)encoder, -1);
struct timeval now ;
//get the program to wait for an encoder device to be attached
printf("Waiting for encoder to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)encoder, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
gettimeofday(&now,NULL);
encTime0 = now.tv_sec + ((double)now.tv_usec)/1000000.0;
encTime1 = encTime0 ;
//Display the properties of the attached encoder device
display_properties(encoder);
//read encoder event data
printf("Reading.....\n");
//keep displaying encoder 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)encoder);
CPhidget_delete((CPhidgetHandle)encoder);
fclose(file);
//all done, exit
return 0;
}
int main(int argc, char* argv[]) {
// Set up a SIGINT handler.
signal(SIGINT, catch_sigint);
// Create the InterfaceKit object.
CPhidgetInterfaceKitHandle ifKit = 0;
CPhidgetInterfaceKit_create(&ifKit);
// Register device handlers.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, attach_handler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, detach_handler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, error_handler, NULL);
// Open the interfacekit for device connections.
CPhidget_open((CPhidgetHandle)ifKit, -1);
// Wait for a device attachment.
fprintf(stderr, "waiting for interface kit to be attached....\n");
int result;
const char *err;
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000))) {
CPhidget_getErrorDescription(result, &err);
fprintf(stderr, "problem waiting for attachment: %s\n", err);
return 0;
}
// Check some properties of the device.
const char *device_type;
int num_outputs;
CPhidget_getDeviceType((CPhidgetHandle)ifKit, &device_type);
CPhidgetInterfaceKit_getOutputCount(ifKit, &num_outputs);
if ((strcmp(device_type, "PhidgetInterfaceKit") != 0)) {
fprintf(stderr, "unexpected device type: %s\n", device_type);
return 1;
}
if (num_outputs != 4) {
fprintf(stderr, "unexpected number of device outputs: %d\n", num_outputs);
return 1;
}
while (1) {
double time = get_time_in_seconds();
double factor = 1;
int l0_state = fmod(time * factor, 20) < 10;
int l1_state = fmod(time * factor, 20) >= 10;
CPhidgetInterfaceKit_setOutputState(ifKit, 0, l0_state);
CPhidgetInterfaceKit_setOutputState(ifKit, 2, l1_state);
usleep(100000);
}
fprintf(stderr, "shutting down...\n");
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
return 0;
}
int tempsensor_simple()
{
int result;
const char *err;
//Declare an temperature sensor handle
CPhidgetTemperatureSensorHandle temp = 0;
//create the temperature sensor object
CPhidgetTemperatureSensor_create(&temp);
//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)temp, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)temp, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)temp, ErrorHandler, NULL);
//Registers a callback that will run if the Temperature changes by more than the Temperature trigger.
//Requires the handle for the Temperature Sensor, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetTemperatureSensor_set_OnTemperatureChange_Handler(temp, TemperatureChangeHandler, NULL);
//open the temperature sensor for device connections
CPhidget_open((CPhidgetHandle)temp, -1);
//get the program to wait for an temperature sensor device to be attached
printf("Waiting for TemperatureSensor to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)temp, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached accelerometer device
display_properties(temp);
//read temperature sensor event data
printf("Reading.....\n");
//keep displaying temperature sensor event data until user input is read
//modify the sensor sensitivity, index 1 is the thermocouple sensor, index 0 is the onboard or ambient sensor
printf("Setting sensitivity of the thermocouple to 2.00. Press any key to continue\n");
getchar();
CPhidgetTemperatureSensor_setTemperatureChangeTrigger (temp, 1, 2.00);
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)temp);
CPhidget_delete((CPhidgetHandle)temp);
//all done, exit
return 0;
}
int ph_simple()
{
int result;
const char *err;
//Declare an PH Sensor handle
CPhidgetPHSensorHandle ph = 0;
//create the PH Sensor object
CPhidgetPHSensor_create(&ph);
//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)ph, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ph, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ph, ErrorHandler, NULL);
//Registers a callback that will run if the PH changes by more than the PH trigger.
//Requires the handle for the PHSensor, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetPHSensor_set_OnPHChange_Handler(ph, PHChangeHandler, NULL);
//open the PH Sensor for device connections
CPhidget_open((CPhidgetHandle)ph, -1);
//get the program to wait for an PH Sensor device to be attached
printf("Waiting for PH Sensor to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ph, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached textlcd device
display_properties(ph);
//read led event data
printf("Reading.....\n");
//increase the sensitivity
printf("Increasing sensitivity to 10.00, Press any key to continue\n");
getchar();
CPhidgetPHSensor_setPHChangeTrigger (ph, 10.00);
//end
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)ph);
CPhidget_delete((CPhidgetHandle)ph);
//all done, exit
return 0;
}
// The name is ambiguous, but the main purpose here is dry things out a bit
// and let us do a better job of reporting errors albeit not halting.
// This is necessary in the threaded callbacks where access to the interpreter
// is unavailable:
int report(int result) {
if (result == EPHIDGET_OK) return result;
const char *description;
CPhidget_getErrorDescription(result, &description);
CPhidget_log(PHIDGET_LOG_ERROR, "N/A", "PhidgetNative report error: %s", description);
return result;
}
int spatial_simple()
{
int result;
const char *err;
//Declare a spatial handle
CPhidgetSpatialHandle spatial = 0;
//create the spatial object
CPhidgetSpatial_create(&spatial);
//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)spatial, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)spatial, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
//Registers a callback that will run according to the set data rate that will return the spatial data changes
//Requires the handle for the Spatial, the callback handler function that will be called,
//and an arbitrary pointer that will be supplied to the callback function (may be NULL)
CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);
//open the spatial object for device connections
CPhidget_open((CPhidgetHandle)spatial, -1);
//get the program to wait for a spatial device to be attached
printf("Waiting for spatial to be attached.... \n");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached spatial device
display_properties((CPhidgetHandle)spatial);
//read spatial event data
printf("Reading.....\n");
//Set the data rate for the spatial events
CPhidgetSpatial_setDataRate(spatial, 16);
//run 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)spatial);
CPhidget_delete((CPhidgetHandle)spatial);
return 0;
}
int MotorController::regHanlders(void)
{
printf("MotorController::regHandlers\n");
#if (!DEBUG_MODE_PC)
int result;
const char *err;
#endif
//Declare a motor control handle
motorCtrlHandle = 0;
//create the motor control object
CPhidgetMotorControl_create(&motorCtrlHandle);
//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)motorCtrlHandle, MotorAttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motorCtrlHandle, MotorDetachHandler, NULL);
CPhidget_set_OnError_Handler( (CPhidgetHandle)motorCtrlHandle, MotorErrorHandler, NULL);
//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(motorCtrlHandle, MotorInputChangeHandler, 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(motorCtrlHandle, MotorVelocityChangeHandler, 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(motorCtrlHandle, MotorCurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motorCtrlHandle, -1);
//get the program to wait for a motor control device to be attached
printf("Waiting for MotorControl to be attached....\n");
#if (!DEBUG_MODE_PC)
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)motorCtrlHandle, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
#endif
//Display the properties of the attached motor control device
MotorDisplayProperties(motorCtrlHandle);
//all done, exit
return 0;
}
bool attach(CPhidgetSpatialHandle &phid,
int serial_number,
int data_rate)
{
int result;
const char *err;
//create the spatial object
CPhidgetSpatial_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 according to the
// set data rate that will return the spatial data changes
// Requires the handle for the Spatial, the callback
// handler function that will be called,
// and an arbitrary pointer that will be supplied to
// the callback function (may be NULL)
CPhidgetSpatial_set_OnSpatialData_Handler(phid,
SpatialDataHandler,
NULL);
// open the spatial object for device connections
CPhidget_open((CPhidgetHandle)phid, -1);
// get the program to wait for a spatial device
// to be attached
ROS_INFO("Waiting for spatial to be attached.... \n");
if ((result =
CPhidget_waitForAttachment((CPhidgetHandle)phid,
10000))) {
CPhidget_getErrorDescription(result, &err);
ROS_INFO("Problem waiting for attachment: %s\n", err);
return 0;
}
else {
//Set the data rate for the spatial events
CPhidgetSpatial_setDataRate(phid, data_rate);
return true;
}
}
bool qPhidgetRFID::checkIfDeviceConnected()
{
int result=0;
const char *err;
if((result = CPhidget_waitForAttachment((CPhidgetHandle)rfidHandle, 10000)))
{
CPhidget_getErrorDescription(result, &err);
qDebug()<<err;
return false;
}
CPhidgetRFID_setAntennaOn(rfidHandle,1);
return true;
}
CPhidgetAdvancedServoHandle InitializeServos(int n, int* channels, int grip_channel, double* AccelThrottle, double* VelLimThrottle, double grip_AccelThrottle, double grip_VelLimThrottle) {
int result;
const char *err;
double minAccel, maxAccel, minVel, maxVel;
/* If servo is already defined, return. */
if (servo != NULL) {
return servo;
}
/* Create the advanced servo object */
CPhidgetAdvancedServo_create(&servo);
/* Set the handlers to be run when the device is plugged in (opened) from software, unplugged (closed) from software, or generates an error. */
CPhidget_set_OnAttach_Handler((CPhidgetHandle)servo, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)servo, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL);
/* Open the device for connections */
CPhidget_open((CPhidgetHandle)servo, -1);
LOG(logINFO) << "Waiting for servos to be attached...";
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 5000))) {
CPhidget_getErrorDescription(result, &err);
LOG(logERROR) << "Problem waiting for attachment: "<< err;
return 0;
}
/* Set the acceleration and velocity limits of each joint angle actuator */
for (int i = 0; i < n; i++) {
CPhidgetAdvancedServo_getAccelerationMin(servo, channels[i], &minAccel);
CPhidgetAdvancedServo_getAccelerationMax(servo, channels[i], &maxAccel);
CPhidgetAdvancedServo_setAcceleration(servo, channels[i], AccelThrottle[i]*(maxAccel - minAccel) + minAccel );
CPhidgetAdvancedServo_getVelocityMin(servo, channels[i], &minVel);
CPhidgetAdvancedServo_getVelocityMax(servo, channels[i], &maxVel);
CPhidgetAdvancedServo_setVelocityLimit(servo, channels[i], VelLimThrottle[i]*(maxVel - minVel) + minVel );
}
/* Set the acceleration and velocity limits of the end effector */
CPhidgetAdvancedServo_getAccelerationMin(servo, grip_channel, &minAccel);
CPhidgetAdvancedServo_getAccelerationMax(servo, grip_channel, &maxAccel);
CPhidgetAdvancedServo_setAcceleration(servo, grip_channel, grip_AccelThrottle*(maxAccel - minAccel) + minAccel );
CPhidgetAdvancedServo_getVelocityMin(servo, grip_channel, &minVel);
CPhidgetAdvancedServo_getVelocityMax(servo, grip_channel, &maxVel);
CPhidgetAdvancedServo_setVelocityLimit(servo, grip_channel, grip_VelLimThrottle*(maxVel - minVel) + minVel );
return (CPhidgetAdvancedServoHandle)servo;
}
int Gps::initDevice()
{
std::cout<<"Opening device"<<std::endl;
this->open(serial_number);
std::cout<<"Waiting for GPS to be attached..."<<std::endl;
int result = waitForAttachment(10000);
if(result)
{
const char *err;
CPhidget_getErrorDescription(result, &err);
std::cout<<"Problem waiting for GPS attachment: %s Make sure the USB cable is connected and you have executed the phidgets_c_api/setup-udev.sh script."<<std::endl;
return 1;
}
return 0;
}
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;
}
bool attach(
CPhidgetRFIDHandle &phid,
int serial_number)
{
int result;
const char *err;
//Declare an RFID handle
CPhidgetRFIDHandle rfid = 0;
//create the RFID object
CPhidgetRFID_create(&rfid);
//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)rfid, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)rfid, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)rfid, ErrorHandler, 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).
CPhidgetRFID_set_OnOutputChange_Handler(rfid, OutputChangeHandler, NULL);
//Registers a callback that will run when a Tag is read.
//Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetRFID_set_OnTag_Handler(rfid, TagHandler, NULL);
//Registers a callback that will run when a Tag is lost (removed from antenna read range).
//Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetRFID_set_OnTagLost_Handler(rfid, TagLostHandler, NULL);
//open the RFID for device connections
CPhidget_open((CPhidgetHandle)rfid, -1);
//get the program to wait for an RFID device to be attached
ROS_INFO("Waiting for RFID sensor to be attached....");
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)rfid, 10000))) {
CPhidget_getErrorDescription(result, &err);
ROS_INFO("Problem waiting for RFID sensor attachment: %s\n", err);
return false;
}
else return true;
}
// This error handler can handle any CPhidget function that returns an int
int LocalErrorCatcher (int errorCode) {
const char *errorDescription;
// If the error code is 0, everything is okay
if (errorCode != 0) {
// Otherwise, you can print specific messages or perform actions by error value.
switch (errorCode) {
default:
printf("Error: An error occurred with code %d.\n", errorCode);
LocalErrorCatcher(
CPhidget_getErrorDescription (errorCode, &errorDescription));
printf("The description for this error is: %s\n", errorDescription);
break;
}
}
return 0;
}
void connect(){
spatial = 0;
int result;
const char *err;
CPhidgetSpatial_create(&spatial);
CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);
//open the spatial object for device connections
CPhidget_open((CPhidgetHandle)spatial, -1);
//get the program to wait for a spatial device to be attached
if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 10000))){
CPhidget_getErrorDescription(result, &err);
printf("IMU:Problem waiting for attachment: %s\n", err);
exit(0);
}
CPhidgetSpatial_setDataRate(spatial, 1);
}
void RFIDreader::setup()
{
#if ENABLE_RFID
if(cfg().test){
int result;
const char *err;
tagAvailable=false;
CPhidgetRFID_create(&rfid);
//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)rfid, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)rfid, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)rfid, ErrorHandler, 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).
CPhidgetRFID_set_OnOutputChange_Handler(rfid, OutputChangeHandler, NULL);
//Registers a callback that will run when a Tag is read.
//Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetRFID_set_OnTag_Handler(rfid, TagHandler, NULL);
//Registers a callback that will run when a Tag is lost (removed from antenna read range).
//Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetRFID_set_OnTagLost_Handler(rfid, TagLostHandler, NULL);
//open the RFID for device connections
CPhidget_open((CPhidgetHandle)rfid, -1);
//get the program to wait for an RFID device to be attached
printf("Waiting for RFID to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)rfid, 2000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
}
CPhidgetRFID_setAntennaOn(rfid, 1);
}
#endif
}
void test()
{
const char *err;
int result;
CPhidgetFrequencyCounterHandle freq;
//CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL);
CPhidgetFrequencyCounter_create(&freq);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)freq, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)freq, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)freq, ErrorHandler, NULL);
CPhidgetFrequencyCounter_set_OnCount_Handler(freq, Count, NULL);
CPhidget_open((CPhidgetHandle)freq, -1);
//Wait for 10 seconds, otherwise exit
if(result = CPhidget_waitForAttachment((CPhidgetHandle)freq, 10000))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return;
}
display_generic_properties((CPhidgetHandle)freq);
//Wait for enter
getchar();
printf("Closing...\n");
CPhidget_close((CPhidgetHandle)freq);
CPhidget_delete((CPhidgetHandle)freq);
return;
}
void motor_callback(const std_msgs::String::ConstPtr& msg){
ROS_INFO("Motor Move Request:");
int result;
const char *err;
//Declare a motor control handle
CPhidgetMotorControlHandle motoControl = 0;
//create the motor control object
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);
CPhidget_open((CPhidgetHandle)motoControl, -1);
printf("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;
}
CPhidgetMotorControl_setAcceleration (motoControl, 0, 50.00);
CPhidgetMotorControl_setVelocity (motoControl, 0, 100.00);
//Wait for some time...
ros::Duration(1).sleep();
CPhidgetMotorControl_setAcceleration (motoControl, 0,0);
CPhidgetMotorControl_setVelocity (motoControl, 0, 0);
printf("Closing...\n");
CPhidget_close((CPhidgetHandle)motoControl);
CPhidget_delete((CPhidgetHandle)motoControl);
}
int Create_KITLCD()
/*-----------------
creation du handler pour le kit LCD
------------------*/
{
int err2;
const char *errStr;
CPhidgetInterfaceKit_create(&IFK_LCD);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK_LCD,
IFK_LCD_SensorChangeHandler,
NULL);
CPhidgetInterfaceKit_setSensorChangeTrigger (IFK_LCD, 1, 1);
CPhidget_open((CPhidgetHandle)IFK_LCD,lcd_display_number);
//wait 5 seconds for attachment
if((err2 = CPhidget_waitForAttachment((CPhidgetHandle)IFK_LCD, 5000))
!= EPHIDGET_OK)
{
CPhidget_getErrorDescription(err2, &errStr);
printf("Error waiting for attachment kit LCD: (%d): %s\n",err2,errStr);
return 0;
}
return 1;
}
