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 ClientUSBControlEndpoint::Send(const void* pData, XnUInt32 nSize)
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnUSBSendControl(m_hUSBDevice, XN_USB_CONTROL_TYPE_VENDOR, 0, 0, 0, (XnUInt8*)pData, nSize, SEND_TIMEOUT);
XN_IS_STATUS_OK_LOG_ERROR("Send USB control data", nRetVal);
return XN_STATUS_OK;
}
XnStatus OpenNIMotorController::Move( int angle )
{
if( !m_bOpened )
return XN_STATUS_OS_EVENT_OPEN_FAILED;
XnStatus res = xnUSBSendControl( m_xDevice, XN_USB_CONTROL_TYPE_VENDOR, 0x31, 2 * angle, 0x00, NULL, 0, 0 );
if( res != XN_STATUS_OK )
return res;
return XN_STATUS_OK;
}
XnStatus OpenNIMotorController::SetLED( LED_STATUS eStatus )
{
if( !m_bOpened )
return XN_STATUS_OS_EVENT_OPEN_FAILED;
XnStatus res = xnUSBSendControl( m_xDevice, XN_USB_CONTROL_TYPE_VENDOR, 0x06, eStatus, 0x00, NULL, 0, 0 );
if( res != XN_STATUS_OK )
return res;
return XN_STATUS_OK;
}
bool Device::SetLedColour(int colour)
{
XnStatus res;
for (XnUInt32 index = 0; index < num; ++index)
{
res = xnUSBSendControl(devices[index], XN_USB_CONTROL_TYPE_VENDOR, 0x06, colour, 0x00, NULL, 0, 0);
if (res != XN_STATUS_OK)
{
xnPrintError(res, "xnUSBSendControl failed");
return false;
}
}
return true;
}
bool Device::Move(int angle)
{
XnStatus res;
if(angle > currentPosition && angle > 40 || angle < currentPosition && angle < -40)
return true;
// Send move control requests
for (XnUInt32 index = 0; index < num; ++index)
{
res = xnUSBSendControl(devices[index], XN_USB_CONTROL_TYPE_VENDOR, 0x31, angle, 0x00, NULL, 0, 0);
if (res != XN_STATUS_OK)
{
xnPrintError(res, "xnUSBSendControl failed");
return false;
}
}
currentPosition = angle;
return true;
}