Controller::~Controller()
{
CPhidget_close((CPhidgetHandle)motoControl);
CPhidget_delete((CPhidgetHandle)motoControl);
CPhidgetAdvancedServo_setEngaged(servo, 0, 0);
CPhidget_close((CPhidgetHandle)servo);
CPhidget_delete((CPhidgetHandle)servo);
}
LoadCellData::~LoadCellData(void)
{
bIsRecording = false;
kill = 1;
CPhidget_close((CPhidgetHandle)bridge1);
CPhidget_close((CPhidgetHandle)bridge2);
CPhidget_delete((CPhidgetHandle)bridge1);
CPhidget_delete((CPhidgetHandle)bridge2);
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "phidget_component");
ros::NodeHandle n;
ros::NodeHandle nh("~");
nh.param("rearBumper", m_rearBumperPresent, false);
nh.param("bwOutput", bwOutput, -1);
nh.param("strobeOutput", strobeOutput, -1);
nh.param("lastSonarInput", lastSonarInput, -1);
nh.param("firstSonarInput", firstSonarInput, -1);
//index of the battery voltage sensor
nh.param("battery", batteryPort, 0);
// index of the front ir sensor
nh.param("irFront", irFrontPort, 1);
//index of the back ir sensor
nh.param("irBack", irBackPort, 2);
nh.param("imu", imu, true);
//create an updater that will send information on the diagnostics topics
diagnostic_updater::Updater updater;
updater.setHardwareIDf("Phidget");
//function that will be executed with updater.update()
updater.add("Interface Kit", phidget_ik_diagnostic);
//function that will be executed with updater.update()
updater.add("Spatial", phidget_spatial_diagnostic);
interfacekit_simple();
ros::Rate rate(70);
while (ros::ok())
{
// ROS loop
ros::spinOnce();
// acquire new sonar data if sonar sensors are present
if(sonarsPresent)
sendSonarResult();
//update diagnostics
updater.update();
rate.sleep();
}
// close all the phidget devices
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
if (imu)
{
CPhidget_close((CPhidgetHandle)spatial);
CPhidget_delete((CPhidgetHandle)spatial);
}
return 0;
}
/*!
* \brief disconnect
*/
void disconnect(
CPhidgetSpatialHandle &phid)
{
ROS_INFO("Closing...");
CPhidget_close((CPhidgetHandle)phid);
CPhidget_delete((CPhidgetHandle)phid);
}
LCD::~LCD() {
setBacklight(0);
setText("",0);
setText("",1);
CPhidget_close((CPhidgetHandle)txt_lcd);
CPhidget_delete((CPhidgetHandle)txt_lcd);
}
/*!
* \brief disconnect the interface kit
*/
void disconnect(
CPhidgetInterfaceKitHandle &phid)
{
ROS_INFO("Closing...");
CPhidget_close((CPhidgetHandle)phid);
CPhidget_delete((CPhidgetHandle)phid);
}
int main(int argc, char* argv[]) {
int result, numOutputs = 3, state = 0;
const char *err, *ptr;
CPhidgetInterfaceKitHandle phid = 0;
LocalErrorCatcher(CPhidgetInterfaceKit_create(&phid));
LocalErrorCatcher(CPhidget_set_OnAttach_Handler((CPhidgetHandle) phid, AttachHandler, NULL));
LocalErrorCatcher(CPhidget_set_OnDetach_Handler((CPhidgetHandle) phid, DetachHandler, NULL));
LocalErrorCatcher(CPhidget_set_OnError_Handler((CPhidgetHandle) phid, ErrorHandler, NULL));
// printf("Phidget Simple Playground (plug and unplug devices)\n");
// printf("Opening...\n");
LocalErrorCatcher(CPhidget_open((CPhidgetHandle) phid, -1));
LocalErrorCatcher(CPhidget_waitForAttachment((CPhidgetHandle) phid, 100000));
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
// printf("%s\n", ptr);
printf("Turning power on\n");
CPhidgetInterfaceKit_setOutputState(phid, 0, 1);
printf("done\n");
LocalErrorCatcher(CPhidget_close((CPhidgetHandle) phid));
LocalErrorCatcher(CPhidget_delete((CPhidgetHandle) phid));
return 0;
}
// destructor
PhidgetConnector::~PhidgetConnector() {
cout << "closing Phidgets..." << endl;
for(int i=0; i<ifkits.size(); i++) {
CPhidget_close((CPhidgetHandle)*ifkits.at(i));
}
ifkits.clear();
ifKitModels.clear();
}
// destructor
phidgetIMU::~phidgetIMU()
{
if( mHandle != NULL )
{
CPhidget_close((CPhidgetHandle)mHandle);
mHandle = NULL;
}
}
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 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 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;
}
RFIDreader::~RFIDreader()
{
#if ENABLE_RFID
if(cfg().test){
CPhidget_close((CPhidgetHandle)rfid);
CPhidget_delete((CPhidgetHandle)rfid);
}
#endif
}
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;
}
JNIEXPORT void JNICALL
Java_com_phidgets_Phidget_nativeClose(JNIEnv *env, jobject obj)
{
CPhidgetHandle h = (CPhidgetHandle)(uintptr_t)(*env)->GetLongField(env,
obj, handle_fid);
int error;
if ((error = CPhidget_close(h)))
PH_THROW(error);
}
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 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;
}
// reset
void PhidgetConnector::reset() {
cout << "closing Phidgets..." << endl;
for(int i=0; i<ifkits.size(); i++) {
CPhidget_close((CPhidgetHandle)*ifkits.at(i));
}
cout << "opening them back up..." << endl;
for(int i=0; i<ifkits.size(); i++) {
// CPhidget_close((CPhidgetHandle)*ifkits.at(i));
int mySerial = ifKitModels.at(i)->getSerial();
//CPhidget_getSerialNumber((CPhidgetHandle)*ifkits.at(i), &mySerial);
cout << "whats the frequency, Kenneth? " << mySerial;
CPhidgetHandle thisIFKit = (CPhidgetHandle)ifkits.at(i);
CPhidget_open(thisIFKit, mySerial);
cout << "attempting to reopen... " << endl;
}
}
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 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 main() {
double accSum[3] = {0,0,0};
double accOffset[3] = {0,0,0};
double gyroSum[3] = {0,0,0};
double gyroOffset[3] = {0,0,0};
int events = 10000;
//Creating/Initializing Spatial Handle
//-----------------------------------
CPhidgetSpatialHandle spatial = 0;
CPhidgetSpatial_create(&spatial);
//Setting up queues
// dataQ - the Q that phidget events go to
//-----------------------------------
spatial::PhidgetRawDataQ* dataQueue = new spatial::PhidgetRawDataQ();
spatial::PhidgetRawDataQ::iterator it = dataQueue->begin();
//Initializing Mutex
//-----------------------------------
if(pthread_mutex_init(&mutex, NULL) != 0) {
printf("mutex init failed");
//erm if it doesn't work??
return 1;
}
else {
printf("mutex init success \n");
}
//Setting up Phidget
//-----------------------------------
spatial::spatial_setup(spatial, dataQueue, dataRate);
for(int i = 0; i<events; i++)
{
//Getting data from Phidget
//-----------------------------------
while(dataQueue->empty()) {}
//Copied event data is TESTED and WORKING.
pthread_mutex_lock(&mutex);
it = dataQueue->begin();
CPhidgetSpatial_SpatialEventData* newest = spatial::copy(*it);
dataQueue->pop_front();
pthread_mutex_unlock(&mutex);
if(i%100 ==0) cout << "event: " << i << endl;
for(int i =0; i < 3; i++) {
gyroSum[i]= gyroSum[i] + newest->angularRate[i];
}
for(int i =0; i< 3; i++) {
accSum[i] = accSum[i] + newest->acceleration[i];
}
}
//Finding our "zeroes"
//-----------------------------------
cout << "Writing to phidgetOffset.txt" <<endl;
fstream fout;
fout.open("phidgetOffset.txt", fstream::out);
for(int i =0; i< 3; i++) {
gyroOffset[i] = gyroSum[i]/events;
cout << "Gyro Offset axis " << i << ": " << gyroOffset[i] << endl;
fout << "Gyro Offset axis " << i << ": " << gyroOffset[i] << endl;
}
for(int i =0; i< 3; i++) {
accOffset[i] = accSum[i]/events;
cout << "Acc Offset axis " << i << ": " << accOffset[i] << endl;
fout << "Acc Offset axis " << i << ": " << accOffset[i] << endl;
}
fout.close();
//Odds and Ends.
//-----------------------------------
CPhidget_close((CPhidgetHandle)spatial);
CPhidget_delete((CPhidgetHandle)spatial);
pthread_mutex_destroy(&mutex);
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 accelerometer_simple()
{
int result, numAxes;
const char *err;
//Declare an accelerometer handle
CPhidgetAccelerometerHandle accel = 0;
//create the accelerometer object
CPhidgetAccelerometer_create(&accel);
//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)accel, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)accel, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)accel, ErrorHandler, NULL);
//Registers a callback that will run if the acceleration changes by more than the Acceleration trigger.
//Requires the handle for the Accelerometer, the function that will be called,
//and an arbitrary pointer that will be supplied to the callback function (may be NULL)
CPhidgetAccelerometer_set_OnAccelerationChange_Handler(accel, accel_AccelChangeHandler, NULL);
//open the acclerometer for device connections
CPhidget_open((CPhidgetHandle)accel, -1);
//get the program to wait for an accelerometer device to be attached
printf("Waiting for accelerometer to be attached.... \n");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)accel, 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((CPhidgetHandle)accel);
//read accelerometer event data
printf("Reading.....\n");
//get the number of available axes on the attached accelerometer
CPhidgetAccelerometer_getAxisCount(accel, &numAxes);
//Most accelerometers have 2 axes so we'll pre-set their sensitivity to make the data more readable
CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 0, 0.500);
CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 1, 0.500);
//If the accelerometer attached is a 3-axis, we'll set the sensitivity of the 3rd axis
if(numAxes > 2)
{
CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 2, 0.500);
}
//wait 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)accel);
CPhidget_delete((CPhidgetHandle)accel);
//all done, exit
return 0;
}
int motorcontrol_simple()
{
int result;
const char *err;
//Declare a motor control handle
CPhidgetMotorControlHandle motoControl = 0;
//CPhidgetMotorControlHandle motoControl2 = 1;
//create the motor control object
CPhidgetMotorControl_create(&motoControl);
//CPhidgetMotorControl_create(&motoControl2);
//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);
/* CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl2, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl2, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl2, 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 a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, InputChangeHandler, NULL);
//CPhidgetMotorControl_set_OnInputChange_Handler (motoControl2, 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);
//CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl2, 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);
//CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl2, CurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motoControl, -1);
//CPhidget_open((CPhidgetHandle)motoControl2, -1);
//get the program to wait for a motor control device to be attached
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 0;
}
/*if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl2, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment 2: %s\n", err);
return 0;
}*/
//Display the properties of the attached motor control device
display_properties(motoControl);
//display_properties(motoControl2);
//read motor control event data
printf("Reading.....\n");
//keep displaying motor control event data until user input is read
printf("Press any key to continue\n");
getchar();
//Control the motor a bit.
//Step 1: increase acceleration to 50, set target sped at 100
/*CPhidgetMotorControl_setAcceleration (motoControl, 0, -50.00);
CPhidgetMotorControl_setVelocity (motoControl, 0, -100.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 50.00);
CPhidgetMotorControl_setVelocity (motoControl, 1, 100.00);
printf("Press any key to continue\n");
getchar();*/
//Step 2: Set acceleration to 100, decrease target speed to 75
CPhidgetMotorControl_setAcceleration (motoControl, 0, -100.00);
CPhidgetMotorControl_setVelocity (motoControl, 0, -100.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 100.00);
CPhidgetMotorControl_setVelocity (motoControl, 1, 70.00);
printf("Press any key to continue\n");
getchar();
//Step 3: Stop the motor by decreasing speed to 0;
CPhidgetMotorControl_setVelocity (motoControl, 0, 0.00);
CPhidgetMotorControl_setAcceleration (motoControl, 0, 0.00);
CPhidgetMotorControl_setVelocity (motoControl, 1, 0.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 0.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)motoControl);
CPhidget_delete((CPhidgetHandle)motoControl);
/*CPhidget_close((CPhidgetHandle)motoControl2);
CPhidget_delete((CPhidgetHandle)motoControl2);*/
//all done, exit
return 0;
}
int Phidget::close()
{
cout << "Closing Phidget Device" << endl;
return(CPhidget_close(handle_));
}
void disconnect(CPhidgetMotorControlHandle &phid){
ROS_INFO("Closing...");
CPhidget_close((CPhidgetHandle)phid);
CPhidget_delete((CPhidgetHandle)phid);
}
void dissconnect(){
CPhidget_close((CPhidgetHandle)spatial);
CPhidget_delete((CPhidgetHandle)spatial);
}
int LED_simple()
{
int result, i;
const char *err;
//Declare an LED handle
CPhidgetLEDHandle led = 0;
//create the LED object
CPhidgetLED_create(&led);
//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)led, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)led, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)led, ErrorHandler, NULL);
//open the LED for device connections
CPhidget_open((CPhidgetHandle)led, -1);
//get the program to wait for an LED device to be attached
printf("Waiting for LED to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)led, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached LED device
display_properties(led);
printf("Press any key to continue\n");
getchar();
//turn on the leds one at a time.
//This example assumes LED's plugged into locations 0-9
for(i = 0; i < 10; i++)
{
CPhidgetLED_setBrightness(led, i, 100); //maximum brightness is 100, 0 is off. Can set this value to anything including and inbetween these values.
}
printf("Press any key to continue\n");
getchar();
//turn off the LEDs one at a a time
//This example assumes LED's plugged into locations 0-9
for(i = 0; i < 10; i++)
{
CPhidgetLED_setBrightness(led, i, 0); //maximum brightness is 100, 0 is off. Can set this value to anything including and inbetween these values.
}
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)led);
CPhidget_delete((CPhidgetHandle)led);
//all done, exit
return 0;
}
qPhidgetRFID::~qPhidgetRFID()
{
CPhidget_close((CPhidgetHandle)rfidHandle);
CPhidget_delete((CPhidgetHandle)rfidHandle);
}
