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 XnSensorIO::OpenDevice(const XnChar* strPath)
{
XnStatus nRetVal;
XnUSBDeviceSpeed DevSpeed;
nRetVal = xnUSBInit();
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_USB_ALREADY_INIT)
return nRetVal;
xnLogVerbose(XN_MASK_DEVICE_IO, "Connecting to USB device...");
if (strPath == NULL || strcmp(strPath, "*:0") == 0)
{
// support old style API
XnConnectionString aConnections[1];
XnUInt32 nCount = 1;
nRetVal = EnumerateSensors(aConnections, &nCount);
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_OUTPUT_BUFFER_OVERFLOW)
{
return nRetVal;
}
strPath = aConnections[0];
}
// try to open the device
xnLogVerbose(XN_MASK_DEVICE_IO, "Trying to open sensor '%s'...", strPath);
nRetVal = xnUSBOpenDeviceByPath(strPath, &m_pSensorHandle->USBDevice);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnUSBGetDeviceSpeed(m_pSensorHandle->USBDevice, &DevSpeed);
XN_IS_STATUS_OK(nRetVal);
if (DevSpeed != XN_USB_DEVICE_HIGH_SPEED)
{
XN_LOG_WARNING_RETURN(XN_STATUS_USB_UNKNOWN_DEVICE_SPEED, XN_MASK_DEVICE_IO, "Device is not high speed!");
}
// on older firmwares, control was sent over BULK endpoints. Check if this is the case
xnLogVerbose(XN_MASK_DEVICE_IO, "Trying to open endpoint 0x4 for control out (for old firmwares)...");
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x4, XN_USB_EP_BULK, XN_USB_DIRECTION_OUT, &m_pSensorHandle->ControlConnection.ControlOutConnectionEp);
if (nRetVal == XN_STATUS_USB_ENDPOINT_NOT_FOUND || nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_TYPE || nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_DIRECTION)
{
// this is not the case. use regular control endpoint (0)
m_pSensorHandle->ControlConnection.bIsBulk = FALSE;
}
else
{
XN_IS_STATUS_OK(nRetVal);
xnLogVerbose(XN_MASK_DEVICE_IO, "Opening endpoint 0x85 for control in...");
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x85, XN_USB_EP_BULK, XN_USB_DIRECTION_IN, &m_pSensorHandle->ControlConnection.ControlInConnectionEp);
XN_IS_STATUS_OK(nRetVal);
m_pSensorHandle->ControlConnection.bIsBulk = TRUE;
}
nRetVal = IsSensorLowBandwidth(strPath, &m_bIsLowBandwidth);
XN_IS_STATUS_OK(nRetVal);
xnLogInfo(XN_MASK_DEVICE_IO, "Connected to USB device%s", m_bIsLowBandwidth ? " (LowBand)" : "");
strcpy(m_strDeviceName, strPath);
return XN_STATUS_OK;
}
XnStatus XnSensorIO::OpenDevice(const XnChar* strPath, XnBool bLeanInit)
{
XnStatus nRetVal;
XnUSBDeviceSpeed DevSpeed;
xnLogVerbose(XN_MASK_DEVICE_IO, "Connecting to USB device...");
// try to open the device
xnLogVerbose(XN_MASK_DEVICE_IO, "Trying to open sensor '%s'...", strPath);
nRetVal = xnUSBOpenDeviceByPath(strPath, &m_pSensorHandle->USBDevice);
XN_IS_STATUS_OK(nRetVal);
if (!bLeanInit)
{
nRetVal = xnUSBGetDeviceSpeed(m_pSensorHandle->USBDevice, &DevSpeed);
XN_IS_STATUS_OK(nRetVal);
if (DevSpeed != XN_USB_DEVICE_HIGH_SPEED)
{
XN_LOG_WARNING_RETURN(XN_STATUS_USB_UNKNOWN_DEVICE_SPEED, XN_MASK_DEVICE_IO, "Device is not high speed!");
}
}
// on older firmwares, control was sent over BULK endpoints. Check if this is the case
xnLogVerbose(XN_MASK_DEVICE_IO, "Trying to open endpoint 0x4 for control out (for old firmwares)...");
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x4, XN_USB_EP_BULK, XN_USB_DIRECTION_OUT, &m_pSensorHandle->ControlConnection.ControlOutConnectionEp);
if (nRetVal == XN_STATUS_USB_ENDPOINT_NOT_FOUND || nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_TYPE || nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_DIRECTION)
{
// this is not the case. use regular control endpoint (0)
m_pSensorHandle->ControlConnection.bIsBulk = FALSE;
}
else
{
XN_IS_STATUS_OK(nRetVal);
xnLogVerbose(XN_MASK_DEVICE_IO, "Opening endpoint 0x85 for control in...");
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x85, XN_USB_EP_BULK, XN_USB_DIRECTION_IN, &m_pSensorHandle->ControlConnection.ControlInConnectionEp);
XN_IS_STATUS_OK(nRetVal);
m_pSensorHandle->ControlConnection.bIsBulk = TRUE;
}
nRetVal = XnDeviceEnumeration::IsSensorLowBandwidth(strPath, &m_bIsLowBandwidth);
XN_IS_STATUS_OK(nRetVal);
xnLogInfo(XN_MASK_DEVICE_IO, "Connected to USB device%s", m_bIsLowBandwidth ? " (LowBand)" : "");
// check if we're currently on BULK interfaces or ISO ones
XN_USB_EP_HANDLE hEP;
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x82, XN_USB_EP_BULK, XN_USB_DIRECTION_IN, &hEP);
if (nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_TYPE)
{
m_interface = XN_SENSOR_USB_INTERFACE_ISO_ENDPOINTS;
}
else
{
m_interface = XN_SENSOR_USB_INTERFACE_BULK_ENDPOINTS;
xnUSBCloseEndPoint(hEP);
}
strcpy(m_strDeviceName, strPath);
return XN_STATUS_OK;
}
