first

.gitignore

+.pio
+.pioenvs
+.piolibdeps
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json

.gitmodules

+[submodule "lib/ArduinoEigen"]
+	path = lib/ArduinoEigen
+	url = https://hideakitai@github.com/hideakitai/ArduinoEigen.git

.travis.yml

+# Continuous Integration (CI) is the practice, in software
+# engineering, of merging all developer working copies with a shared mainline
+# several times a day < https://docs.platformio.org/page/ci/index.html >
+#
+# Documentation:
+#
+# * Travis CI Embedded Builds with PlatformIO
+#   < https://docs.travis-ci.com/user/integration/platformio/ >
+#
+# * PlatformIO integration with Travis CI
+#   < https://docs.platformio.org/page/ci/travis.html >
+#
+# * User Guide for `platformio ci` command
+#   < https://docs.platformio.org/page/userguide/cmd_ci.html >
+#
+#
+# Please choose one of the following templates (proposed below) and uncomment
+# it (remove "# " before each line) or use own configuration according to the
+# Travis CI documentation (see above).
+#
+
+
+#
+# Template #1: General project. Test it using existing `platformio.ini`.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio run
+
+
+#
+# Template #2: The project is intended to be used as a library with examples.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# env:
+#     - PLATFORMIO_CI_SRC=path/to/test/file.c
+#     - PLATFORMIO_CI_SRC=examples/file.ino
+#     - PLATFORMIO_CI_SRC=path/to/test/directory
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N

.vscode/extensions.json

+{
+	// See http://go.microsoft.com/fwlink/?LinkId=827846
+	// for the documentation about the extensions.json format
+	"recommendations": [
+		"platformio.platformio-ide"
+	]
+}
\ No newline at end of file

.vscode/settings.json

+{
+    "terminal.integrated.env.windows": {
+        "PATH": "C:\\Users\\HT\\.platformio\\penv\\Scripts;C:\\Users\\HT\\.platformio\\penv;C:\\Users\\HT\\Anaconda3\\Library\\bin;C:\\tools\\msys64;C:\\Users\\HT\\Anaconda3\\Scripts;C:\\Users\\HT\\Anaconda3;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C:\\ProgramData\\chocolatey\\bin;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Program Files\\LLVM\\bin;C:\\Program Files\\Microsoft SQL Server\\130\\Tools\\Binn\\;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Git\\mingw64\\bin;C:\\Program Files\\Git\\usr\\bin;C:\\Program Files (x86)\\ZeroTier\\One\\;C:\\Users\\HT\\AppData\\Local\\Microsoft\\WindowsApps;C:\\tools\\cmder;;C:\\Users\\HT\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;C:\\Program Files\\Oracle\\VirtualBox;C:\\tools\\mingw64\\bin;C:\\tools\\msys64;;C:\\Users\\HT\\Anaconda3\\Library\\bin;C:\\tools\\msys64;C:\\Users\\HT\\Anaconda3\\Scripts;C:\\Users\\HT\\Anaconda3;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C:\\ProgramData\\chocolatey\\bin;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Program Files\\LLVM\\bin;C:\\Program Files\\Microsoft SQL Server\\130\\Tools\\Binn\\;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Git\\mingw64\\bin;C:\\Program Files\\Git\\usr\\bin;C:\\Program Files (x86)\\ZeroTier\\One\\;C:\\Users\\HT\\AppData\\Local\\Microsoft\\WindowsApps;C:\\tools\\cmder;;C:\\Users\\HT\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;C:\\Program Files\\Oracle\\VirtualBox;C:\\tools\\mingw64\\bin;C:\\tools\\msys64;",
+        "PLATFORMIO_CALLER": "vscode"
+    },
+    "terminal.integrated.env.osx": {
+        "PATH": "/Users/HT/.platformio/penv/bin:/Users/HT/.platformio/penv:/Users/HT/.nvm/versions/node/v9.2.0/bin:/Users/HT/.pyenv/shims:/Users/HT/anaconda3/bin:/usr/local/cuda/bin:/usr/local/bin:/usr/local/bin:/usr/bin:/bin:/usr/sbin:/sbin:/Library/TeX/texbin:/usr/local/share/dotnet:/opt/X11/bin:/Library/Frameworks/Mono.framework/Versions/Current/Commands:/usr/local/sbin:/Users/HT/.pyenv/bin",
+        "PLATFORMIO_CALLER": "vscode"
+    },
+    "files.associations": {
+        "__bit_reference": "cpp",
+        "deque": "cpp",
+        "memory": "cpp",
+        "type_traits": "cpp"
+    },
+    "python.pythonPath": "/Users/HT/.pyenv/versions/anaconda3-4.2.0/bin/python"
+}
\ No newline at end of file

include/README

+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

include/global.h

+#pragma once
+#ifndef WHILL_GLOBAL_H
+#define WHILL_GLOBAL_H
+
+#include <Arduino.h>
+
+#define WHILL_SERIAL_L Serial1
+#define WHILL_SERIAL_R Serial3
+#define XBEE_SERIAL Serial2
+
+static constexpr uint8_t MY_ID = 0;
+static constexpr uint32_t WHILL_SERIAL_BAUD = 38400;
+static constexpr uint32_t XBEE_SERIAL_BAUD = 38400;
+static constexpr uint16_t WHILL_INFO_INTERVAL_MS = 1000;
+
+
+
+#endif // WHILL_GLOBAL_H

include/main.h

+#pragma once
+#ifndef MAIN_HEADER_H
+#define MAIN_HEADER_H
+
+#include <Arduino.h>
+#include <WhillOmni.h>
+#include <BorderXBeePacketizer.h>
+
+using namespace Whill;
+
+WhillOmni whill;
+BorderXBeePacker packer;
+BorderXBeeUnpacker unpacker;
+
+void printSpeedProfile(WhillOmni::LR lr)
+{
+    Serial.println("SPEED_PROFILE");
+    Serial.print("dataset : "); Serial.println(whill.dataset(lr));
+    Serial.print("mode    : "); Serial.println(whill.speedMode(lr));
+    Serial.print("fwd spd : "); Serial.println(whill.forwardSpeed(lr));
+    Serial.print("fwd acc : "); Serial.println(whill.forwardAccel(lr));
+    Serial.print("fwd dec : "); Serial.println(whill.forwardDecel(lr));
+    Serial.print("bwd spd : "); Serial.println(whill.reverseSpeed(lr));
+    Serial.print("bwd acc : "); Serial.println(whill.reverseAccel(lr));
+    Serial.print("bwd dec : "); Serial.println(whill.reverseDecel(lr));
+    Serial.print("trn spd : "); Serial.println(whill.turnSpeed(lr));
+    Serial.print("trn acc : "); Serial.println(whill.turnAccel(lr));
+    Serial.print("trn dec : "); Serial.println(whill.turnDecel(lr));
+}
+
+void printSensorInfo(WhillOmni::LR lr)
+{
+    Serial.print("SENSOR_INFO");
+    Serial.print("sensor acc x : "); Serial.println(whill.sensorAccelX(lr));
+    Serial.print("sensor acc y : "); Serial.println(whill.sensorAccelY(lr));
+    Serial.print("sensor acc z : "); Serial.println(whill.sensorAccelZ(lr));
+    Serial.print("sensor gyr x : "); Serial.println(whill.sensorGyroX(lr));
+    Serial.print("sensor gyr y : "); Serial.println(whill.sensorGyroY(lr));
+    Serial.print("sensor gyr z : "); Serial.println(whill.sensorGyroZ(lr));
+    Serial.print("joystick fb  : "); Serial.println(whill.joystickFront(lr));
+    Serial.print("joystick lr  : "); Serial.println(whill.joystickSide(lr));
+    Serial.print("battery      : "); Serial.println(whill.battery(lr));
+    Serial.print("battery crnt : "); Serial.println(whill.batteryCurrent(lr));
+    Serial.print("angle rad r  : "); Serial.println(whill.angleRadR(lr));
+    Serial.print("angle rad l  : "); Serial.println(whill.angleRadL(lr));
+    Serial.print("speed r      : "); Serial.println(whill.speedR(lr));
+    Serial.print("speed l      : "); Serial.println(whill.speedL(lr));
+    Serial.print("power state  : "); Serial.println(whill.powerState(lr));
+    Serial.print("speed mode   : "); Serial.println(whill.speedModeIndicator(lr));
+    Serial.print("error code   : "); Serial.println(whill.errorCode(lr));
+}
+
+void setVelocity()
+{
+    unpacker.parse();
+    while (unpacker.available())
+    {
+        if (unpacker.id() == MY_ID)
+            whill.move(unpacker.velocityX(), unpacker.velocityY(), unpacker.velocityW());
+
+        unpacker.pop();
+    }
+}
+
+void sendStatus(WhillOmni::LR lr)
+{
+    while (whill.available(lr))
+    {
+        packer.pack(MY_ID, (uint8_t)lr, whill.battery(lr), whill.errorCode(lr));
+        XBEE_SERIAL.write(packer.data(), packer.size());
+
+        if      (whill.dataset(lr) == 0) printSpeedProfile(lr);
+        else if (whill.dataset(lr) == 1) printSensorInfo(lr);
+        else    Serial.println("invalid dataset number");
+
+        whill.pop(lr);
+    }
+}
+
+void waitReply(WhillOmni::LR lr, uint16_t wait_ms = 0)
+{
+    delay(wait_ms);
+
+    whill.parse();
+    while(whill.available(lr))
+    {
+        if      (whill.dataset(lr) == 0) printSpeedProfile(lr);
+        else if (whill.dataset(lr) == 1) printSensorInfo(lr);
+        else    Serial.println("invalid dataset number");
+        whill.pop(lr);
+    }
+}
+
+#endif // MAIN_HEADER_H

ArduinoEigen @ 6bd02d8e

+Subproject commit 6bd02d8e41239b3207fdb957b6b7be2eec36ae87

lib/BorderXBeePacketizer/BorderXBeePacketizer.cpp

+#include "BorderXBeePacketizer.h"

lib/BorderXBeePacketizer/BorderXBeePacketizer.h

+#pragma once
+#ifndef BORDERXBEEPACKETIZER_H
+#define BORDERXBEEPACKETIZER_H
+
+#include <Arduino.h>
+#include <queue>
+#include <array>
+
+namespace BorderXBeeProtocol
+{
+    static constexpr uint8_t BYTE_HEDADER = 0xFF;
+    static constexpr uint8_t BYTE_FOOTER = 0xFE;
+}
+
+class BorderXBeeUnpacker
+{
+    struct WhillXBeeData
+    {
+        uint8_t id;
+        uint8_t size;
+        std::array<uint8_t, 8> data;
+        uint8_t sum;
+
+        std::array<uint16_t, 3> vel;
+        void clear() { id = 0xFF; size = 0; data.fill(0); sum = 0; vel.fill(0); }
+    };
+
+    enum class WhillXBeeState { Header, ID, DATA };
+
+public:
+
+    void attatch(Stream& stream) { serial = &stream; }
+    uint8_t available() { return packets.size(); }
+
+    uint8_t size() { return packets.front().size; }
+    uint8_t data(uint8_t i) { return packets.front().data[i]; }
+
+    uint8_t id() { return packets.front().id; }
+    uint16_t velocityX() { return packets.front().vel[0]; }
+    uint16_t velocityY() { return packets.front().vel[1]; }
+    uint16_t velocityW() { return packets.front().vel[2]; }
+
+    void pop() { packets.pop(); }
+
+
+    void parse()
+    {
+        while (serial->available())
+        {
+            uint8_t data = serial->read();
+
+            switch(state)
+            {
+                case WhillXBeeState::Header:
+                {
+                    clearPacket();
+                    if (data == BorderXBeeProtocol::BYTE_HEDADER)
+                        state = WhillXBeeState::ID;
+                    break;
+                }
+                case WhillXBeeState::ID:
+                {
+                    buffer.id = data;
+                    state = WhillXBeeState::DATA;
+                    break;
+                }
+                case WhillXBeeState::DATA:
+                {
+                    if (data == BorderXBeeProtocol::BYTE_FOOTER)
+                    {
+                        if (buffer.size != 6)
+                        {
+                            Serial.print("recv data size error!!!! size = ");
+                            Serial.println(buffer.size);
+                        }
+                        if (buffer.sum == buffer.data[buffer.size - 1])
+                            packets.push(buffer);
+                        else
+                            ++error_count;
+
+                        clearPacket();
+                        break;
+                    }
+                    buffer.data[buffer.size] = data;
+                    ++buffer.size;
+                    buffer.sum += data;
+                    break;
+                }
+                default:
+                {
+                    clearPacket();
+                    break;
+                }
+            }
+        }
+
+    }
+
+private:
+
+    void calcurateVel()
+    {
+        float spd = (float)(((uint16_t)buffer.data[1] << 8) | (uint16_t)buffer.data[0]); // [mm/s]
+        float rad = (float)(((uint16_t)buffer.data[3] << 8) | (uint16_t)buffer.data[2]) / 10000.f; // [rad]
+        float rot = (float)(((uint16_t)buffer.data[5] << 8) | (uint16_t)buffer.data[4]) - 1000.f; // [rad/s] ??
+
+        buffer.vel[0] = spd * cos(rad) / 1000.f; // [m/s]
+        buffer.vel[1] = spd * sin(rad) / 1000.f; // [m/s]
+        buffer.vel[2] = map(rot, -1000.f, 1000.f, -PI, PI); // [rad/s] ??
+    }
+
+    void clearPacket() { buffer.clear(); state = WhillXBeeState::Header; }
+
+    Stream* serial;
+    WhillXBeeData buffer;
+    std::queue<WhillXBeeData> packets;
+    WhillXBeeState state;
+    uint32_t error_count;
+};
+
+
+class BorderXBeePacker
+{
+public:
+
+    void pack(uint8_t id, uint8_t lr, uint8_t battery, uint8_t error)
+    {
+        buffer[0] = BorderXBeeProtocol::BYTE_HEDADER;
+        buffer[1] = id;
+        buffer[2] = lr;
+        buffer[3] = battery;
+        buffer[4] = error;
+        buffer[5] = BorderXBeeProtocol::BYTE_FOOTER;
+    }
+
+    uint8_t* data() { return buffer.data(); }
+    uint8_t size() { return buffer.size(); }
+
+private:
+
+    std::array<uint8_t, 6> buffer;
+};
+
+#endif // BORDERXBEEPACKETIZER_H

lib/README

+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

lib/Whill/CMakeLists.txt

+cmake_minimum_required(VERSION 3.1)
+add_library(WHILL STATIC
+  USBSerial.cpp
+  ProtocolPacker.cpp
+  ProtocolUnpacker.cpp
+  Whill.cpp
+  WhillOmni.cpp
+)
\ No newline at end of file

lib/Whill/OmniModel.h

+#pragma once
+
+#include <Arduino.h>
+#include <ArduinoEigen.h>
+#include "WorkSpaceModel.h"
+
+namespace Work
+{
+    using namespace Eigen;
+
+    namespace Model
+    {
+        // right hand coord
+        // +X: right, +Y: forward, +Z: up
+
+        template <typename WorkType, typename JointType, size_t DmsW, size_t DmsJ>
+        struct Omni : public Base<WorkType, JointType, DmsW, DmsJ>
+        {
+            // !!!!! required to use this class as pointers !!!!!
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+            const double g = 9.80665; // gravitational acceleraation [m/s^2]
+
+            const double R = 0.1; // wheel radius [m]
+            const double W = 0.2; // wheel base / 2.0 [m] (from center to wheel)
+            // const double T = 0.29; // tread / 2.0 [m] (from center to wheel)
+            const double Mp = 10.0; // mass of platform [kg]
+            const double Ml = 10.0; // mass of load [kg]
+            // const double Fwz = (Mp + Ml) * g; // z axis force limit of each wheel [Nm]
+            const double Iw = 0.1; // inertia of wheel (include motor)
+            const double Ip = 0.5; // inertia of platform center (around Z axis)
+            const double H = 0.5; // load height (center of gravity) [m]
+            // const double L = T + W;
+            // const double A = (Mp + Ml) * R * R / 8.0;
+            // const double B = Ip * R * R / (16.0 * L * L);
+            const double AA = Mp * R * R / 2.0;
+            const double BB = Ip * R * R / (4.0 * W * W);
+            const double CC = (AA + BB) / (R * R);
+
+            const double wheel_thickness = 0.05;
+
+            // const....
+            MatrixXd I     {DmsJ, DmsJ};
+            MatrixXd I_inv {DmsJ, DmsJ};
+            MatrixXd In    {DmsJ, DmsJ};
+            MatrixXd C     {DmsJ, 2};
+            MatrixXd C_inv {2, DmsJ};
+
+            Omni()
+            : Model::Base<WorkType, JointType, DmsW, DmsJ>()
+            {
+                // C <<
+                //    Ml * H * T,  Ml * H *W,
+                //   -Ml * H * T,  Ml * H *W,
+                //   -Ml * H * T, -Ml * H *W,
+                //    Ml * H * T, -Ml * H *W;
+
+                // C_inv = PseudoInverse(C);
+
+                I <<
+                    AA + BB,     -AA,  BB,
+                        -AA, 2. * AA, -AA,
+                         BB,     -AA,  CC;
+
+                I_inv = I.inverse();
+
+                this->Jacobian_local <<
+                    -1.,  1., sqrt(2.) * W,
+                     1.,  1., sqrt(2.) * W,
+                     1., -1., sqrt(2.) * W,
+                    -1., -1., sqrt(2.) * W;
+
+                this->Jacobian_local /= sqrt(2.) * R;
+                updateJacobian(0.0);
+
+                this->Jacobian_tra = this->Jacobian.transpose();
+
+                Matrix<double, DmsJ, DmsJ> J_null_tmp = this->Jacobian_inv * this->Jacobian;
+
+                this->Jacobian_null = MatrixXd(DmsJ, DmsJ).setIdentity() - J_null_tmp;
+
+                MatrixXd M_buff(DmsW, DmsJ);
+                M_buff = this->Jacobian * I_inv;
+                this->M_inv = M_buff * this->Jacobian_tra;
+
+
+                this->Mn.setIdentity();
+
+                this->Mn_inv = this->Mn.inverse();
+
+                for (size_t i = 0; i < DmsJ; ++i) In(i, i) = I(i, i);
+
+            }
+
+            virtual void updateJacobian(const double& rad) override
+            {
+                this->rotation = AngleAxisd(rad, Vector3d::UnitZ()).matrix();
+                this->Jacobian = this->rotation * this->Jacobian_local;
+                this->Jacobian_inv = PseudoInverse(this->Jacobian);
+            }
+
+
+            virtual WorkType estimateGravity() override
+            {
+                WorkType gravity = WorkType::Zero();
+                return gravity;
+            }
+
+            virtual WorkType estimateFriction(const WorkType& vel) override
+            {
+                WorkType friction = WorkType::Zero();
+//                if (abs(vel.x) > 0.0)
+//                    friction.x = -Math::sign<double>(vel.x) * 0.002 * (Mp + Ml) * g;
+//                else
+//                    friction.x = 0.0;
+//                if (abs(vel.y) > 0.0)
+//                    friction.y = -Math::sign<double>(vel.y) * 0.002 * (Mp + Ml) * g;
+//                else
+//                    friction.y = 0.0;
+//                if (abs(res.vel.z) > 0.0)
+//                    friction.z = -Math::sign<double>(vel.z) * 0.002 * (Mp + Ml) * g;
+//                else
+//                    friction.z = 0.0;
+                return friction;
+            }
+
+            virtual WorkType estimateCoriolis() override
+            {
+                WorkType coriolis = WorkType::Zero();
+                return coriolis;
+            }
+
+//             virtual WorkType estimateLimitation(const WorkType& acc) override
+//             {
+//                 WorkType limit = WorkType::Zero();
+//                 bool b_excess = false;
+//                 JointType fz(JointType::Zero());
+//                 JointType f_excess(JointType::Zero());
+//                 fz = C * acc;
+//                 for (size_t i = 0; i < DmsJ; ++i)
+//                 {
+//                     if (fz(i) > Fwz)
+//                     {
+//                         f_excess(i) = fz(i) - Fwz;
+//                         b_excess = true;
+//                     }
+//                 }
+//                 if (b_excess)
+//                 {
+//                     limit = C_inv * f_excess;
+// //                    LOG_NOTICE("EXCESS FORCE");// << f_excess;
+//                 }
+//                 return limit;
+//             }
+
+
+            const double& getWheelBase() const { return W; }
+            // const double& getTread() const { return T; }
+            const double& getDiameter() const { return R; }
+            const double& getWheelThickness() const { return wheel_thickness; }
+        };
+
+    }
+
+}
+
+

lib/Whill/ProtocolPacker.cpp

+#include <Arduino.h>
+#include <cstdint>
+#include "ProtocolPacker.h"
+
+using namespace Whill;
+
+void Packer::power(bool b)
+{
+    SW sw = b ? SW::ENABLE : SW::DISABLE;
+    pack((char)CMD::PWR_WHILL, (char)sw);
+}
+
+void Packer::powerSupply(bool b)
+{
+    SW sw = b ? SW::ENABLE : SW::DISABLE;
+    pack((char)CMD::PWR_SUPPLY, (char)sw);
+}
+
+void Packer::controlEnable(bool b)
+{
+    USR enable = b ? USR::DISABLE : USR::ENABLE;
+    pack((char)CMD::JOYSTICK, (char)enable, 0, 0);
+}
+
+void Packer::joystick(int8_t fb, int8_t lr)
+{
+    pack((char)CMD::JOYSTICK, (char)USR::DISABLE, (char)fb, (char)lr);
+}
+
+void Packer::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1)
+{
+    pack (
+        (char)CMD::SPEED, (char)mode,
+        f_m1, f_a1, f_d1,
+        r_m1, r_a1, r_d1,
+        t_m1, t_a1, t_d1
+    );
+}
+
+void Packer::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
+{
+    if (b) pack((char)CMD::STREAMING_START, (char)n, (char)((interval >> 8) & 0xFF), (char)(interval & 0xFF), (char)mode);
+    else   pack((char)CMD::STREAMING_STOP);
+}

lib/Whill/ProtocolPacker.h

+#pragma once
+#ifndef WHILL_PROTOCOL_PACKER_H
+#define WHILL_PROTOCOL_PACKER_H
+
+#include <cstdint>
+#include <deque>
+#include <vector>
+#include "WhillCommon.h"
+
+WHILL_NAMESPACE_BEGIN
+
+class Packer
+{
+public:
+
+    const char* data() { return packet.data(); }
+    const char size() { return packet.size(); }
+    void clear() { packet.clear(); } // TODO: smarter way...
+
+    void power(bool b);
+    void powerSupply(bool b);
+    void controlEnable(bool b);
+
+    // fb, lr : -100 - +100
+    void joystick(int8_t fb, int8_t lr);
+
+    // mode : SPEEDMODE { JOYSTICK_1, JOYSTICK_2, JOYSTICK_3, JOYSTICK_4, RS232C, IPHONE };
+    // f_m1 : max speed    at forward movement. range is ( 8 -  80), unit of value is 0.1km/h, special range for RZM, less than 60 is better
+    // f_a1 : acceleration at forward movement. range is (10 -  90)
+    // f_d1 : deceleration at forward movement. range is (40 - 160)
+    // r_m1 : max speed    at reverse movement. range is ( 8 -  80), unit of value is 0.1km/h, special range for RZM, less than 60 is better
+    // r_a1 : acceleration at reverse movement. range is (10 -  50)
+    // r_d1 : deceleration at reverse movement. range is (40 -  80)
+    // t_m1 : max speed    at turn    movement. range is ( 8 -  80), unit of value is 0.1km/h, special range for RZM, less than 60 is better
+    // t_a1 : acceleration at turn    movement. range is (10 -  60)
+    // t_d1 : deceleration at turn    movement. range is (40 - 160)
+    void speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1);
+
+    void streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode);
+
+private:
+
+    enum class CMD { STREAMING_START = 0, STREAMING_STOP, PWR_WHILL, JOYSTICK, SPEED, PWR_SUPPLY };
+    enum class USR { DISABLE = 0, ENABLE };
+    enum class SW  { DISABLE = 0, ENABLE };
+
+    template <typename First, typename... Rest>
+    void pack(First&& first, Rest&&... rest)
+    {
+        cmd.push_back(std::move(first));
+        pack(std::forward<Rest>(rest)...);
+    }
+
+    void pack()
+    {
+        char checksum = 0;
+
+        packet.emplace_back(PROTOCOL_SIGN);
+        checksum ^= packet.back();
+        packet.emplace_back(cmd.size() + 1); // length of data + 1(=checksum)
+        checksum ^= packet.back();
+
+        for(uint8_t i = 0; i < cmd.size(); i++)
+        {
+            packet.emplace_back(cmd[i]);
+            checksum ^= packet.back();
+        }
+        packet.emplace_back(checksum);
+
+        cmd.clear();
+    }
+
+    std::deque<char> cmd;
+    std::vector<char> packet;
+};
+
+
+WHILL_NAMESPACE_END
+
+#endif // WHILL_PROTOCOL_PACKER_H

lib/Whill/ProtocolUnpacker.cpp

+#include <cstdint>
+#include "ProtocolUnpacker.h"
+
+using namespace Whill;
+
+void Unpacker::feed(const char* const data, const uint8_t size)
+{
+    for (uint8_t i = 0; i < size; ++i) feed(data[i]);
+}
+
+void Unpacker::feed(char data)
+{
+
+    if (state != State::Checksum) sum ^= data;
+
+    switch(state)
+    {
+        case State::Start:
+        {
+            reset();
+            if (data == PROTOCOL_SIGN) state = State::Size;
+            sum ^= data; // because sum was reset above
+            break;
+        }
+        case State::Size:
+        {
+            r_buffer.size = data - 1; // exclude checksum
+            state = State::Data;
+            break;
+        }
+        case State::Data:
+        {
+            r_buffer.write(&data, 1);
+            if (++count >= r_buffer.size) state = State::Checksum;
+            break;
+        }
+        case State::Checksum:
+        {
+            if (sum == data) _readBuffer.push_back(r_buffer);
+            reset();
+            break;
+        }
+        default:
+        {
+            reset();
+            break;
+        }
+    }
+}
+
+void Unpacker::reset()
+{
+    r_buffer.clear();
+    sum = count = 0;
+    state = State::Start;
+}

lib/Whill/ProtocolUnpacker.h

+#pragma once
+#ifndef WHILL_PROTOCOL_UNPACKER_H
+#define WHILL_PROTOCOL_UNPACKER_H
+
+#include <cstring>
+#include <deque>
+#include "WhillCommon.h"
+
+WHILL_NAMESPACE_BEGIN
+
+class Unpacker
+{
+public:
+
+    const uint8_t available() const { return _readBuffer.size(); }
+    const uint8_t size() const { return _readBuffer.front().size; }
+    const char data(uint8_t i) const { return _readBuffer.front().sbuf[i]; }
+    const char* const data() const { return _readBuffer.front().sbuf; }
+
+    void pop() { _readBuffer.pop_front(); }
+
+    void feed(const char* const data, const uint8_t size);
+    void feed(char data);
+
+    bool checkPowerOn()
+    {
+        if (data(0) == POWER_ON_RESPONSE_BYTE) return true;
+        return false;
+    }
+
+    // DATASET Number
+    uint8_t dataset() { return data(0); }
+
+    // DATASET::SPEED_PROFILE
+    uint8_t speedMode() { return data(1); }
+    uint8_t forwardSpeed() { return data(2); }
+    uint8_t forwardAccel() { return data(3); }
+    uint8_t forwardDecel() { return data(4); }
+    uint8_t reverseSpeed() { return data(5); }
+    uint8_t reverseAccel() { return data(6); }
+    uint8_t reverseDecel() { return data(7); }
+    uint8_t turnSpeed() { return data(8); }
+    uint8_t turnAccel() { return data(9); }
+    uint8_t turnDecel() { return data(10); }
+
+    // DATASET::SENSOR_INFO
+    float sensorAccelX() { return (uint16_t)((float)((data(1) << 8) || (data(2))) * 0.122f); } // [mg]
+    float sensorAccelY() { return (uint16_t)((float)((data(3) << 8) || (data(4))) * 0.122f); } // [mg]
+    float sensorAccelZ() { return (uint16_t)((float)((data(5) << 8) || (data(6))) * 0.122f); } // [mg]
+    float sensorGyroX()  { return (uint16_t)((float)((data(7) << 8) || (data(8))) * 4.375f); } // [mdps]
+    float sensorGyroY()  { return (uint16_t)((float)((data(9) << 8) || (data(10))) * 4.375f); } // [mdps]
+    float sensorGyroZ()  { return (uint16_t)((float)((data(11) << 8) || (data(12))) * 4.375f); } // [mdps]
+    int8_t joystickFront() { return (int8_t)data(13); } // -100 - +100
+    int8_t joystickSide()  { return (int8_t)data(14); } // -100 - +100
+    uint8_t battery() { return data(15); } // 0 - 100[%]
+    int16_t batteryCurrent() { return (int16_t)((data(16) << 8) || (data(17))) * 2; } // [mA], sampling rate is 4Hz
+    float angleRadR() { return (float)(data(18) << 8) || (data(19)) * 0.001f; } // [rad], range is (-PI, PI)
+    float angleRadL() { return (float)(data(20) << 8) || (data(21)) * 0.001f; } // [rad], range is (-PI, PI)
+    float speedR() { return (float)(data(22) << 8) || (data(23)) * 0.004f; } // [km/h]
+    float speedL() { return (float)(data(24) << 8) || (data(25)) * 0.004f; } // [km/h]
+    bool powerState() { return (bool)data(26); }
+    uint8_t speedModeIndicator() { return data(27); }
+    uint8_t errorCode() { return data(28); }
+
+private:
+
+    static const std::size_t READ_BUFFER_SIZE = 32;
+    enum class State { Start, Size, Data, Checksum };
+
+    static const uint8_t POWER_ON_RESPONSE_BYTE = 0x52;
+
+    struct Buffer
+    {
+        uint8_t size {0};
+        char sbuf[READ_BUFFER_SIZE] {0};
+        uint8_t count {0};
+
+        void clear() { size = count = 0; memset(sbuf, 0, READ_BUFFER_SIZE); }
+        void write(char* data, uint8_t size)
+        {
+            memcpy(sbuf + count, data, size);
+            count += size;
+        }
+    };
+
+    void reset();
+
+    State state {State::Start};
+    char sum {0};
+    char count {0};
+
+    Buffer r_buffer;
+    std::deque<Buffer> _readBuffer;
+};
+
+
+WHILL_NAMESPACE_END
+
+#endif // WHILL_PROTOCOL_UNPACKER_H

lib/Whill/Whill.cpp

+#include <Arduino.h>
+#include <cstdint>
+#include "ProtocolPacker.h"
+#include "ProtocolUnpacker.h"
+#include "Whill.h"
+
+using namespace Whill;
+
+bool WhillModelC::open(Stream& device, int baud)
+{
+    return true;
+}
+
+void WhillModelC::attatch(Stream& device)
+{
+    serial = &device;
+}
+
+void WhillModelC::close()
+{
+    // serial->end();
+}
+
+bool WhillModelC::power(bool b)
+{
+    packer.power(b);
+    return this->send();
+}
+
+bool WhillModelC::powerSupply(bool b)
+{
+    packer.powerSupply(b);
+    return this->send();
+}
+
+bool WhillModelC::controlEnable(bool b)
+{
+    packer.controlEnable(b);
+    return this->send();
+}
+
+bool WhillModelC::move(char fb, char lr)
+{
+    Serial.print("Whill move : ");
+    Serial.print((int)fb);
+    Serial.print(" ");
+    Serial.println((int)lr);
+    packer.joystick(fb, lr);
+    return this->send();
+}
+
+bool WhillModelC::moveVel(float x, float y) // [m/s]
+{
+    int8_t fb, lr;
+
+    fb = (int8_t)(y / spd_profile.f_m1 * 100.f);
+    lr = (int8_t)(y / spd_profile.t_m1 * 100.f);
+
+    if (abs(fb) > 100)
+    {
+        Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(fb);
+        fb = (int8_t)constrain(fb, -100, 100);
+    }
+    if (abs(lr) > 100)
+    {
+        Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(lr);
+        lr = (int8_t)constrain(lr, -100, 100);
+    }
+
+    return this->move(fb, lr);
+}
+
+bool WhillModelC::stop()
+{
+    return this->move(0, 0);
+}
+
+bool WhillModelC::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1)
+{
+    f_m1 = constrain(f_m1,  8,  60); // less than 60 is better
+    f_a1 = constrain(f_a1, 10,  90);
+    f_d1 = constrain(f_d1, 40, 160);
+    r_m1 = constrain(r_m1,  8,  60); // less than 60 is better
+    r_a1 = constrain(r_a1, 10,  50);
+    r_d1 = constrain(r_d1, 40,  80);
+    t_m1 = constrain(t_m1,  8,  60); // less than 60 is better
+    t_a1 = constrain(t_a1, 10,  60);
+    t_d1 = constrain(t_d1, 40, 160);
+
+    // save speed as [m/s]
+    spd_profile.f_m1 = f_m1 / 10.f * 1000.f / 60.f / 60.f;
+    spd_profile.r_m1 = r_m1 / 10.f * 1000.f / 60.f / 60.f;
+    spd_profile.t_m1 = t_m1 / 10.f * 1000.f / 60.f / 60.f;
+
+    packer.speed(mode, f_m1, f_a1, f_d1, r_m1, r_a1, r_d1, t_m1, t_a1, t_d1);
+    return this->send();
+}
+
+bool WhillModelC::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
+{
+    packer.streaming(b, interval, n, mode);
+    return this->send();
+}
+
+void WhillModelC::parse()
+{
+    while(serial->available() > 0)
+    {
+        char data = serial->read();
+        unpacker.feed(data);
+    }
+}
+
+bool WhillModelC::send()
+{
+    Serial.print("data : ");
+    for (uint8_t i = 0; i < packer.size(); ++i)
+    {
+        Serial.print((int)packer.data()[i]);
+        Serial.print(" ");
+    }
+    Serial.println();
+
+    int n = serial->write(packer.data(), packer.size());
+    packer.clear(); // TODO: smarter way...
+    return (n == packer.size());
+}
+

lib/Whill/Whill.h

+#pragma once
+#ifndef WHILL_H
+#define WHILL_H
+
+#include <Arduino.h>
+#include "ProtocolPacker.h"
+#include "ProtocolUnpacker.h"
+
+WHILL_NAMESPACE_BEGIN
+
+class WhillModelC
+{
+    struct SpeedProfile
+    {
+        float f_m1;
+        float r_m1;
+        float t_m1;
+    };
+
+public:
+
+    bool open(Stream& device, int baud);
+    void attatch(Stream& device);
+    void close();
+
+    bool power(bool b);
+    bool powerSupply(bool b);
+    bool controlEnable(bool b);
+
+    bool move(char fb, char lr);
+    bool moveVel(float x, float y);
+    bool stop();
+    bool speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1);
+
+    bool streaming(bool b, uint16_t interval = 1000, DATASET n = DATASET::SENSOR_INFO, SPEEDMODE mode = SPEEDMODE::RS232C);
+    void parse();
+
+    const uint8_t available() const { return unpacker.available(); }
+    const uint8_t size() const { return unpacker.size(); }
+    const char data(char i) const { return unpacker.data(i); }
+    const char* const data() const { return unpacker.data(); }
+    void pop() { unpacker.pop(); }
+
+    // DATASET Number
+    uint8_t dataset() { return unpacker.dataset(); }
+
+    // DATASET::SPEED_PROFILE
+    uint8_t speedMode() { return unpacker.speedMode(); }
+    uint8_t forwardSpeed() { return unpacker.forwardSpeed(); }
+    uint8_t forwardAccel() { return unpacker.forwardAccel(); }
+    uint8_t forwardDecel() { return unpacker.forwardDecel(); }
+    uint8_t reverseSpeed() { return unpacker.reverseSpeed(); }
+    uint8_t reverseAccel() { return unpacker.reverseAccel(); }
+    uint8_t reverseDecel() { return unpacker.reverseDecel(); }
+    uint8_t turnSpeed() { return unpacker.turnSpeed(); }
+    uint8_t turnAccel() { return unpacker.turnAccel(); }
+    uint8_t turnDecel() { return unpacker.turnDecel(); }
+
+    // DATASET::SENSOR_INFO
+    uint16_t sensorAccelX() { return unpacker.sensorAccelX(); } // [mg]
+    uint16_t sensorAccelY() { return unpacker.sensorAccelY(); } // [mg]
+    uint16_t sensorAccelZ() { return unpacker.sensorAccelZ(); } // [mg]
+    uint16_t sensorGyroX()  { return unpacker.sensorGyroX(); } // [mdps]
+    uint16_t sensorGyroY()  { return unpacker.sensorGyroY(); } // [mdps]
+    uint16_t sensorGyroZ()  { return unpacker.sensorGyroZ(); } // [mdps]
+    int8_t joystickFront() { return unpacker.joystickFront(); } // -100 - +100
+    int8_t joystickSide()  { return unpacker.joystickSide(); } // -100 - +100
+    uint8_t battery() { return unpacker.battery(); } // 0 - 100[%]
+    int16_t batteryCurrent() { return unpacker.batteryCurrent(); } // [mA], sampling rate is 4Hz
+    float angleRadR() { return unpacker.angleRadR(); } // [rad], range is (-PI, PI)
+    float angleRadL() { return unpacker.angleRadL(); } // [rad], range is (-PI, PI)
+    float speedR() { return unpacker.speedR(); } // [km/h]
+    float speedL() { return unpacker.speedL(); } // [km/h]
+    bool powerState() { return unpacker.powerState(); }
+    uint8_t speedModeIndicator() { return unpacker.speedModeIndicator(); }
+    uint8_t errorCode() { return unpacker.errorCode(); }
+
+private:
+
+    bool send();
+
+    Stream* serial;
+    Packer packer;
+    Unpacker unpacker;
+    SpeedProfile spd_profile;
+};
+
+WHILL_NAMESPACE_END
+
+using WhillC = Whill::WhillModelC;
+
+#endif // WHILL_H

lib/Whill/WhillCommon.h

+#pragma once
+#ifndef WHILL_COMMON_H
+#define WHILL_COMMON_H
+
+#define WHILL_NAMESPACE_BEGIN namespace Whill {
+#define WHILL_NAMESPACE_END   } // namespace Whill
+
+WHILL_NAMESPACE_BEGIN
+
+enum class DATASET { SPEED_PROFILE, SENSOR_INFO };
+enum class SPEEDMODE { JOYSTICK_1, JOYSTICK_2, JOYSTICK_3, JOYSTICK_4, RS232C, IPHONE };
+const char PROTOCOL_SIGN {(char)0xAF};
+
+WHILL_NAMESPACE_END
+
+#endif // WHILL_COMMON_H

lib/Whill/WhillOmni.cpp

+#include <Arduino.h>
+#include <cstdint>
+#include "ProtocolPacker.h"
+#include "ProtocolUnpacker.h"
+#include "WhillOmni.h"
+
+using namespace Whill;
+
+bool WhillOmni::open(Stream& device_l, Stream& device_r, int baud)
+{
+    if(!whills[(uint8_t)LR::L].open(device_l, baud)) return false;
+    if(!whills[(uint8_t)LR::R].open(device_r, baud)) return false;
+    return true;
+}
+
+void WhillOmni::attatch(Stream& device_l, Stream& device_r)
+{
+    whills[(uint8_t)LR::L].attatch(device_l);
+    whills[(uint8_t)LR::R].attatch(device_r);
+}
+
+void WhillOmni::close()
+{
+    for (auto& w : whills) w.close();
+}
+
+bool WhillOmni::power(bool b)
+{
+    bool r = true;
+    for (auto& w : whills) r &= w.power(b);
+    return r;
+}
+
+bool WhillOmni::powerSupply(bool b)
+{
+    bool r = true;
+    for (auto& w : whills) r &= w.powerSupply(b);
+    return r;
+}
+
+bool WhillOmni::controlEnable(bool b)
+{
+    bool r = true;
+    for (auto& w : whills) r &= w.controlEnable(b);
+    return r;
+}
+
+bool WhillOmni::move(float x, float y, float r)
+{
+#if 0
+
+    float vl[2], vr[2]; // virtual velocity of whill motors
+    WhillXY wxy[2]; // virtual WHILL's XY velocity
+
+    // convert joystick x y r -> motors
+    vr[1] =  x * C + y * C + RADIUS * r;
+    vl[0] = -x * C + y * C + RADIUS * r;
+    vr[0] = -x * C - y * C + RADIUS * r;
+    vl[1] =  x * C - y * C + RADIUS * r;
+
+    // convert motors -> WHILL's XY
+    wxy[0].x = vr[0] + vl[0];
+    wxy[0].y = (vl[0] - vr[0]) / 2.f;
+    // wxy[0].x *= 25.f; // TODO: ??
+    // wxy[0].y *= 25.f; // TODO: ??
+
+    wxy[1].x = vr[1] + vl[1];
+    wxy[1].y = (vl[1] - vr[1]) / 2.f;
+    // wxy[1].x *= 25.f; // TODO: ??
+    // wxy[1].y *= 25.f; // TODO: ??
+
+    Serial.println("move :");
+    bool ret = true;
+    for (int i = 0; i < NUM_WHILLS; ++i)
+    {
+        Serial.print((int)wxy[i].y); Serial.print(" "); Serial.println((int)wxy[i].x);
+        ret &= whills[i].move((int8_t)wxy[i].y, (int8_t)wxy[i].x);
+    }
+    return ret;
+
+#else
+
+    model.updateJacobian(0.0);
+
+    // 1: L = 1R, 2: T = 0L, 3: R = 0R, 4: B = 1L
+    VectorXd v_omni;
+    VectorXd w_wheel;
+    VectorXd whill[2];
+
+    v_omni << x, y, r; // [m/s], [m/s], [rad/s](??)
+    w_wheel = model.Jacobian_inv * v_omni;
+
+    // convert motors -> WHILL's XY
+    whill[0].x() = w_wheel[1] + w_wheel[2];
+    whill[0].y() = (w_wheel[1] - w_wheel[2]) / 2.f;
+
+    whill[1].x() = w_wheel[3] + w_wheel[0];
+    whill[1].y() = (w_wheel[3] - w_wheel[0]) / 2.f;
+
+    Serial.println("move :");
+    bool ret = true;
+    for (int i = 0; i < NUM_WHILLS; ++i)
+    {
+        Serial.print((int)whill[i].y()); Serial.print(" "); Serial.println((int)whill[i].x());
+        ret &= whills[i].moveVel(whill[i].y(), whill[i].x());
+    }
+    return ret;
+
+#endif
+}
+
+bool WhillOmni::stop()
+{
+    bool r = true;
+    for (auto& w : whills) r &= w.stop();
+    return r;
+}
+
+bool WhillOmni::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1)
+{
+    bool r = true;
+    for (auto& w : whills) r &= w.speed(mode, f_m1, f_a1, f_d1, r_m1, r_a1, r_d1, t_m1, t_a1, t_d1);
+    return r;
+}
+
+bool WhillOmni::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
+{
+    bool r = true;
+    for (auto& w : whills) r &= w.streaming(b, interval, n, mode);
+    return r;
+}
+
+void WhillOmni::parse()
+{
+    for (auto& w : whills) w.parse();
+}
+
+
+
+
+
+
+
+

lib/Whill/WhillOmni.h

+#pragma once
+#ifndef WHILL_OMNI_H
+#define WHILL_OMNI_H
+
+#include <array>
+#include "Whill.h"
+#include "OmniModel.h"
+
+WHILL_NAMESPACE_BEGIN
+
+using namespace Eigen;
+
+class WhillOmni
+{
+public:
+
+    enum class LR { L, R };
+
+    bool open(Stream& device_l, Stream& device_r, int baud);
+    void attatch(Stream& device_l, Stream& device_r);
+    void close();
+
+    bool power(bool b);
+    bool powerSupply(bool b);
+    bool controlEnable(bool b);
+
+    bool move(float x, float y, float r); // [mm/s]
+    bool stop();
+    bool speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1);
+
+    bool streaming(bool b, uint16_t interval = 1000, DATASET n = DATASET::SENSOR_INFO, SPEEDMODE mode = SPEEDMODE::RS232C);
+    void parse();
+
+    const uint8_t available(LR lr) const { return whills[(uint8_t)lr].available(); }
+    const uint8_t size(LR lr) const { return whills[(uint8_t)lr].size(); }
+    const char data(LR lr, char i) const { return whills[(uint8_t)lr].data(i); }
+    const char* const data(LR lr) const { return whills[(uint8_t)lr].data(); }
+    void pop(LR lr) { whills[(uint8_t)lr].pop(); }
+    void pop() { for (auto& w : whills) w.pop(); }
+
+    // DATASET Number
+    uint8_t dataset(LR lr) { return whills[(uint8_t)lr].dataset(); }
+
+    // DATASET::SPEED_PROFILE
+    uint8_t speedMode(LR lr) { return whills[(uint8_t)lr].speedMode(); }
+    uint8_t forwardSpeed(LR lr) { return whills[(uint8_t)lr].forwardSpeed(); }
+    uint8_t forwardAccel(LR lr) { return whills[(uint8_t)lr].forwardAccel(); }
+    uint8_t forwardDecel(LR lr) { return whills[(uint8_t)lr].forwardDecel(); }
+    uint8_t reverseSpeed(LR lr) { return whills[(uint8_t)lr].reverseSpeed(); }
+    uint8_t reverseAccel(LR lr) { return whills[(uint8_t)lr].reverseAccel(); }
+    uint8_t reverseDecel(LR lr) { return whills[(uint8_t)lr].reverseDecel(); }
+    uint8_t turnSpeed(LR lr) { return whills[(uint8_t)lr].turnSpeed(); }
+    uint8_t turnAccel(LR lr) { return whills[(uint8_t)lr].turnAccel(); }
+    uint8_t turnDecel(LR lr) { return whills[(uint8_t)lr].turnDecel(); }
+
+    // DATASET::SENSOR_INFO
+    uint16_t sensorAccelX(LR lr) { return whills[(uint8_t)lr].sensorAccelX(); } // [mg]
+    uint16_t sensorAccelY(LR lr) { return whills[(uint8_t)lr].sensorAccelY(); } // [mg]
+    uint16_t sensorAccelZ(LR lr) { return whills[(uint8_t)lr].sensorAccelZ(); } // [mg]
+    uint16_t sensorGyroX(LR lr)  { return whills[(uint8_t)lr].sensorGyroX(); } // [mdps]
+    uint16_t sensorGyroY(LR lr)  { return whills[(uint8_t)lr].sensorGyroY(); } // [mdps]
+    uint16_t sensorGyroZ(LR lr)  { return whills[(uint8_t)lr].sensorGyroZ(); } // [mdps]
+    int8_t joystickFront(LR lr) { return whills[(uint8_t)lr].joystickFront(); } // -100 - +100
+    int8_t joystickSide(LR lr)  { return whills[(uint8_t)lr].joystickSide(); } // -100 - +100
+    uint8_t battery(LR lr) { return whills[(uint8_t)lr].battery(); } // 0 - 100[%]
+    int16_t batteryCurrent(LR lr) { return whills[(uint8_t)lr].batteryCurrent(); } // [mA], sampling rate is 4Hz
+    float angleRadR(LR lr) { return whills[(uint8_t)lr].angleRadR(); } // [rad], range is (-PI, PI)
+    float angleRadL(LR lr) { return whills[(uint8_t)lr].angleRadL(); } // [rad], range is (-PI, PI)
+    float speedR(LR lr) { return whills[(uint8_t)lr].speedR(); } // [km/h]
+    float speedL(LR lr) { return whills[(uint8_t)lr].speedL(); } // [km/h]
+    bool powerState(LR lr) { return whills[(uint8_t)lr].powerState(); }
+    uint8_t speedModeIndicator(LR lr) { return whills[(uint8_t)lr].speedModeIndicator(); }
+    uint8_t errorCode(LR lr) { return whills[(uint8_t)lr].errorCode(); }
+
+private:
+
+    static constexpr int NUM_WHILLS {2};
+    static constexpr float C {0.71}; // trad??
+    static constexpr float RADIUS {1.0}; // from center to wheel
+
+    struct WhillXY { float x; float y; };
+
+    std::array<WhillModelC, NUM_WHILLS> whills;
+    Work::Model::Omni <Vector3d, Vector4d, 3, 4> model;
+};
+
+WHILL_NAMESPACE_END
+
+#endif // WHILL_OMNI_H

lib/Whill/WorkSpaceModel.h

+#pragma once
+
+namespace Work
+{
+    using namespace Eigen;
+
+    namespace Model
+    {
+        template <typename WorkType, typename JointType, size_t DmsW, size_t DmsJ, typename ControlType = VectorXd, int ControlDim = 6>
+        struct Base
+        {
+            // !!!!! required to use this class as pointers !!!!!
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+			virtual ~Base() {}
+            virtual void update() {};
+            virtual void draw() {}
+
+            virtual WorkType estimateGravity()
+            {
+                // return dis.gravity;
+                return WorkType::Zero();
+            }
+            virtual WorkType estimateFriction(const WorkType& vel)
+            {
+                // return dis.friction;
+                return WorkType::Zero();
+            }
+            virtual WorkType estimateCoriolis()
+            {
+                // return dis.coriolis;
+                return WorkType::Zero();
+            }
+            virtual WorkType estimateLimitation(const WorkType& acc)
+            {
+                return WorkType();
+            }
+            virtual WorkType fromAccelToForce(const WorkType& ddx)
+            {
+                WorkType force = Mn * ddx;
+                return force;
+            }
+            virtual WorkType fromForceToAccel(const WorkType& force)
+            {
+                WorkType acc = M_inv * force;
+                return acc;
+            }
+            virtual JointType fromWorkAccToJointAcc(const WorkType& ddx)
+            {
+                JointType ddq = Jacobian_inv * ddx;
+                return ddq;
+            }
+            virtual WorkType fromJointVelToWorkVel(const JointType& dq)
+            {
+                WorkType dx = Jacobian * dq;
+                return dx;
+            }
+            virtual JointType fromJointVelToNullSpaceDumping(const JointType& dq)
+            {
+                JointType dq_dump = Jacobian_null * dq;
+                return dq_dump;
+            }
+
+            MatrixXd Jacobian_local {DmsW, DmsJ};
+            MatrixXd Jacobian {DmsW, DmsJ};
+            MatrixXd Jacobian_inv {DmsJ, DmsW};
+            MatrixXd Jacobian_tra {DmsJ, DmsW};
+            MatrixXd Jacobian_null {DmsJ, DmsJ};
+            MatrixXd rotation {DmsW, DmsW};
+
+            virtual void updateJacobian(const double& rad) = 0;
+
+            MatrixXd M_inv {DmsW, DmsW};
+            MatrixXd Mn {DmsW, DmsW};
+            MatrixXd Mn_inv {DmsW, DmsW};
+
+            // void setDisturbance(SimulateVariables::Disturbance<WorkType> d) { dis = d; }
+
+            // void estimateAcceleration(const ControlVariables::Reference<WorkType>& ref)
+            // {
+            //     est.force = ref.force - (dis.force + dis.gravity + dis.friction + dis.coriolis);
+            //     est.acc = fromForceToAccel(est.force);
+            // }
+
+            // void estimateResponse(const double& interval)
+            // {
+            //     est.vel = ddx_to_dx.get(est.acc, interval);
+            //     est.pos = dx_to_x.get(est.vel, interval);
+            // }
+
+            // void setAccelerationResponse(const WorkType& acc) { est.acc = acc; }
+
+            // const SimulateVariables::Estimation<WorkType>& getEstimatedResponse() const { return est; }
+            // const WorkType& getEstimatedPosition() const { return est.pos; }
+            // const WorkType& getEstimatedVelocity() const { return est.vel; }
+
+            void selectCartesianCoordinate()
+            {
+                // TODO: variable template is better!!
+            }
+
+            void setControlSelectMatrix(const MatrixXd& mat)
+            {
+                mat_euler_to_min = mat;
+                mat_min_to_euler = mat.transpose();
+            }
+
+
+            const MatrixXd& getControlVariableMinToEulerMatrix() { return mat_min_to_euler; }
+            const MatrixXd& getControlVariableEulerToMinMatrix() { return mat_euler_to_min; }
+
+            VectorXd getControlVariableEulerToQuat(const VectorXd& v) const
+            {
+                assert(v.size() == 6);
+
+                Quaternionf q;
+                q = AngleAxisf(v(3), Vector3f::UnitX()) *
+                    AngleAxisf(v(4), Vector3f::UnitY()) *
+                    AngleAxisf(v(5), Vector3f::UnitZ());
+
+                VectorXd p {7};
+                p << v(0), v(1), v(2), q.x(), q.y(), q.z(), q.w();
+
+                return p;
+            }
+
+            VectorXd getControlVariableQuatToEuler(const VectorXd& v) const
+            {
+                assert(v.size() == 7);
+
+                Quaternionf q(v(3), v(4), v(5), v(6));
+                Vector3f e = q.toRotationMatrix().eulerAngles(0, 1, 2);
+                VectorXd p {6};
+                p << v(0), v(1), v(2), e(0), e(1), e(2);
+
+                return p;
+            }
+
+            // void setPosition(const WorkType& pos) { dx_to_x.reset(pos); }
+
+        private:
+
+            // SimulateVariables::Disturbance<WorkType> dis;
+            // SimulateVariables::Estimation<WorkType> est;
+            // Calculus::Integral<WorkType, DmsW> dx_to_x;
+            // Calculus::Integral<WorkType, DmsW> ddx_to_dx;
+
+            MatrixXd mat_min_to_euler {DmsW, ControlDim};
+            MatrixXd mat_euler_to_min {ControlDim, DmsW};
+        };
+    }
+}

lib/Whill/test/01_datastreaming/01_datastreaming.ino

+#include <Arduino.h>
+#include <Whill.h>
+
+using namespace Whill;
+
+WhillC whill;
+
+void printSpeedProfile()
+{
+    Serial.println("SPEED_PROFILE");
+    Serial.print("dataset : "); Serial.println(whill.dataset());
+    Serial.print("mode    : "); Serial.println(whill.speedMode());
+    Serial.print("fwd spd : "); Serial.println(whill.forwardSpeed());
+    Serial.print("fwd acc : "); Serial.println(whill.forwardAccel());
+    Serial.print("fwd dec : "); Serial.println(whill.forwardDecel());
+    Serial.print("bwd spd : "); Serial.println(whill.reverseSpeed());
+    Serial.print("bwd acc : "); Serial.println(whill.reverseAccel());
+    Serial.print("bwd dec : "); Serial.println(whill.reverseDecel());
+    Serial.print("trn spd : "); Serial.println(whill.turnSpeed());
+    Serial.print("trn acc : "); Serial.println(whill.turnAccel());
+    Serial.print("trn dec : "); Serial.println(whill.turnDecel());
+}
+
+void printSensorInfo()
+{
+    Serial.print("SENSOR_INFO");
+    Serial.print("sensor acc x : "); Serial.println(whill.sensorAccelX());
+    Serial.print("sensor acc y : "); Serial.println(whill.sensorAccelY());
+    Serial.print("sensor acc z : "); Serial.println(whill.sensorAccelZ());
+    Serial.print("sensor gyr x : "); Serial.println(whill.sensorGyroX());
+    Serial.print("sensor gyr y : "); Serial.println(whill.sensorGyroY());
+    Serial.print("sensor gyr z : "); Serial.println(whill.sensorGyroZ());
+    Serial.print("joystick fb  : "); Serial.println(whill.joystickFront());
+    Serial.print("joystick lr  : "); Serial.println(whill.joystickSide());
+    Serial.print("battery      : "); Serial.println(whill.battery());
+    Serial.print("battery crnt : "); Serial.println(whill.batteryCurrent());
+    Serial.print("angle rad r  : "); Serial.println(whill.angleRadR());
+    Serial.print("angle rad l  : "); Serial.println(whill.angleRadL());
+    Serial.print("speed r      : "); Serial.println(whill.speedR());
+    Serial.print("speed l      : "); Serial.println(whill.speedL());
+    Serial.print("power state  : "); Serial.println(whill.powerState());
+    Serial.print("speed mode   : "); Serial.println(whill.speedModeIndicator());
+    Serial.print("error code   : "); Serial.println(whill.errorCode());
+}
+
+void parseReply()
+{
+    whill.parse();
+
+    Serial.print("available : "); Serial.println(whill.available());
+
+    while(whill.available())
+    {
+        Serial.println("[data received]");
+        Serial.print("size  "); Serial.println(whill.size());
+        Serial.print("data  ");
+        for (uint8_t i = 0; i < whill.size(); ++i)
+        {
+            Serial.print(whill.data(i)); Serial.print(" ");
+        }
+        Serial.println(); Serial.println();
+
+        if      (whill.dataset() == 0) printSpeedProfile();
+        else if (whill.dataset() == 1) printSensorInfo();
+        else    Serial.println("invalid dataset number");
+
+        whill.pop();
+    }
+}
+
+
+void setup()
+{
+    Serial1.begin(38400, SERIAL_8N2);
+    whill.attatch(Serial1);
+
+    delay(1000);
+
+    // first, check speed profile
+    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
+    delay(1500);
+    parseReply();
+
+    Serial.println("start streaming");
+
+    // second, stream sensor data
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    delay(1500);
+
+    int count = 0;
+    while(count < 10)
+    {
+        Serial.print("in while : ");
+        Serial.println(count);
+        parseReply();
+        delay(1000);
+        ++count;
+    }
+
+    whill.streaming(false);
+    delay(1000);
+    parseReply(); // maybe nothing
+
+    Serial.println("end streaming test");
+}
+
+void loop()
+{
+}
+

lib/Whill/test/02_power_on_off/02_power_on_off.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/Whill.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillC whill;
+
+void printSpeedProfile()
+{
+    cout << "SPEED_PROFILE" << endl;
+    cout << "dataset : " << (int)whill.dataset() << endl;
+    cout << "mode    : " << (int)whill.speedMode() << endl;
+    cout << "fwd spd : " << (int)whill.forwardSpeed() << endl;
+    cout << "fwd acc : " << (int)whill.forwardAccel() << endl;
+    cout << "fwd dec : " << (int)whill.forwardDecel() << endl;
+    cout << "bwd spd : " << (int)whill.reverseSpeed() << endl;
+    cout << "bwd acc : " << (int)whill.reverseAccel() << endl;
+    cout << "bwd dec : " << (int)whill.reverseDecel() << endl;
+    cout << "trn spd : " << (int)whill.turnSpeed() << endl;
+    cout << "trn acc : " << (int)whill.turnAccel() << endl;
+    cout << "trn dec : " << (int)whill.turnDecel() << endl;
+}
+
+void printSensorInfo()
+{
+    cout << "SENSOR_INFO" << endl;
+    cout << "sensor acc x : " << (float)whill.sensorAccelX() << endl;
+    cout << "sensor acc y : " << (float)whill.sensorAccelY() << endl;
+    cout << "sensor acc z : " << (float)whill.sensorAccelZ() << endl;
+    cout << "sensor gyr x : " << (float)whill.sensorGyroX() << endl;
+    cout << "sensor gyr y : " << (float)whill.sensorGyroY() << endl;
+    cout << "sensor gyr z : " << (float)whill.sensorGyroZ() << endl;
+    cout << "joystick fb  : " << (int)whill.joystickFront() << endl;
+    cout << "joystick lr  : " << (int)whill.joystickSide() << endl;
+    cout << "battery      : " << (int)whill.battery() << endl;
+    cout << "battery crnt : " << (int)whill.batteryCurrent() << endl;
+    cout << "angle rad r  : " << (float)whill.angleRadR() << endl;
+    cout << "angle rad l  : " << (float)whill.angleRadL() << endl;
+    cout << "speed r      : " << (float)whill.speedR() << endl;
+    cout << "speed l      : " << (float)whill.speedL() << endl;
+    cout << "power state  : " << (int)whill.powerState() << endl;
+    cout << "speed mode   : " << (int)whill.speedModeIndicator() << endl;
+    cout << "error code   : " << (int)whill.errorCode() << endl;
+}
+
+void parseReply()
+{
+    whill.parse();
+
+    cout << "available : " << (int)whill.available() << endl;
+
+    while(whill.available())
+    {
+        cout << "[data received]" << endl;
+        cout << "size  " << to_string((int)whill.size()) << endl;
+        cout << "data  ";
+        for (uint8_t i = 0; i < whill.size(); ++i)
+            cout << hex << setw(2) << setfill('0') << (int)whill.data(i) << " ";
+        cout << dec << endl << endl;
+
+        if      (whill.dataset() == 0) printSpeedProfile();
+        else if (whill.dataset() == 1) printSensorInfo();
+        else    cout << "invalid dataset number" << endl;
+
+        whill.pop();
+    }
+}
+
+int main()
+{
+    if (whill.open("/dev/ttyUSB0", 38400)) // USB
+    // if (whill.open("/dev/ttyUSB1", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    // first, check speed profile
+    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
+    sleep(1.5);
+    parseReply();
+
+    // second, stream sensor data
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    sleep(1.5);
+    parseReply();
+
+    whill.streaming(false);
+
+    cout << "power off and wait for more than 5 sec..." << endl;
+
+    whill.power(false);
+    sleep(6);
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    sleep(1.5);
+    parseReply();
+
+    cout << "start" << endl;
+
+    while (1)
+    {
+        whill.power(true);
+        sleep(1.5); // wait for reply (max 1s)
+        whill.parse();
+        if (whill.available())
+        {
+            if (whill.checkPowerOn()) break;
+        }
+        cout << "re-issue power on command..." << endl;
+    }
+
+    cout << "whill power on" << endl;
+
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    sleep(1.5);
+    parseReply();
+
+    whill.power(false);
+
+    sleep(1);
+    parseReply();
+
+    whill.streaming(false);
+
+    cout << "end test" << endl;
+    whill.close();
+}
+
+
+
+

lib/Whill/test/03_power_supply_on_off/03_power_supply_on_off.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/Whill.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillC whill;
+
+void printSpeedProfile()
+{
+    cout << "SPEED_PROFILE" << endl;
+    cout << "dataset : " << (int)whill.dataset() << endl;
+    cout << "mode    : " << (int)whill.speedMode() << endl;
+    cout << "fwd spd : " << (int)whill.forwardSpeed() << endl;
+    cout << "fwd acc : " << (int)whill.forwardAccel() << endl;
+    cout << "fwd dec : " << (int)whill.forwardDecel() << endl;
+    cout << "bwd spd : " << (int)whill.reverseSpeed() << endl;
+    cout << "bwd acc : " << (int)whill.reverseAccel() << endl;
+    cout << "bwd dec : " << (int)whill.reverseDecel() << endl;
+    cout << "trn spd : " << (int)whill.turnSpeed() << endl;
+    cout << "trn acc : " << (int)whill.turnAccel() << endl;
+    cout << "trn dec : " << (int)whill.turnDecel() << endl;
+}
+
+void printSensorInfo()
+{
+    cout << "SENSOR_INFO" << endl;
+    cout << "sensor acc x : " << (float)whill.sensorAccelX() << endl;
+    cout << "sensor acc y : " << (float)whill.sensorAccelY() << endl;
+    cout << "sensor acc z : " << (float)whill.sensorAccelZ() << endl;
+    cout << "sensor gyr x : " << (float)whill.sensorGyroX() << endl;
+    cout << "sensor gyr y : " << (float)whill.sensorGyroY() << endl;
+    cout << "sensor gyr z : " << (float)whill.sensorGyroZ() << endl;
+    cout << "joystick fb  : " << (int)whill.joystickFront() << endl;
+    cout << "joystick lr  : " << (int)whill.joystickSide() << endl;
+    cout << "battery      : " << (int)whill.battery() << endl;
+    cout << "battery crnt : " << (int)whill.batteryCurrent() << endl;
+    cout << "angle rad r  : " << (float)whill.angleRadR() << endl;
+    cout << "angle rad l  : " << (float)whill.angleRadL() << endl;
+    cout << "speed r      : " << (float)whill.speedR() << endl;
+    cout << "speed l      : " << (float)whill.speedL() << endl;
+    cout << "power state  : " << (int)whill.powerState() << endl;
+    cout << "speed mode   : " << (int)whill.speedModeIndicator() << endl;
+    cout << "error code   : " << (int)whill.errorCode() << endl;
+}
+
+void parseReply()
+{
+    whill.parse();
+
+    cout << "available : " << (int)whill.available() << endl;
+
+    while(whill.available())
+    {
+        cout << "[data received]" << endl;
+        cout << "size  " << to_string((int)whill.size()) << endl;
+        cout << "data  ";
+        for (uint8_t i = 0; i < whill.size(); ++i)
+            cout << hex << setw(2) << setfill('0') << (int)whill.data(i) << " ";
+        cout << dec << endl << endl;
+
+        if      (whill.dataset() == 0) printSpeedProfile();
+        else if (whill.dataset() == 1) printSensorInfo();
+        else    cout << "invalid dataset number" << endl;
+
+        whill.pop();
+    }
+}
+
+int main()
+{
+    if (whill.open("/dev/ttyUSB0", 38400)) // USB
+    // if (whill.open("/dev/ttyUSB1", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    // first, check speed profile
+    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
+    sleep(1.5);
+    parseReply();
+
+    // second, stream sensor data
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    sleep(1.5);
+    parseReply();
+
+    whill.streaming(false);
+
+    cout << "power on power supply" << endl;
+
+    whill.powerSupply(true);
+    sleep(1);
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    sleep(1.5);
+    parseReply();
+
+    cout << "power off power supply" << endl;
+
+    whill.powerSupply(false);
+    sleep(1);
+    parseReply();
+
+    whill.streaming(false);
+
+    cout << "end test" << endl;
+    whill.close();
+}

lib/Whill/test/04_speedprofile/04_speedprofile.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/Whill.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillC whill;
+
+void printSpeedProfile()
+{
+    cout << "SPEED_PROFILE" << endl;
+    cout << "dataset : " << (int)whill.dataset() << endl;
+    cout << "mode    : " << (int)whill.speedMode() << endl;
+    cout << "fwd spd : " << (int)whill.forwardSpeed() << endl;
+    cout << "fwd acc : " << (int)whill.forwardAccel() << endl;
+    cout << "fwd dec : " << (int)whill.forwardDecel() << endl;
+    cout << "bwd spd : " << (int)whill.reverseSpeed() << endl;
+    cout << "bwd acc : " << (int)whill.reverseAccel() << endl;
+    cout << "bwd dec : " << (int)whill.reverseDecel() << endl;
+    cout << "trn spd : " << (int)whill.turnSpeed() << endl;
+    cout << "trn acc : " << (int)whill.turnAccel() << endl;
+    cout << "trn dec : " << (int)whill.turnDecel() << endl;
+}
+
+void printSensorInfo()
+{
+    cout << "SENSOR_INFO" << endl;
+    cout << "sensor acc x : " << (float)whill.sensorAccelX() << endl;
+    cout << "sensor acc y : " << (float)whill.sensorAccelY() << endl;
+    cout << "sensor acc z : " << (float)whill.sensorAccelZ() << endl;
+    cout << "sensor gyr x : " << (float)whill.sensorGyroX() << endl;
+    cout << "sensor gyr y : " << (float)whill.sensorGyroY() << endl;
+    cout << "sensor gyr z : " << (float)whill.sensorGyroZ() << endl;
+    cout << "joystick fb  : " << (int)whill.joystickFront() << endl;
+    cout << "joystick lr  : " << (int)whill.joystickSide() << endl;
+    cout << "battery      : " << (int)whill.battery() << endl;
+    cout << "battery crnt : " << (int)whill.batteryCurrent() << endl;
+    cout << "angle rad r  : " << (float)whill.angleRadR() << endl;
+    cout << "angle rad l  : " << (float)whill.angleRadL() << endl;
+    cout << "speed r      : " << (float)whill.speedR() << endl;
+    cout << "speed l      : " << (float)whill.speedL() << endl;
+    cout << "power state  : " << (int)whill.powerState() << endl;
+    cout << "speed mode   : " << (int)whill.speedModeIndicator() << endl;
+    cout << "error code   : " << (int)whill.errorCode() << endl;
+}
+
+void parseReply()
+{
+    whill.parse();
+
+    cout << "available : " << (int)whill.available() << endl;
+
+    while(whill.available())
+    {
+        cout << "[data received]" << endl;
+        cout << "size  " << to_string((int)whill.size()) << endl;
+        cout << "data  ";
+        for (uint8_t i = 0; i < whill.size(); ++i)
+            cout << hex << setw(2) << setfill('0') << (int)whill.data(i) << " ";
+        cout << dec << endl << endl;
+
+        if      (whill.dataset() == 0) printSpeedProfile();
+        else if (whill.dataset() == 1) printSensorInfo();
+        else    cout << "invalid dataset number" << endl;
+
+        whill.pop();
+    }
+}
+
+int main()
+{
+    // if (whill.open("/dev/ttyUSB0", 38400)) // USB
+    if (whill.open("/dev/ttyUSB1", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    cout << endl << "----------------------------------------" << endl;
+    cout << "first, check current speed profile" << endl;
+
+    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
+    sleep(1.5);
+    parseReply();
+
+    cout << endl << "----------------------------------------" << endl;
+    cout << "second, change speed profile to MAX speed" << endl;
+
+    whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 30, 50, 90, 35, 60, 160);
+    // whill.speed(SPEEDMODE::RS232C, 59, 89, 159, 29, 49, 89, 34, 59, 159);
+    // whill.speed(SPEEDMODE::RS232C, 50, 50, 60, 60, 60, 60, 60, 60, 60); // bug??
+    sleep(1.5);
+    parseReply();
+
+    cout << endl << "----------------------------------------" << endl;
+    cout << "third, change speed profile to MIN speed" << endl;
+
+    whill.speed(SPEEDMODE::RS232C, 8, 10, 40, 8, 10, 40, 8, 10, 40);
+    sleep(1.5);
+    parseReply();
+
+    cout << endl << "----------------------------------------" << endl;
+    cout << "fourth, back to standard speed profile" << endl;
+
+    whill.speed(SPEEDMODE::RS232C, 40, 40, 40, 40, 40, 40, 40, 40, 40);
+    sleep(1.5);
+    parseReply();
+
+    cout << endl << "----------------------------------------" << endl;
+    cout << "finally, disable streaming" << endl;
+
+    whill.streaming(false);
+    sleep(1.5);
+    parseReply(); // maybe nothing
+
+    cout << "end test" << endl;
+    whill.close();
+}
+
+
+
+

lib/Whill/test/05_control_joystick/05_control_joystick.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/Whill.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillC whill;
+
+void parseReply()
+{
+    whill.parse();
+
+    cout << "available : " << (int)whill.available() << endl;
+
+    while(whill.available())
+    {
+        cout << "[data received]" << endl;
+        cout << "size  " << to_string((int)whill.size()) << endl;
+        cout << "data  ";
+        for (uint8_t i = 0; i < whill.size(); ++i)
+            cout << hex << setw(2) << setfill('0') << (int)whill.data(i) << " ";
+        cout << dec << endl << endl;
+        whill.pop();
+    }
+}
+
+int main()
+{
+    if (whill.open("/dev/ttyUSB0", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    sleep(1);
+    parseReply();
+
+    whill.controlEnable(false);
+
+    sleep(1);
+    parseReply();
+
+    whill.close();
+}

lib/Whill/test/06_control_rs232c/06_control_rs232c.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/Whill.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillC whill;
+
+void parseReply()
+{
+    whill.parse();
+
+    cout << "available : " << (int)whill.available() << endl;
+
+    while(whill.available())
+    {
+        cout << "[data received]" << endl;
+        cout << "size  " << to_string((int)whill.size()) << endl;
+        cout << "data  ";
+        for (uint8_t i = 0; i < whill.size(); ++i)
+            cout << hex << setw(2) << setfill('0') << (int)whill.data(i) << " ";
+        cout << dec << endl << endl;
+        whill.pop();
+    }
+}
+
+int main()
+{
+    if (whill.open("/dev/ttyUSB0", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    sleep(1);
+    parseReply();
+
+    int count = 0;
+    while(count < 5000)
+    {
+        whill.move(20, 0);
+        sleep(0.1);
+        count++;
+    }
+    count = 0;
+    while(count < 5000)
+    {
+        whill.move(-20, 0);
+        sleep(0.1);
+        count++;
+    }
+    count = 0;
+    while(count < 5000)
+    {
+        whill.move(0, 20);
+        sleep(0.1);
+        count++;
+    }
+    count = 0;
+    while(count < 5000)
+    {
+        whill.move(0, -20);
+        sleep(0.1);
+        count++;
+    }
+
+    sleep(1);
+    parseReply();
+
+    whill.close();
+}

lib/Whill/test/07_whill/07_whill.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/Whill.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillC whill;
+
+void printSpeedProfile()
+{
+    cout << "SPEED_PROFILE" << endl;
+    cout << "dataset : " << (int)whill.dataset() << endl;
+    cout << "mode    : " << (int)whill.speedMode() << endl;
+    cout << "fwd spd : " << (int)whill.forwardSpeed() << endl;
+    cout << "fwd acc : " << (int)whill.forwardAccel() << endl;
+    cout << "fwd dec : " << (int)whill.forwardDecel() << endl;
+    cout << "bwd spd : " << (int)whill.reverseSpeed() << endl;
+    cout << "bwd acc : " << (int)whill.reverseAccel() << endl;
+    cout << "bwd dec : " << (int)whill.reverseDecel() << endl;
+    cout << "trn spd : " << (int)whill.turnSpeed() << endl;
+    cout << "trn acc : " << (int)whill.turnAccel() << endl;
+    cout << "trn dec : " << (int)whill.turnDecel() << endl;
+}
+
+void printSensorInfo()
+{
+    cout << "SENSOR_INFO" << endl;
+    cout << "sensor acc x : " << (float)whill.sensorAccelX() << endl;
+    cout << "sensor acc y : " << (float)whill.sensorAccelY() << endl;
+    cout << "sensor acc z : " << (float)whill.sensorAccelZ() << endl;
+    cout << "sensor gyr x : " << (float)whill.sensorGyroX() << endl;
+    cout << "sensor gyr y : " << (float)whill.sensorGyroY() << endl;
+    cout << "sensor gyr z : " << (float)whill.sensorGyroZ() << endl;
+    cout << "joystick fb  : " << (int)whill.joystickFront() << endl;
+    cout << "joystick lr  : " << (int)whill.joystickSide() << endl;
+    cout << "battery      : " << (int)whill.battery() << endl;
+    cout << "battery crnt : " << (int)whill.batteryCurrent() << endl;
+    cout << "angle rad r  : " << (float)whill.angleRadR() << endl;
+    cout << "angle rad l  : " << (float)whill.angleRadL() << endl;
+    cout << "speed r      : " << (float)whill.speedR() << endl;
+    cout << "speed l      : " << (float)whill.speedL() << endl;
+    cout << "power state  : " << (int)whill.powerState() << endl;
+    cout << "speed mode   : " << (int)whill.speedModeIndicator() << endl;
+    cout << "error code   : " << (int)whill.errorCode() << endl;
+}
+
+void parseReply()
+{
+    whill.parse();
+
+    cout << "available : " << (int)whill.available() << endl;
+
+    while(whill.available())
+    {
+        cout << "[data received]" << endl;
+        cout << "size  " << to_string((int)whill.size()) << endl;
+        cout << "data  ";
+        for (uint8_t i = 0; i < whill.size(); ++i)
+            cout << hex << setw(2) << setfill('0') << (int)whill.data(i) << " ";
+        cout << dec << endl << endl;
+
+        if      (whill.dataset() == 0) printSpeedProfile();
+        else if (whill.dataset() == 1) printSensorInfo();
+        else    cout << "invalid dataset number" << endl;
+
+        whill.pop();
+    }
+}
+
+
+int main()
+{
+    if (whill.open("/dev/ttyUSB0", 38400)) // USB
+    // if (whill.open("/dev/ttyUSB1", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    cout << "power off and wait for more than 5 sec..." << endl;
+
+    // power off WHILL
+    whill.power(false);
+    sleep(5); // wait for more than 5s
+
+    // set speed profile
+    whill.speed(SPEEDMODE::RS232C, 40, 40, 40, 40, 40, 40, 40, 40, 40);
+    sleep(1);
+
+    // first, check speed profile
+    whill.streaming(true, 1000, DATASET::SPEED_PROFILE);
+    sleep(1.5);
+    parseReply();
+
+    // second, stream sensor data
+    whill.streaming(true, 1000, DATASET::SENSOR_INFO);
+    sleep(1.5);
+    parseReply();
+
+    cout << "start" << endl;
+
+    // power on WHILL
+    while (1)
+    {
+        whill.power(true);
+        sleep(1.5); // wait for reply (max 1s)
+
+        // after 1s, reply will come if power on success
+        whill.parse();
+        if (whill.available())
+        {
+            if (whill.checkPowerOn()) break;
+        }
+        cout << "re-issue power on command..." << endl;
+    }
+
+    cout << "whill power on" << endl;
+
+
+    // do what we want
+    int count = 0;
+    while(count < 5000)
+    {
+        whill.move(20, 0);
+        sleep(0.1);
+        count++;
+    }
+    count = 0;
+    while(count < 5000)
+    {
+        whill.move(-20, 0);
+        sleep(0.1);
+        count++;
+    }
+    count = 0;
+    while(count < 5000)
+    {
+        whill.move(0, 20);
+        sleep(0.1);
+        count++;
+    }
+    count = 0;
+    while(count < 5000)
+    {
+        whill.move(0, -20);
+        sleep(0.1);
+        count++;
+    }
+
+
+
+
+    whill.streaming(false);
+
+    cout << "end test" << endl;
+    whill.close();
+}
+
+
+
+

lib/Whill/test/08_whillomni/08_whillomni.ino

+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <unistd.h>
+
+#include "../libs/Whill/WhillOmni.h"
+
+using namespace std;
+using namespace Whill;
+
+WhillOmni whill;
+
+void parseReply()
+{
+    whill.parse();
+
+    for (uint8_t i = 0; i < 2; ++i)
+    {
+        WhillOmni::LR lr = (i == 0) ? WhillOmni::LR::L : WhillOmni::LR::R;
+        cout << "available : " << (int)whill.available(lr) << endl;
+        while(whill.available(lr))
+        {
+            cout << "[data received]" << endl;
+            cout << "size  " << to_string((int)whill.size(lr)) << endl;
+            cout << "data  ";
+            for (uint8_t i = 0; i < whill.size(lr); ++i)
+                cout << hex << setw(2) << setfill('0') << (int)whill.data(lr, i) << " ";
+            cout << dec << endl << endl;
+            whill.pop(lr);
+        }
+    }
+}
+
+int main()
+{
+    if (whill.open("/dev/ttyUSB0", "/dev/ttyUSB1", 38400)) // USB
+        cout << "serial open success" << endl;
+    else
+        cout << "serial open failed" << endl;
+
+    whill.speed(SPEEDMODE::RS232C, 60, 40, 40, 60, 40, 40, 60, 40, 40);
+    sleep(1);
+    parseReply();
+
+    whill.streaming(true);
+    // whill.streaming(true, 1000, NUM_0);
+    sleep(1);
+    parseReply();
+
+    int count = 0;
+    while(count < 10)
+    {
+        sleep(1);
+        cout << "in while : " << count << endl;
+
+        switch(count)
+        {
+            case 0: whill.power(true); break;
+            case 1: whill.power(false); break;
+            case 2: whill.powerSupply(true); break;
+            case 3: whill.powerSupply(false); break;
+            case 4: whill.controlEnable(true); break;
+            case 5: whill.controlEnable(false); break;
+            case 6: whill.stop(); break;
+            case 7: whill.move(10, 0, 0); break;
+            case 8: whill.move(0, 10, 0); break;
+            case 9: whill.move(0, 0, 10); break;
+            case 10: whill.move(-10, 0, 0); break;
+            case 11: whill.move(0, -10, 0); break;
+            case 12: whill.move(0, 0, -10); break;
+            default: break;
+        }
+
+        sleep(2);
+
+        parseReply();
+
+        ++count;
+    }
+
+    whill.streaming(false);
+    sleep(1);
+    parseReply();
+
+    whill.close();
+}
+
+

lib/Whill/test/README

+
+This directory is intended for PIO Unit Testing and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PIO Unit Testing:
+- https://docs.platformio.org/page/plus/unit-testing.html

platformio.ini

+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:teensy35]
+platform = teensy
+board = teensy35
+framework = arduino
+build_flags = -Wall
+lib_extra_dirs = /lib/Whill, lib/ArduinoEigen

src/main.cpp

+#include "global.h"
+#include "main.h"
+
+void setup()
+{
+    WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L
+    WHILL_SERIAL_R.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // R
+    whill.attatch(WHILL_SERIAL_L, WHILL_SERIAL_R);
+
+    XBEE_SERIAL.begin(XBEE_SERIAL_BAUD); // XBee
+
+    delay(2000);
+
+    whill.stop();
+
+    delay(2000);
+
+    whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 60, 50, 80, 60, 60, 160); // max
+    whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
+    waitReply(WhillOmni::LR::L, WHILL_INFO_INTERVAL_MS + 500);
+    waitReply(WhillOmni::LR::R);
+
+    whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
+    delay(1000);
+
+    Serial.println("start program");
+}
+
+
+void loop()
+{
+    setVelocity();
+
+    whill.parse();
+    sendStatus(WhillOmni::LR::L);
+    sendStatus(WhillOmni::LR::R);
+}

test/README

+
+This directory is intended for PIO Unit Testing and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PIO Unit Testing:
+- https://docs.platformio.org/page/plus/unit-testing.html