int display_properties(CPhidgetInterfaceKitHandle phid)
{
int serial_number, version, ratiometric, num_sensors, num_inputs, num_outputs, triggerVal;
const char* ptr;
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidget_getSerialNumber((CPhidgetHandle)phid, &serial_number);
CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
CPhidgetInterfaceKit_getInputCount(phid, &num_inputs);
CPhidgetInterfaceKit_getOutputCount(phid, &num_outputs);
CPhidgetInterfaceKit_getSensorCount(phid, &num_sensors);
CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);
ROS_INFO("%s", ptr);
ROS_INFO("Serial Number: %d", serial_number);
ROS_INFO("Version: %d", version);
ROS_INFO("Number of digital inputs %d", num_inputs);
ROS_INFO("Number of digital outputs %d", num_outputs);
ROS_INFO("Number of sensors %d", num_sensors);
ROS_INFO("Ratiometric %d", ratiometric);
for (int i = 0; i < num_sensors; i++)
{
CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);
//CPhidgetInterfaceKit_setSensorChangeTrigger (phid, i, 10);
ROS_INFO("Sensor %d Sensitivity Trigger %d", i, triggerVal);
}
return 0;
}
//Display the properties of the attached phidget to the screen. We will be displaying the name, serial number and version of the attached device.
//Will also display the number of inputs, outputs, and analog inputs on the interface kit as well as the state of the ratiometric flag
//and the current analog sensor sensitivity.
int IKDisplayProperties(CPhidgetInterfaceKitHandle phid)
{
int serialNo, version, numInputs, numOutputs, numSensors, triggerVal, ratiometric, i;
const char* ptr;
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
CPhidgetInterfaceKit_getInputCount(phid, &numInputs);
CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
CPhidgetInterfaceKit_getSensorCount(phid, &numSensors);
CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);
SetupLog("%s", ptr);
SetupLog("Serial Number: %10d\nVersion: %8d", serialNo, version);
SetupLog("# Digital Inputs: %d\n# Digital Outputs: %d", numInputs, numOutputs);
SetupLog("# Sensors: %d", numSensors);
SetupLog("Ratiometric: %d", ratiometric);
for(i = 0; i < numSensors; i++)
{
CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);
SetupLog("Sensor#: %d > Sensitivity Trigger: %d\n", i, triggerVal);
}
return 0;
}
// display the properties and create the ifkit model. Should really refactor...
int PhidgetConnector::display_properties(CPhidgetInterfaceKitHandle phid) {
int serialNo, version, numInputs, numOutputs, numSensors, triggerVal, ratiometric, i;
const char* ptr;
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
CPhidgetInterfaceKit_getInputCount(phid, &numInputs);
CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
CPhidgetInterfaceKit_getSensorCount(phid, &numSensors);
CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);
ifKitModels.push_back(new IFKitModel(serialNo, numSensors));
printf("%s\n", ptr);
printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);
printf("# Digital Inputs: %d\n# Digital Outputs: %d\n", numInputs, numOutputs);
printf("# Sensors: %d\n", numSensors);
printf("Ratiometric: %d\n", ratiometric);
for(i = 0; i < numSensors; i++) {
CPhidgetInterfaceKit_setSensorChangeTrigger(phid, i, 0);
CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);
printf("Sensor#: %d > Sensitivity Trigger: %d\n", i, triggerVal);
}
return 0;
}
int test_interfacekit()
{
int numInputs, numOutputs, numSensors;
int err;
CPhidgetInterfaceKitHandle IFK = 0;
CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL);
CPhidgetInterfaceKit_create(&IFK);
CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnOutputChange_Handler(IFK, IFK_OutputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)IFK, IFK_AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)IFK, IFK_DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)IFK, IFK_ErrorHandler, NULL);
CPhidget_open((CPhidgetHandle)IFK, -1);
//wait 5 seconds for attachment
if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 0)) != EPHIDGET_OK )
{
const char *errStr;
CPhidget_getErrorDescription(err, &errStr);
printf("Error waiting for attachment: (%d): %s\n",err,errStr);
goto exit;
}
display_generic_properties((CPhidgetHandle)IFK);
CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle)IFK, &numOutputs);
CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle)IFK, &numInputs);
CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle)IFK, &numSensors);
CPhidgetInterfaceKit_setOutputState((CPhidgetInterfaceKitHandle)IFK, 0, 1);
printf("Sensors:%d Inputs:%d Outputs:%d\n", numSensors, numInputs, numOutputs);
//err = CPhidget_setDeviceLabel((CPhidgetHandle)IFK, "test");
while(1)
{
sleep(1);
}
while(1)
{
CPhidgetInterfaceKit_setOutputState(IFK, 7, 1);
CPhidgetInterfaceKit_setOutputState(IFK, 7, 0);
}
CPhidgetInterfaceKit_setOutputState(IFK, 0, 1);
sleep(1);
CPhidgetInterfaceKit_setOutputState(IFK, 0, 0);
sleep(1);
CPhidgetInterfaceKit_setOutputState(IFK, 0, 1);
sleep(1);
CPhidgetInterfaceKit_setOutputState(IFK, 0, 0);
sleep(5);
exit:
CPhidget_close((CPhidgetHandle)IFK);
CPhidget_delete((CPhidgetHandle)IFK);
return 0;
}
Wt::WContainerWidget* WidgetsInterfaceKit::CreateWidget()
{
Wt::WContainerWidget* tab_container = new Wt::WContainerWidget();
Wt::WVBoxLayout* vbox = new Wt::WVBoxLayout(tab_container);
Wt::WGroupBox* spesific_box = new Wt::WGroupBox(Wt::WString::tr("PhidgetInterfaceKit"));
vbox->addWidget(spesific_box);
Wt::WHBoxLayout* hbox = new Wt::WHBoxLayout(spesific_box);
Wt::WTable* table = new Wt::WTable();
hbox->addWidget(table);
table->columnAt(0)->setWidth(GetLeftColumnWidth());
table->columnAt(1)->setWidth(Wt::WLength::Auto);
int row = 0;
int i, int_value;
bool ratiometric = true;
/* Ratiometric */
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("Ratiometric")));
m_ratiometric_checkbox = new Wt::WCheckBox();
table->elementAt(row++, 1)->addWidget(m_ratiometric_checkbox);
m_ratiometric_checkbox->changed().connect(boost::bind(&WidgetsInterfaceKit::OnWtRatiometricStateChanged, this, m_ratiometric_checkbox));
if (EPHIDGET_OK == CPhidgetInterfaceKit_getRatiometric(m_phidget->GetNativeHandle(), &int_value))
{
ratiometric = (PTRUE == int_value);
m_ratiometric_checkbox->setChecked(ratiometric);
}
/* Sensors */
if (EPHIDGET_OK == CPhidgetInterfaceKit_getSensorCount(m_phidget->GetNativeHandle(), &int_value))
{
m_sensor_widget_array_length = int_value;
m_sensor_widget_array = new SensorWidget*[m_sensor_widget_array_length];
for (i=0; i<m_sensor_widget_array_length; i++)
{
m_sensor_widget_array[i] = new SensorWidget(m_phidget, i, ratiometric);
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("SensorArgs").arg(i)));
table->elementAt(row++, 1)->addWidget(m_sensor_widget_array[i]->CreateWidget());
}
}
/* Input */
if (EPHIDGET_OK == CPhidgetInterfaceKit_getInputCount(m_phidget->GetNativeHandle(), &int_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("Input")));
Wt::WTable* input_table = new Wt::WTable();
table->elementAt(row++, 1)->addWidget(input_table);
m_input_checkbox_array_length = int_value;
m_input_checkbox_array = new Wt::WCheckBox*[m_input_checkbox_array_length];
for (i=0; i<m_input_checkbox_array_length; i++)
{
m_input_checkbox_array[i] = new Wt::WCheckBox();
Wt::WTableCell* cell = input_table->elementAt(0, i);
cell->addWidget(m_input_checkbox_array[i]);
cell->setContentAlignment(Wt::AlignCenter|Wt::AlignMiddle);
cell = input_table->elementAt(1, i);
cell->addWidget(new Wt::WText(Wt::WString::tr("GeneralArg").arg(i)));
cell->setContentAlignment(Wt::AlignCenter|Wt::AlignMiddle);
int input_state;
if (EPHIDGET_OK == CPhidgetInterfaceKit_getInputState(m_phidget->GetNativeHandle(), i, &input_state))
{
m_input_checkbox_array[i]->setChecked(PTRUE == input_state);
}
m_input_checkbox_array[i]->setEnabled(false);
}
}
/* Output */
if (EPHIDGET_OK == CPhidgetInterfaceKit_getOutputCount(m_phidget->GetNativeHandle(), &int_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("Output")));
Wt::WTable* output_table = new Wt::WTable();
table->elementAt(row++, 1)->addWidget(output_table);
m_output_checkbox_array_length = int_value;
m_output_checkbox_array = new Wt::WCheckBox*[m_output_checkbox_array_length];
for (i=0; i<m_output_checkbox_array_length; i++)
{
m_output_checkbox_array[i] = new Wt::WCheckBox();
m_output_checkbox_array[i]->changed().connect(boost::bind(&WidgetsInterfaceKit::OnWtOutputStateChanged, this, m_output_checkbox_array[i]));
Wt::WTableCell* cell = output_table->elementAt(0, i);
cell->addWidget(m_output_checkbox_array[i]);
cell->setContentAlignment(Wt::AlignCenter|Wt::AlignMiddle);
cell = output_table->elementAt(1, i);
cell->addWidget(new Wt::WText(Wt::WString::tr("GeneralArg").arg(i)));
cell->setContentAlignment(Wt::AlignCenter|Wt::AlignMiddle);
int output_state;
if (EPHIDGET_OK == CPhidgetInterfaceKit_getOutputState(m_phidget->GetNativeHandle(), i, &output_state))
{
m_output_checkbox_array[i]->setChecked(PTRUE == output_state);
}
}
}
Wt::WGroupBox* generic_box = new Wt::WGroupBox(Wt::WString::tr("Phidget (Common)"));
vbox->addWidget(generic_box);
generic_box->addWidget(WidgetsCommon::CreateWidget());
return tab_container;
}
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;
}
