Simple WebSocket server to control a tank.
Dependencies: SNICInterface_mod WebSocketServer mbed-rtos mbed PowerControl C12832
au Firefox OS WoTハッカソン on ホワイトデーで使用した、タンクを動かすプログラムです。 ゲームパッドでタンクを操縦します。
ゲームパッドは PC に接続し、ブラウザ上の Web アプリから Gamepad API で入力を取得します。 取得した入力データは WebSocket で mbed 上の WebSocket サーバに送信します。
WebSocket サーバのコードはライブラリ化したので、他のプログラムでもインポートして使えます。
使用した機材
- LPC1768
- Murata TypeYD
- LPC1768 用アプリケーションボード
- TAMIYA トラック&ホイールセット
- TAMIYA ダブルキヤボックス(左右独立4速タイプ)
- TAMIYA ユニバーサルプレート
- TOSHIBA TA7291P x 2
- その他、モバイルバッテリー、電池ボックス等
左右のモータードライバにそれぞれ LPC1768 の p12, p13 と p14, p15 のピンを割り当てていますが、必要に応じてコードを変更してください。
コントローラー側(Webアプリ)
https://github.com/chikoski/wotxwot-control
Firefox ブラウザで動作確認しています(他のブラウザでは動かないかも)。 ゲームパッドの左右のスティックの前後の操作が左右それぞれのモータの前転・後転に対応しています。
動いているところの動画
https://www.facebook.com/video.php?v=456620974491805
ハッカソンでは ARM 賞をいただきました!
(参考) au Firefox OS WoTハッカソン on ホワイトデー
http://au-fx.kddi.com/event/20150314/wot_hackathon0314.html
Revision 11:a66b3b2aeb6c, committed 2015-03-16
- Comitter:
- flatbird
- Date:
- Mon Mar 16 10:18:34 2015 +0000
- Parent:
- 10:578778037efb
- Commit message:
- updated.
Changed in this revision
--- a/GSwifi.lib Mon Mar 16 18:48:50 2015 +0900 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/gsfan/code/GSwifi/#0b5e2727e020
--- a/WebSocketConnection.cpp Mon Mar 16 18:48:50 2015 +0900 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,235 +0,0 @@ -#include "WebSocketConnection.h" -#include "WebSocketServer.h" -#include "sha1.h" - -#define UPGRADE_WEBSOCKET "Upgrade: websocket" -#define SEC_WEBSOCKET_KEY "Sec-WebSocket-Key:" -#define MAGIC_NUMBER "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" -#define OP_CONT 0x0 -#define OP_TEXT 0x1 -#define OP_BINARY 0x2 -#define OP_CLOSE 0x8 -#define OP_PING 0x9 -#define OP_PONG 0xA - -WebSocketConnection::WebSocketConnection(WebSocketServer* server) -{ - mServer = server; -} - -WebSocketConnection::~WebSocketConnection() -{ -} - -void WebSocketConnection::run() -{ - char buf[1024]; - bool isWebSocket = false; - - // it doesn't work... - // mConnection.set_blocking(true); - - while (mConnection.is_connected()) { - int ret = mConnection.receive(buf, sizeof(buf) - 1); - if (ret == 0) { - // printf("No data to receive\r\n"); - continue; - } - if (ret < 0) { - printf("ERROR: Failed to receive %d\r\n", ret); - break; - } - if (!isWebSocket) { - if (this->handleHTTP(buf, ret)) { - isWebSocket = true; - } else { - printf("ERROR: Non websocket\r\n"); - break; - } - } else { - if (!this->handleWebSocket(buf, ret)) { - break; - } - } - } - // printf("Closed\r\n"); - mConnection.close(); -} - -bool WebSocketConnection::handleHTTP(char* buf, int size) -{ - char* line = &buf[0]; - char key[128]; - bool isUpgradeWebSocket = false; - bool isSecWebSocketKeyFound = false; - - for (int i = 0; i < size; i++) { - if (buf[i] == '\r' && i+1 < size && buf[i+1] == '\n') { - buf[i] = '\0'; - if (strlen(buf) <= 0) { - break; - } - printf("[%s]\r\n", line); - if (line == &buf[0]) { - char* method = strtok(buf, " "); - char* path = strtok(NULL, " "); - char* version = strtok(NULL, " "); - // printf("[%s] [%s] [%s]\r\n", method, path, version); - mHandler = mServer->getHandler(path); - if (!mHandler) { - printf("ERROR: Handler not found for %s\r\n", path); - return false; - } - } else if (strncmp(line, UPGRADE_WEBSOCKET, strlen(UPGRADE_WEBSOCKET)) == 0) { - isUpgradeWebSocket = true; - } else if (strncmp(line, SEC_WEBSOCKET_KEY, strlen(SEC_WEBSOCKET_KEY)) == 0) { - isSecWebSocketKeyFound = true; - char* ptr = line + strlen(SEC_WEBSOCKET_KEY); - while (*ptr == ' ') ++ptr; - strcpy(key, ptr); - } - i += 2; - line = &buf[i]; - } - } - - if (isUpgradeWebSocket && isSecWebSocketKeyFound) { - this->sendUpgradeResponse(key); - if (mHandler) { - mHandler->onOpen(); - } - mPrevFin = true; - return true; - } - - return false; -} - -bool WebSocketConnection::handleWebSocket(char* buf, int size) -{ - uint8_t* ptr = (uint8_t*)buf; - - bool fin = (*ptr & 0x80) == 0x80; - uint8_t opcode = *ptr & 0xF; - - if (opcode == OP_PING) { - *ptr = ((*ptr & 0xF0) | OP_PONG); - mConnection.send_all(buf, size); - return true; - } - if (opcode == OP_CLOSE) { - if (mHandler) { - mHandler->onClose(); - } - return false; - } - ptr++; - - if (!fin || !mPrevFin) { - printf("WARN: Data consists of multiple frame not supported\r\n"); - mPrevFin = fin; - return true; // not an error, just discard it - } - mPrevFin = fin; - - bool mask = (*ptr & 0x80) == 0x80; - uint8_t len = *ptr & 0x7F; - ptr++; - - if (len > 125) { - printf("WARN: Extended payload length not supported\r\n"); - return true; // not an error, just discard it - } - - char* data; - if (mask) { - char* maskingKey = (char*)ptr; - data = (char*)(ptr + 4); - for (int i = 0; i < len; i++) { - data[i] = data[i] ^ maskingKey[(i % 4)]; - } - } else { - data = (char*)ptr; - } - if (mHandler) { - if (opcode == OP_TEXT) { - data[len] = '\0'; - mHandler->onMessage(data); - } else if (opcode == OP_BINARY) { - mHandler->onMessage(data, len); - } - } - return true; -} - -char* base64Encode(const uint8_t* data, size_t size, - char* outputBuffer, size_t outputBufferSize) -{ - static char encodingTable[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', - 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', - 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', - 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', - 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', - 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', - 'w', 'x', 'y', 'z', '0', '1', '2', '3', - '4', '5', '6', '7', '8', '9', '+', '/'}; - size_t outputLength = 4 * ((size + 2) / 3); - if (outputBufferSize - 1 < outputLength) { // -1 for NUL - return NULL; - } - - for (size_t i = 0, j = 0; i < size; /* nothing */) { - uint32_t octet1 = i < size ? (unsigned char)data[i++] : 0; - uint32_t octet2 = i < size ? (unsigned char)data[i++] : 0; - uint32_t octet3 = i < size ? (unsigned char)data[i++] : 0; - - uint32_t triple = (octet1 << 0x10) + (octet2 << 0x08) + octet3; - - outputBuffer[j++] = encodingTable[(triple >> 3 * 6) & 0x3F]; - outputBuffer[j++] = encodingTable[(triple >> 2 * 6) & 0x3F]; - outputBuffer[j++] = encodingTable[(triple >> 1 * 6) & 0x3F]; - outputBuffer[j++] = encodingTable[(triple >> 0 * 6) & 0x3F]; - } - - static int padTable[] = { 0, 2, 1 }; - int paddingCount = padTable[size % 3]; - - for (int i = 0; i < paddingCount; i++) { - outputBuffer[outputLength - 1 - i] = '='; - } - outputBuffer[outputLength] = '\0'; // NUL - - return outputBuffer; -} - -bool WebSocketConnection::sendUpgradeResponse(char* key) -{ - char buf[128]; - - if (strlen(key) + sizeof(MAGIC_NUMBER) > sizeof(buf)) { - return false; - } - strcpy(buf, key); - strcat(buf, MAGIC_NUMBER); - - unsigned char hash[20]; - char encoded[30]; - sha1(buf, strlen(buf), (char*)hash); - base64Encode(hash, 20, encoded, sizeof(encoded)); - - char resp[] = "HTTP/1.1 101 Switching Protocols\r\n" \ - "Upgrade: websocket\r\n" \ - "Connection: Upgrade\r\n" \ - "Sec-WebSocket-Accept: XXXXXXXXXXXXXXXXXXXXXXXXXXXXX\r\n\r\n"; - char* ptr = strstr(resp, "XXXXX"); - strcpy(ptr, encoded); - strcpy(ptr+strlen(encoded), "\r\n\r\n"); - - int ret = mConnection.send_all(resp, strlen(resp)); - if (ret < 0) { - printf("ERROR: Failed to send response\r\n"); - return false; - } - - return true; -}
--- a/WebSocketConnection.h Mon Mar 16 18:48:50 2015 +0900 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,31 +0,0 @@ -#ifndef _WEB_SOCKET_CONNECTION_H_ -#define _WEB_SOCKET_CONNECTION_H_ - -#include "TCPSocketServer.h" -#include "WebSocketHandler.h" -#include <string> -#include <map> - -class WebSocketServer; - -class WebSocketConnection -{ -public: - WebSocketConnection(WebSocketServer* server); - virtual ~WebSocketConnection(); - - void run(); - TCPSocketConnection& getTCPSocketConnection() { return mConnection; } - -private: - bool handleHTTP(char* buf, int size); - bool handleWebSocket(char* buf, int size); - bool sendUpgradeResponse(char* key); - - WebSocketServer* mServer; - TCPSocketConnection mConnection; - WebSocketHandler* mHandler; - bool mPrevFin; -}; - -#endif
--- a/WebSocketHandler.h Mon Mar 16 18:48:50 2015 +0900 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,16 +0,0 @@ -#ifndef _WEB_SOCKET_HANDLER_H_ -#define _WEB_SOCKET_HANDLER_H_ - -class WebSocketHandler -{ -public: - virtual void onOpen() {}; - virtual void onClose() {}; - // to receive text message - virtual void onMessage(char* text) {}; - // to receive binary message - virtual void onMessage(char* data, size_t size) {}; - virtual void onError() {}; -}; - -#endif
--- a/WebSocketServer.cpp Mon Mar 16 18:48:50 2015 +0900 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,59 +0,0 @@ -#include "WebSocketServer.h" -#include "WebSocketConnection.h" - -WebSocketServer::WebSocketServer() -{ -} - -WebSocketServer::~WebSocketServer() -{ -} - -bool WebSocketServer::init(int port) -{ - mTCPSocketServer.set_blocking(true); - - int ret = mTCPSocketServer.bind(port); - if (ret != 0) { - printf("ERROR: Failed to bind %d\r\n", ret); - return false; - } - ret = mTCPSocketServer.listen(); - if (ret != 0) { - printf("ERROR: Failed to listen %d\r\n", ret); - return false; - } - - return true; -} - -void WebSocketServer::run() -{ - WebSocketConnection connection(this); - - while (true) { - // printf("accepting\r\n"); - int ret = mTCPSocketServer.accept(connection.getTCPSocketConnection()); - if (ret != 0) { - continue; - } - connection.run(); - } -} - -void WebSocketServer::setHandler(const char* path, WebSocketHandler* handler) -{ - mHandlers[path] = handler; -} - -WebSocketHandler* WebSocketServer::getHandler(const char* path) -{ - WebSocketHandlerContainer::iterator it; - - it = mHandlers.find(path); - if (it != mHandlers.end()) { - return it->second; - } - return NULL; -} -
--- a/WebSocketServer.h Mon Mar 16 18:48:50 2015 +0900 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,27 +0,0 @@ -#ifndef _WEB_SOCKET_SERVER_H_ -#define _WEB_SOCKET_SERVER_H_ - -#include "TCPSocketServer.h" -#include "WebSocketHandler.h" -#include <string> -#include <map> - -class WebSocketServer -{ -public: - WebSocketServer(); - virtual ~WebSocketServer(); - - bool init(int port); - void run(); - void setHandler(const char* path, WebSocketHandler* handler); - WebSocketHandler* getHandler(const char* path); - -private: - typedef std::map<std::string, WebSocketHandler*> WebSocketHandlerContainer; - - TCPSocketServer mTCPSocketServer; - WebSocketHandlerContainer mHandlers; -}; - -#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WebSocketServer.lib Mon Mar 16 10:18:34 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/flatbird/code/WebSocketServer/#a816c25e83ed
--- a/main.cpp Mon Mar 16 18:48:50 2015 +0900 +++ b/main.cpp Mon Mar 16 10:18:34 2015 +0000 @@ -127,7 +127,7 @@ lcd_printf("websocket closed\r\n"); } -#define THRESHOLD 20 +#define THRESHOLD 10 void WSHandler::onMessage(char* data, size_t size) {