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;
}
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;
}
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;
}
}
// Init
bool phidgetIMU::Init()
{
// create driver object
if( CPhidgetSpatial_create((CPhidgetSpatialHandle*)&mHandle) != 0 )
{
printf("phidgetIMU -- failed to create IMU device\n");
return false;
}
//displayProperties();
// 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)mHandle, imuAttach, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)mHandle, imuDetach, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)mHandle, imuError, NULL);
// registers a callback that will run according to the set data rate that will return the spatial data changes
CPhidgetSpatial_set_OnSpatialData_Handler((CPhidgetSpatialHandle)mHandle, imuData, this);
// open the spatial object for device connections
if( CPhidget_open((CPhidgetHandle)mHandle, -1) != 0 )
{
printf("failed to open IMU device\n");
return false;
}
//displayProperties();
// get the program to wait for a spatial device to be attached
/*printf("waiting for IMU to be attached...\n");
const int result = CPhidget_waitForAttachment((CPhidgetHandle)mImu, 2000);
if( result != 0 )
{
const char* err = NULL;
CPhidget_getErrorDescription(result, &err);
printf("failed to find attached IMU (%s)\n", err);
return false;
}*/
//Set the data rate for the spatial events
CPhidgetSpatial_setDataRate((CPhidgetSpatialHandle)mHandle, /*16*/ 4);
printf("phidgetIMU -- initialized IMU device\n");
return true;
}
Imu::Imu():
Phidget(),
imu_handle_(0)
{
// create the handle
CPhidgetSpatial_create(&imu_handle_);
// pass handle to base class
Phidget::init((CPhidgetHandle)imu_handle_);
// register base class callbacks
Phidget::registerHandlers();
// register imu data callback
CPhidgetSpatial_set_OnSpatialData_Handler(imu_handle_, SpatialDataHandler, this);
}
Spatial::Spatial(void):
Phidget(),
spatial_handle_(0)
{
int result;
CPhidgetSpatial_create(&spatial_handle_);
Phidget::init((CPhidgetHandle) spatial_handle_);
Phidget::registerHandlers();
CPhidgetSpatial_set_OnSpatialData_Handler(spatial_handle_, SpatialDataHandler, this);
Phidget::open();
printf("Waiting for spatial to be attached.... \n");
if((result = waitForAttachment(10000)))
{
printf("Problem waiting for attachment \n");
}
}
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);
}
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()
// initialize the phidget interface kit board and phidget spatial (imu)
{
int result, num_analog_inputs, num_digital_inputs;
const char *err;
ros::NodeHandle n;
//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_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, DigitalInputHandler, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, AnalogInputHandler, NULL);
//open the interfacekit and spatial for device connections
CPhidget_open((CPhidgetHandle)ifKit, -1);
CPhidgetInterfaceKit_getInputCount(ifKit, &num_digital_inputs);
CPhidgetInterfaceKit_getSensorCount(ifKit, &num_analog_inputs);
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);
interfaceKitError = 1;
}
else
{
irData_pub = n.advertise<corobot_msgs::SensorMsg>("infrared_data", 100);
powerdata_pub = n.advertise<corobot_msgs::PowerMsg>("power_data", 100);
bumper_pub = n.advertise<corobot_msgs::SensorMsg>("bumper_data", 100);
// sensors connected to the phidget interface kit other than bumpers, voltage sensor, ir sensor and sonars.
other_pub = n.advertise<corobot_msgs::SensorMsg>("sensor_data", 100);
}
//Initialize the phidget spatial board, if any
if (imu)
{
CPhidgetSpatial_create(&spatial);
CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);
CPhidget_open((CPhidgetHandle)spatial, -1);
// attach the devices
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;
}
else
{
imu_pub = n.advertise<sensor_msgs::Imu>("imu_data",100);
mag_pub = n.advertise<sensor_msgs::MagneticField>("magnetic_data",100);
calibrate_gyroscope_service = n.advertiseService("calibrate_gyroscope",calibrate_gyroscope);
}
CPhidgetSpatial_setDataRate(spatial, 4);
}
CPhidgetInterfaceKit_setRatiometric(ifKit, 0);
//Initialize the sonars, if any are present
if(sonarsPresent)
{
CPhidgetInterfaceKit_setOutputState(ifKit, bwOutput, 1);
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
// sleep for 250ms
ros::Duration(0.250).sleep();
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
// sleep for 2ms
ros::Duration(0.002).sleep();
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
// sleep for 150ms
ros::Duration(0.150).sleep();
sonar_pub = n.advertise<corobot_msgs::SensorMsg>("sonar_data", 100);
}
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;
}
