Created
April 24, 2012 03:28
-
-
Save nomadalex/2476181 to your computer and use it in GitHub Desktop.
kinect motor cotrol
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// Usb control: http://openkinect.org/wiki/Protocol_Documentation#Control_Packet_Structure | |
#include <XnUSB.h> | |
#include <cstdio> | |
#include <cstring> | |
#ifdef _WIN32 | |
#include <Windows.h> | |
void sleep_ (int s) | |
{ | |
Sleep(1000 * s); | |
} | |
#else | |
#include <unistd.h> | |
void sleep_ (int s) | |
{ | |
sleep(s); | |
} | |
#endif | |
/** | |
* Class to control Kinect's motor. | |
*/ | |
class KinectMotors | |
{ | |
public: | |
enum { MaxDevs = 16 }; | |
enum LED_STATUS { | |
LED_OFF = 0, | |
LED_GREEN, | |
LED_RED, | |
LED_ORANGE, | |
LED_BLINK_ORANGE, | |
LED_BLINK_GREEN, | |
LED_BLINK_RED_ORANGE | |
}; | |
enum STATUS { | |
STOPED = 0, | |
AT_LIMIT, | |
MOVING = 4, | |
QUICK_BREAK = 8, | |
UNKNOW = -1 | |
}; | |
class Device | |
{ | |
private: | |
XN_USB_DEV_HANDLE handle; | |
bool m_isOpen; | |
Device(const Device& d); | |
Device& operator =(const Device& d); | |
public: | |
Device() : handle(NULL), m_isOpen(false) {} | |
Device(XN_USB_DEV_HANDLE h) : handle(h), m_isOpen(false) {} | |
virtual ~Device() { if (m_isOpen) Close(); } | |
void SetHandle(XN_USB_DEV_HANDLE h) { handle = h; } | |
/** | |
* Open device. | |
* @return true if succeeded, false - overwise | |
*/ | |
bool Open(); | |
/** | |
* Close device. | |
*/ | |
void Close(); | |
/** | |
* Move motor up or down to specified angle value. | |
* @param angle angle value | |
* @return true if succeeded, false - overwise | |
*/ | |
bool Move(int angle); | |
/** | |
* Set Led Status | |
* @param status status code | |
* @return true if succeeded, false - overwise | |
*/ | |
bool SetLed(int status); | |
/** | |
* Get current status (status code, speed, angle) | |
* @param status motor status code | |
* @param speed current speed if in moving | |
* @param angle current angle value | |
* @return true if succeeded, false - overwise | |
*/ | |
bool GetStatus(int& status, int& speed, int& angle); | |
}; | |
KinectMotors(); | |
virtual ~KinectMotors(); | |
bool Initialize(); | |
size_t Count() { return m_num; } | |
Device& operator [](const int idx) { | |
return m_devs[idx]; | |
} | |
private: | |
Device m_devs[MaxDevs]; | |
XnUInt32 m_num; | |
}; | |
KinectMotors::KinectMotors() : m_num(0) | |
{ | |
} | |
KinectMotors::~KinectMotors() | |
{ | |
} | |
bool KinectMotors::Initialize() | |
{ | |
const XnUSBConnectionString *paths; | |
XnUInt32 count; | |
XnStatus res; | |
// 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 | |
XN_USB_DEV_HANDLE h; | |
for (XnUInt32 index = 0; index < count; ++index) | |
{ | |
res = xnUSBOpenDeviceByPath(paths[index], &h); | |
if (res != XN_STATUS_OK) { | |
xnPrintError(res, "xnUSBOpenDeviceByPath failed"); | |
return false; | |
} | |
m_devs[index].SetHandle(h); | |
} | |
m_num = count; | |
return true; | |
} | |
bool KinectMotors::Device::Open() | |
{ | |
if (handle == NULL) return false; | |
XnStatus res; | |
XnUChar buf[1]; // output buffer | |
// Init motors | |
res = xnUSBSendControl(handle, (XnUSBControlType) 0xc0, 0x10, 0x00, 0x00, buf, sizeof(buf), 0); | |
if (res != XN_STATUS_OK) { | |
xnPrintError(res, "xnUSBSendControl failed"); | |
Close(); | |
return false; | |
} | |
res = xnUSBSendControl(handle, XN_USB_CONTROL_TYPE_VENDOR, 0x06, 0x01, 0x00, NULL, 0, 0); | |
if (res != XN_STATUS_OK) { | |
xnPrintError(res, "xnUSBSendControl failed"); | |
Close(); | |
return false; | |
} | |
m_isOpen = true; | |
return true; | |
} | |
void KinectMotors::Device::Close() | |
{ | |
if (m_isOpen) { | |
xnUSBCloseDevice(handle); | |
m_isOpen = false; | |
} | |
} | |
bool KinectMotors::Device::Move(int angle) | |
{ | |
XnStatus res; | |
// Send move control requests | |
res = xnUSBSendControl(handle, XN_USB_CONTROL_TYPE_VENDOR, 0x31, 2*angle, 0x00, NULL, 0, 0); | |
if (res != XN_STATUS_OK) | |
{ | |
xnPrintError(res, "xnUSBSendControl failed"); | |
return false; | |
} | |
return true; | |
} | |
bool KinectMotors::Device::SetLed(int status) | |
{ | |
XnStatus res; | |
res = xnUSBSendControl(handle, XN_USB_CONTROL_TYPE_VENDOR, 0x06, status, 0x00, NULL, 0, 0); | |
if (res != XN_STATUS_OK) | |
{ | |
xnPrintError(res, "xnUSBSendControl failed"); | |
return false; | |
} | |
return true; | |
} | |
bool KinectMotors::Device::GetStatus(int& status, int& speed, int& angle) | |
{ | |
XnStatus res; | |
XnUChar buf[10] = {0}; | |
XnUInt32 size = 0; | |
res = xnUSBReceiveControl(handle, XN_USB_CONTROL_TYPE_VENDOR, 0x32, 0, 0, buf, 10, &size, 0); | |
if (res != XN_STATUS_OK) | |
{ | |
xnPrintError(res, "xnUSBSendControl failed"); | |
return false; | |
} | |
status = static_cast<int>(buf[9]); | |
speed = static_cast<int>(buf[1]); | |
angle = static_cast<int>(static_cast<char>(buf[8]))/2; | |
return true; | |
} | |
int main(int argc, char *argv[]) | |
{ | |
KinectMotors motors; | |
int status, speed, angle; | |
if (!motors.Initialize()) // Open motor devices | |
return 1; | |
if (motors.Count() > 0) { | |
KinectMotors::Device& dev = motors[0]; | |
dev.GetStatus(status, speed, angle); | |
std::printf("%d %d %d\n", status, speed, angle); | |
dev.Move(15); | |
sleep_(2); | |
dev.GetStatus(status, speed, angle); | |
std::printf("%d %d %d\n", status, speed, angle); | |
dev.Move(31); // Move them up to 31 degree | |
sleep_(2); | |
dev.Move(-31); // Move them down to 31 degree. | |
dev.GetStatus(status, speed, angle); | |
std::printf("%d %d %d\n", status, speed, angle); | |
sleep_(4); | |
dev.GetStatus(status, speed, angle); | |
std::printf("%d %d %d\n", status, speed, angle); | |
dev.Move(0); | |
dev.SetLed(KinectMotors::LED_OFF); | |
sleep_(2); | |
dev.SetLed(KinectMotors::LED_RED); | |
sleep_(2); | |
dev.SetLed(KinectMotors::LED_BLINK_GREEN); | |
} | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment