kinect motor cotrol

kinect-motor.cpp

// 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;
}