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;
}
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 Spatial::setDataRate(int rate)
{
CPhidgetSpatial_setDataRate(spatial_handle_, rate);
}
void Imu::setDataRate(int rate)
{
CPhidgetSpatial_setDataRate(imu_handle_, rate);
}
int CCONV spatial_on_attach(CPhidgetHandle phid, void *userptr) {
PhidgetInfo *info = userptr;
SpatialInfo *spatial_info = info->type_info;
// Accelerometer Attributes:
report(CPhidgetSpatial_getAccelerationAxisCount((CPhidgetSpatialHandle)phid, &spatial_info->accelerometer_axes));
report(CPhidgetSpatial_getGyroAxisCount((CPhidgetSpatialHandle)phid, &spatial_info->gyro_axes));
report(CPhidgetSpatial_getCompassAxisCount((CPhidgetSpatialHandle)phid, &spatial_info->compass_axes));
report(CPhidgetSpatial_getDataRateMax((CPhidgetSpatialHandle)phid, &spatial_info->data_rate_max));
report(CPhidgetSpatial_getDataRateMin((CPhidgetSpatialHandle)phid, &spatial_info->data_rate_min));
// Dealloc if we're alloc'd, this will prevent memory leaks on device re-attachment:
if (spatial_info->acceleration) xfree(spatial_info->acceleration);
if (spatial_info->acceleration_min) xfree(spatial_info->acceleration_min);
if (spatial_info->acceleration_max) xfree(spatial_info->acceleration_max);
if (spatial_info->compass) xfree(spatial_info->compass);
if (spatial_info->compass_min) xfree(spatial_info->compass_min);
if (spatial_info->compass_max) xfree(spatial_info->compass_max);
if (spatial_info->gyroscope) xfree(spatial_info->gyroscope);
if (spatial_info->gyroscope_min) xfree(spatial_info->gyroscope_min);
if (spatial_info->gyroscope_max) xfree(spatial_info->gyroscope_max);
// Allocate space for our extents:
spatial_info->acceleration = ALLOC_N(double, spatial_info->accelerometer_axes);
spatial_info->acceleration_min = ALLOC_N(double, spatial_info->accelerometer_axes);
spatial_info->acceleration_max = ALLOC_N(double, spatial_info->accelerometer_axes);
spatial_info->compass = ALLOC_N(double, spatial_info->compass_axes);
spatial_info->compass_min = ALLOC_N(double, spatial_info->compass_axes);
spatial_info->compass_max = ALLOC_N(double, spatial_info->compass_axes);
spatial_info->gyroscope = ALLOC_N(double, spatial_info->gyro_axes);
spatial_info->gyroscope_min = ALLOC_N(double, spatial_info->gyro_axes);
spatial_info->gyroscope_max = ALLOC_N(double, spatial_info->gyro_axes);
// Initialize the gyro dcm:
if (spatial_info->gyro_axes == 3) {
double identity3x3[3][3] = {{1.0,0.0,0.0}, {0.0,1.0,0.0}, {0.0,0.0,1.0}};
memcpy(&spatial_info->gyroscope_dcm, &identity3x3, sizeof(identity3x3));
}
// Accelerometer
for(int i=0; i < spatial_info->accelerometer_axes; i++) {
report(CPhidgetSpatial_getAccelerationMin((CPhidgetSpatialHandle)phid, i, &spatial_info->acceleration_min[i]));
report(CPhidgetSpatial_getAccelerationMax((CPhidgetSpatialHandle)phid, i, &spatial_info->acceleration_max[i]));
}
for(int i=0; i < spatial_info->compass_axes; i++) {
report(CPhidgetSpatial_getMagneticFieldMin((CPhidgetSpatialHandle)phid, i, &spatial_info->compass_min[i]));
report(CPhidgetSpatial_getMagneticFieldMax((CPhidgetSpatialHandle)phid, i, &spatial_info->compass_max[i]));
}
for(int i=0; i < spatial_info->gyro_axes; i++) {
report(CPhidgetSpatial_getAngularRateMin((CPhidgetSpatialHandle)phid, i, &spatial_info->gyroscope_min[i]));
report(CPhidgetSpatial_getAngularRateMax((CPhidgetSpatialHandle)phid, i, &spatial_info->gyroscope_max[i]));
}
// Ahrs Values:
spatial_info->is_ahrs_initialized = false;
// Set the data rate for the spatial events in milliseconds.
// Note that 1000/16 = 62.5 Hz
report(CPhidgetSpatial_setDataRate((CPhidgetSpatialHandle)phid, spatial_info->data_rate));
// Strictly speaking, this is entirely optional:
if (spatial_info->is_compass_correction_known)
spatial_set_compass_correction_by_array( (CPhidgetSpatialHandle)phid,
spatial_info->compass_correction);
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;
}
