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;
}
Wt::WContainerWidget* WidgetsCommon::CreateWidget()
{
PhidgetsInfo* item = ::GetPhidgetManager()->FindPhidgetBySerial(GetSerial());
if (!item)
return NULL;
CPhidgetHandle handle = item->m_phidget->GetHandle();
Wt::WContainerWidget* tab_container = new Wt::WContainerWidget();
Wt::WHBoxLayout* hbox = new Wt::WHBoxLayout(tab_container);
Wt::WTable* table = new Wt::WTable();
hbox->addWidget(table);
table->columnAt(0)->setWidth(GetLeftColumnWidth());
table->columnAt(1)->setWidth(Wt::WLength::Auto);
int row = 0;
const char* string_value;
int int_value;
if (EPHIDGET_OK == CPhidget_getDeviceName(handle, &string_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceName")));
table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString(string_value, Wt::UTF8)));
}
if (EPHIDGET_OK == CPhidget_getSerialNumber(handle, &int_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("SerialNumber")));
table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString("{1}").arg(int_value)));
}
if (EPHIDGET_OK == CPhidget_getDeviceVersion(handle, &int_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceVersion")));
table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString("{1}").arg(int_value)));
}
if (EPHIDGET_OK == CPhidget_getDeviceStatus(handle, &int_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceStatus")));
table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString("{1}").arg(int_value)));
}
if (EPHIDGET_OK == CPhidget_getDeviceType(handle, &string_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceType")));
table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString(string_value, Wt::UTF8)));
}
if (EPHIDGET_OK == CPhidget_getDeviceLabel(handle, &string_value))
{
table->elementAt(row, 0)->addWidget(new Wt::WText(Wt::WString::tr("DeviceLabel")));
table->elementAt(row++, 1)->addWidget(new Wt::WText(Wt::WString(string_value, Wt::UTF8)));
}
return tab_container;
}
JNIEXPORT jboolean JNICALL
Java_com_phidgets_Manager_isAttached(JNIEnv *env, jobject obj)
{
CPhidgetHandle h = (CPhidgetHandle)(uintptr_t)(*env)->GetLongField(env,
obj, manager_handle_fid);
int error;
int s;
if ((error = CPhidget_getDeviceStatus(h, &s)))
PH_THROW(error);
return s == PHIDGET_ATTACHED;
}
/**
* @brief Publish the data on their corresponding topics
*/
int publish_data(){
if(spatial_good)
{
corobot_msgs::spatial spatial;
sensor_msgs::Imu imu;
spatial.acc1 = acc[0];
spatial.acc2 = acc[1];
spatial.acc3 = acc[2];
spatial.ang1 = ang[0];
spatial.ang2 = ang[1];
spatial.ang3 = ang[2];
spatial.mag1 = mag[0];
spatial.mag2 = mag[1];
spatial.mag3 = mag[2];
imu.header.frame_id = "base_link";
imu.header.stamp = ros::Time::now();
imu.orientation = tf::createQuaternionMsgFromRollPitchYaw((atan2(dCMMatrix[2][1],dCMMatrix[2][2]) + rollOffset),(-asin(dCMMatrix[2][0]) + pitchOffset), (atan2(dCMMatrix[1][0],dCMMatrix[0][0])));
imu.angular_velocity.x = ang[0];
imu.angular_velocity.y = ang[1];
imu.angular_velocity.z = ang[2];
imu.linear_acceleration.x = acc[0];
imu.linear_acceleration.y = acc[1];
imu.linear_acceleration.z = acc[2];
spatial_pub.publish(spatial);
imu_pub.publish(imu);
spatialError = 0;
}
if(m_encodersGood) //the position is 4times the number of encoder counts.
{
corobot_msgs::PosMsg posdata;
// prepare to read the encoder status
int phidgetEncoderStatus = 0;
CPhidget_getDeviceStatus((CPhidgetHandle) m_leftEncoder,
&phidgetEncoderStatus);
if (phidgetEncoderStatus != 0)
{
int value;
CPhidgetEncoder_getPosition(m_leftEncoder, m_leftEncoderNumber, &value);
posdata.px = -value;
printf("Left encoder value = %d", value);
}
else
{
encoderError = 2;
}
phidgetEncoderStatus = 0;
CPhidget_getDeviceStatus((CPhidgetHandle) m_rightEncoder,
&phidgetEncoderStatus);
if (phidgetEncoderStatus != 0)
{
int value;
CPhidgetEncoder_getPosition(m_rightEncoder, m_rightEncoderNumber, &value);
// we negate this value since the right side motors turn the opposite direction
// to the left side
posdata.py = value;
}
else
{
encoderError = 2;
}
posdata.header.stamp = ros::Time::now();
posdata_pub.publish(posdata);
encoderError = 0;
}
else
{
encoderError = 1;
}
if (m_validAnalogs){
////////////////////////////
// Update power data
if(batteryPort != -1)
{
corobot_msgs::PowerMsg powerdata;
powerdata.volts = (float) (analog_inputs_value[batteryPort] - 500) * 0.0734;
//The Min and Max present here are for the Nimh battery as it is the only one type of battery sold with a Corobot at the moment
powerdata.min_volt = 10.0;
powerdata.max_volt = 14.2;
powerdata_pub.publish(powerdata);
}
////////////////////////////
// Update IR data
if(irFrontPort != -1 || irBackPort != -1)
{
corobot_msgs::IrMsg irData;
if(irFrontPort != -1)
irData.voltage1=(float) analog_inputs_value[irFrontPort] / 200.0;
else
irData.voltage1=0;
if(irBackPort != -1)
irData.voltage2=(float) analog_inputs_value[irBackPort] / 200.0;
else
irData.voltage2=0;
irData.range1=irVoltageToDistance(irData.voltage1);
irData.range2=irVoltageToDistance(irData.voltage2);
irData_pub.publish(irData);
}
if(gripperPort != -1)
{
corobot_msgs::GripperMsg gripperData; // Gripper connect to the 4th analog input port?
gripperData.state=analog_inputs_value[gripperPort];
gripper_pub.publish(gripperData);
}
interfaceKitError = 0;
}
if (m_validDigitals)
{
////////////////////////////
// Update the bumper data
corobot_msgs::BumperMsg bumper_data;
if (m_rearBumperPresent)
{
bumper_data.bumpers_count=4;
bumper_data.value0=(digital_inputs_value) & (0x01<<0);
bumper_data.value1=(digital_inputs_value) & (0x01<<1);
bumper_data.value2=(digital_inputs_value) & (0x01<<2);
bumper_data.value3=(digital_inputs_value) & (0x01<<3);
}
else {
bumper_data.bumpers_count=2;
bumper_data.value0=(digital_inputs_value) & (0x01<<0);
bumper_data.value1=(digital_inputs_value) & (0x01<<1);
}
bumper_pub.publish(bumper_data);
interfaceKitError = 0;
}
return 0;
}
