bool Device::Open()
{
const XnUSBConnectionString *paths;
XnUInt32 count;
XnStatus res;
//Don't need this code, because USB is already initialized
/*
// Init OpenNI USB
res = xnUSBInit();
if (res != XN_STATUS_OK)
{
xnPrintError(res, "xnUSBInit failed");
return false;
}
*/
// Open all "Kinect motor" USB devices
res = xnUSBEnumerateDevices(0x045E /* VendorID */, 0x02B0 /*ProductID*/, &paths, &count);
if (res != XN_STATUS_OK)
{
xnPrintError(res, "xnUSBEnumerateDevices failed");
return false;
}
// Open devices
for (XnUInt32 index = 0; index < count; ++index)
{
res = xnUSBOpenDeviceByPath(paths[index], &devices[index]);
if (res != XN_STATUS_OK) {
xnPrintError(res, "xnUSBOpenDeviceByPath failed");
return false;
}
}
num = count;
XnUChar buf[1]; // output buffer
// Init motors
for (XnUInt32 index = 0; index < num; ++index)
{
res = xnUSBSendControl(devices[index], (XnUSBControlType) 0xc0, 0x10, 0x00, 0x00, buf, sizeof(buf), 0);
if (res != XN_STATUS_OK) {
xnPrintError(res, "xnUSBSendControl failed");
Close();
return false;
}
res = xnUSBSendControl(devices[index], XN_USB_CONTROL_TYPE_VENDOR, 0x06, 0x01, 0x00, NULL, 0, 0);
if (res != XN_STATUS_OK) {
xnPrintError(res, "xnUSBSendControl failed");
Close();
return false;
}
}
isOpened = true;
return true;
}
XnStatus OpenNIMotorController::Create()
{
const XnUSBConnectionString *paths;
XnUInt32 count;
XnStatus res;
// Init OpenNI USB
res = xnUSBInit();
//std::cout << xnGetStatusString(res) << " " << res << std::endl;
if( res != XN_STATUS_OK && res != 131142 /** USB alreay initialized **/ )
return res;
// list all "Kinect motor" USB devices
res = xnUSBEnumerateDevices( 0x45e, 0x02b0, &paths, &count );
if( res != XN_STATUS_OK )
return res;
const XnChar* pUSBtoUse = paths[0];
if( count > 0 )
{
res = xnUSBOpenDeviceByPath( pUSBtoUse, &m_xDevice );
for( int i=0; i < count; ++i )
{
unsigned short vendor_id;
unsigned short product_id;
unsigned char bus;
unsigned char address;
sscanf(paths[i], "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
//printf("MotorInfo vendor_id %i product_id %i bus %i address %i \n", vendor_id, product_id, bus, address );
//std::cout << "MI: " << paths[i] << std::endl;
}
if( res != XN_STATUS_OK )
return res;
// Init motors
XnUChar buf;
res = xnUSBSendControl( m_xDevice, (XnUSBControlType) 0xc0, 0x10, 0x00, 0x00, &buf, sizeof(buf), 0 );
if( res != XN_STATUS_OK )
{
Release();
return res;
}
res = xnUSBSendControl( m_xDevice, XN_USB_CONTROL_TYPE_VENDOR, 0x06, 0x01, 0x00, NULL, 0, 0);
if( res != XN_STATUS_OK )
{
Release();
return res;
}
m_bOpened = true;
std::cout << "\n_2Real: The motor controller was initialized correctly" << std::endl;
return XN_STATUS_OK;
}
return XN_STATUS_OS_FILE_OPEN_FAILED;
}
XnStatus Enumerate(XnUInt16 nProduct, XnStringsSet& devicesSet)
{
XnStatus nRetVal = XN_STATUS_OK;
const XnUSBConnectionString* astrDevicePaths;
XnUInt32 nCount;
nRetVal = xnUSBEnumerateDevices(XN_SENSOR_VENDOR_ID, nProduct, &astrDevicePaths, &nCount);
XN_IS_STATUS_OK(nRetVal);
for (XnUInt32 i = 0; i < nCount; ++i)
{
nRetVal = devicesSet.Set(astrDevicePaths[i]);
XN_IS_STATUS_OK(nRetVal);
}
xnUSBFreeDevicesList(astrDevicePaths);
return (XN_STATUS_OK);
}
