OpenNIでチルトモーターを動かす

SyntaxHighlighterを導入してみたのでコードを貼ってみた。

過去にはてなダイアリーに書いてたエントリーのコピペ。

KINETで何か開発するにはチルトモーターの制御は欲しいところ。
しかし、OpenNIのリファレンスを見ても全然載ってない（？）
唯一見つけられたhagino_3000さんのエントリーを参考にしてボタンで制御できるようにした。
http://d.hatena.ne.jp/hagino_3000/20110720/1311097327
※xnUSBInit()は呼び出すだけで落ちるから使っていない。

#include 
#include 
#include //Tilt

#include 
#include 
#include 

#define VID_MICROSOFT 0x45e//Tilt
#define PID_NUI_MOTOR 0x02b0//Tilt

constchar* CONFIG_XML_PATH = "/*****/config.xml";
XN_USB_DEV_HANDLE dev;//Tilt


int main (int argc, constchar * argv[])
{    
    xn::Contextcontext;
    IplImage *rgbImage = NULL;
    int angle = 0;//Tilt
    XnUChar empty[0x1];//Tilt
    
    try {
        xn::EnumerationErrorsctxErrors;
        XnStatus rc = context.InitFromXmlFile(CONFIG_XML_PATH, &ctxErrors);
        if (rc != XN_STATUS_OK) {
            XnChar strError[1024];
            ctxErrors.ToString(strError, 1024);
            std::cout << strError << std::endl;
            return1;
        }
        context.SetGlobalMirror(false);
        xn::ImageGeneratorimage;
        rc = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
        if (rc != XN_STATUS_OK) {
            throwstd::runtime_error(xnGetStatusString(rc));
        }
        
        XnMapOutputMode mapMode;
        image.GetMapOutputMode(mapMode);
        
        int width = mapMode.nXRes;
        int hight = mapMode.nYRes;
        
        rgbImage = cvCreateImage(cvSize(width, hight), IPL_DEPTH_8U, 3);
        
        while (1) {
            context.WaitAndUpdateAll();
            
            
            xn::ImageMetaDataimageMetaData;
            image.GetMetaData(imageMetaData);
            
            xnOSMemCopy(rgbImage->imageData, imageMetaData.RGB24Data(), rgbImage->imageSize);
            
            cvCvtColor(rgbImage, rgbImage, CV_RGB2BGR);
            cvShowImage("Video View", rgbImage);
            
            char key = cvWaitKey(10);
            if (key == 27 || key == 'q') {
                break;
            }
            
            elseif (key == 's'){
                angle = 0;
                
                rc = xnUSBOpenDevice(VID_MICROSOFT, PID_NUI_MOTOR, NULL, NULL, &dev);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
                rc = xnUSBSendControl(dev, XN_USB_CONTROL_TYPE_VENDOR, 0x31, (XnUInt16)angle, 0x0, empty, 0x0, 0);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
                rc = xnUSBCloseDevice(dev);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
            }
            
            elseif (key == 'u'){
                angle += 5;
            
                rc = xnUSBOpenDevice(VID_MICROSOFT, PID_NUI_MOTOR, NULL, NULL, &dev);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
                rc = xnUSBSendControl(dev, XN_USB_CONTROL_TYPE_VENDOR, 0x31, (XnUInt16)angle, 0x0, empty, 0x0, 0);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
                rc = xnUSBCloseDevice(dev);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
            }
            elseif (key == 'd'){
                angle -= 5;
                
                rc = xnUSBOpenDevice(VID_MICROSOFT, PID_NUI_MOTOR, NULL, NULL, &dev);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
                rc = xnUSBSendControl(dev, XN_USB_CONTROL_TYPE_VENDOR, 0x31, (XnUInt16)angle, 0x0, empty, 0x0, 0);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
                
                rc = xnUSBCloseDevice(dev);
                if (rc != XN_STATUS_OK) {
                    throwstd::runtime_error(xnGetStatusString(rc));
                }
            }
            
        }
        
    } catch (std::exception& ex) {
        std::cout << ex.what() << std::endl;
    }

    if (rgbImage != NULL) {
        cvReleaseImage(&rgbImage);
    }
    
    context.Shutdown();
    return0;
}