1 // Forked from: https://groups.google.com/d/msg/openni-dev/T_CeVW_d8ig/dsBKONIpNyQJ
  2
  3 #include <XnUSB.h>
  4 #include <cstdio>
  5
  6 #ifdef _WIN32
  7 #include <Windows.h>
  8 void pause_ ()
  9 {
 10     Sleep(1000);
 11 }
 12 #else
 13 #include <unistd.h>
 14 void pause_ ()
 15 {
 16     sleep(1);
 17 }
 18 #endif
 19
 20 /**
 21  * Class to control Kinect's motor.
 22  */
 23 class KinectMotors
 24 {
 25 public:
 26     enum { MaxDevs = 16 };
 27
 28 public:
 29     KinectMotors();
 30     virtual ~KinectMotors();
 31
 32     /**
 33         * Open device.
 34         * @return true if succeeded, false - overwise
 35         */
 36     bool Open();
 37
 38     /**
 39         * Close device.
 40         */
 41     void Close();
 42
 43     /**
 44         * Move motor up or down to specified angle value.
 45         * @param angle angle value
 46         * @return true if succeeded, false - overwise
 47         */
 48     bool Move(int angle);
 49
 50 private:
 51     XN_USB_DEV_HANDLE m_devs[MaxDevs];
 52     XnUInt32 m_num;
 53     bool m_isOpen;
 54 };
 55
 56 KinectMotors::KinectMotors()
 57 {
 58     m_isOpen = false;
 59 }
 60
 61 KinectMotors::~KinectMotors()
 62 {
 63     Close();
 64 }
 65
 66 bool KinectMotors::Open()
 67 {
 68     const XnUSBConnectionString *paths;
 69     XnUInt32 count;
 70     XnStatus res;
 71
 72     // Init OpenNI USB
 73     res = xnUSBInit();
 74     if (res != XN_STATUS_OK)
 75     {
 76         xnPrintError(res, "xnUSBInit failed");
 77         return false;
 78     }
 79
 80     // Open all "Kinect motor" USB devices
 81     res = xnUSBEnumerateDevices(0x045E /* VendorID */, 0x02B0 /*ProductID*/, &paths, &count);
 82     if (res != XN_STATUS_OK)
 83     {
 84         xnPrintError(res, "xnUSBEnumerateDevices failed");
 85         return false;
 86     }
 87
 88     // Open devices
 89     for (XnUInt32 index = 0; index < count; ++index)
 90     {
 91         res = xnUSBOpenDeviceByPath(paths[index], &m_devs[index]);
 92         if (res != XN_STATUS_OK) {
 93             xnPrintError(res, "xnUSBOpenDeviceByPath failed");
 94             return false;
 95         }
 96     }
 97
 98     m_num = count;
 99     XnUChar buf[1]; // output buffer
100
101     // Init motors
102     for (XnUInt32 index = 0; index < m_num; ++index)
103     {
104         res = xnUSBSendControl(m_devs[index], (XnUSBControlType) 0xc0, 0x10, 0x00, 0x00, buf, sizeof(buf), 0);
105         if (res != XN_STATUS_OK) {
106             xnPrintError(res, "xnUSBSendControl failed");
107             Close();
108             return false;
109         }
110
111         res = xnUSBSendControl(m_devs[index], XN_USB_CONTROL_TYPE_VENDOR, 0x06, 0x01, 0x00, NULL, 0, 0);
112         if (res != XN_STATUS_OK) {
113             xnPrintError(res, "xnUSBSendControl failed");
114             Close();
115             return false;
116         }
117     }
118
119     m_isOpen = true;
120
121     return true;
122 }
123
124 void KinectMotors::Close()
125 {
126     if (m_isOpen) {
127         for (XnUInt32 index = 0; index < m_num; ++index) {
128             xnUSBCloseDevice(m_devs[index]);
129         }
130         m_isOpen = false;
131     }
132 }
133
134 bool KinectMotors::Move(int angle)
135 {
136     XnStatus res;
137
138     // Send move control requests
139     for (XnUInt32 index = 0; index < m_num; ++index)
140     {
141         res = xnUSBSendControl(m_devs[index], XN_USB_CONTROL_TYPE_VENDOR, 0x31, angle, 0x00, NULL, 0, 0);
142
143         if (res != XN_STATUS_OK)
144         {
145             xnPrintError(res, "xnUSBSendControl failed");
146             return false;
147         }
148     }
149     return true;
150 }
151
152 int main(int argc, char *argv[])
153 {
154     KinectMotors motors;
155
156     if (!motors.Open()) // Open motor devices
157         return 1;
158
159     motors.Move(31); // Move them up to 31 degree
160
161     pause_();
162
163     motors.Move(-31); // Move them down to 31 degree.
164
165     pause_();
166
167     motors.Move(0);
168     return 0;
169 }