add sch and src

README.md

@@ -5,5 +5,5 @@ Hexapod v2 using Linkit 7697 (Still under construction)
 ![Hexapod](files/hexapod.jpg)
 
 * [Mechanism](mechanism/README.md)
-* Electronic (TBD) 
+* [Electronics](electronics/README.md)
-* Software (TBD)
+* [Software](software/README.md)

electronics/README.md

@@ -0,0 +1,52 @@
+# Electronics
+
+## Connection Diagram
+
+![connections](files/connections.jpg)
+
+## Bill of Materials
+
+### PCB
+
+Name | Thumbnail | Required |
+-------- | --------- | -------- |
+main |           | 1 |
+sub |           | 2 |
+
+### Components
+
+Name | Thumbnail | Required |
+-------- | --------- | -------- |
+[Linkit 7697](https://labs.mediatek.com/zh-tw/platform/linkit-7697) | <img width=200 src='https://labs.mediatek.com/images/img_hdk_7697.png'/> | 1 |
+[mini360 DC-DC](https://www.aliexpress.com/w/wholesale-mini360-dc-dc.html) Buck voltage regulator | ![](files/mini360.png) | 7 |
+[PCA9865](https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf) (TSSOP28)| ![](files/pca9865.png) | 2 |
+Resistor: 220 ohm (0805) | | 18 |
+Resistor: 10K ohm (0805) | | 12 |
+Resistor: 470 ohm (0805) | | 1 |
+LED: green or any color (0805) | | 1 |
+Capacitor: 10 uF (0805) | | 2 |
+14 pin 2.54mm female header | | 4 |
+4 pin 2.54mm female header | | 2 |
+6 pin 2.54mm female header | | 2 |
+4 pin 2.54mm 90 degress male header | | 2 |
+6 pin 2.54mm 90 degress male header | | 2 |
+4 pin 2.54mm male header | | 2 |
+3 pin 2.54mm male header (black) | | 6 |
+3 pin 2.54mm male header (yellow) | | 6 |
+3 pin 2.54mm male header (red) | | 6 |
+2 pin 2.54mm male header | | 1 |
+Jumper 2.54mm | | 1 |
+
+## PCB detail
+
+> Please use 'Eagle' software to open below schematics
+
+### Main
+
+![main_sch](files/main_sch.png)
+![main_brd](files/main_brd.png)
+
+### Sub (x2)
+
+![sub_sch](files/sub_sch.png)
+![sub_brd](files/sub_brd.png)

mechanism/README.md

@@ -10,6 +10,4 @@
 
 
 
-
-
 ## Body Assemble

software/README.md

@@ -0,0 +1,29 @@
+# Software
+
+Software contain 2 parts, one is software of running on Linkt 7697 (an arduino C++ program), another is path generation (a python program).
+
+* hexapod7697: an arduino program running on Linkit 7697
+* pathTool: a python program that generate 3D points header, included by `hexapod7697`
+
+## hexapod7697
+
+Files/Folder | Description |
+------------ | ----------- |
+src/normal_mode | provide normal calibration (forward/backward/turn left/turn right/rotate/etc...) |
+src/setting_mode | provide calirbration functions, to make sure servo is correctly aligned. |
+src/linkit_control/ | UI control helper |
+src/hexapod | hexapod class |
+src/hexapod/hal | Hardware Abatraction Layer to running on Linkit 7697 |
+
+> hexapod7697 use c++11 and STL, you may encounter problem if want to port it on pure Arduino platform.
+
+## pathTool
+
+* Python compatibility:
+    * 3.x: ok.
+    * 2.x: not verified. 
+* Required package
+    * numpy: `pip3 install numpy`
+
+
+

software/hexapod7697/.gitignore

@@ -0,0 +1 @@
+.vscode/

software/hexapod7697/hexapod7697.ino

@@ -0,0 +1,84 @@
+
+#include "src/setting_mode.h"
+#include "src/normal_mode.h"
+
+#include "src/hexapod/debug.h"
+#include "src/hexapod/hexapod.h"
+
+//
+//  mode
+//    0:  normal mode
+//    1:  setting mode (for servo calibration)
+//
+static int _mode = 0;
+
+static void log_output(const char* log) {
+  Serial.println(log);
+}
+
+//
+// wait for N secs, enter setting mode if pressed USR button
+//
+void boot_wait(int wait_for = 2000) {
+  digitalWrite(LED_BUILTIN, HIGH);
+
+  pinMode(6, INPUT);
+  while (wait_for > 0) {
+    if (HIGH == digitalRead(6)) {
+      _mode = 1;
+      break;
+    }
+    delay(100);
+    wait_for -= 100;
+  }
+
+  digitalWrite(LED_BUILTIN, LOW);
+}
+
+//
+//  Do common setup and mode-specific setup
+//
+void setup() {
+  //Initialize serial and wait for port to open:
+  Serial.begin(115200);
+  Serial.println("Starting...");
+
+  //
+  // Common setup()
+  //
+
+  // Initialize GPIO
+  pinMode(LED_BUILTIN, OUTPUT);
+
+  // Workaround for beta PCB
+  pinMode(2, INPUT);
+  pinMode(3, INPUT);
+
+  boot_wait();
+
+  hexapod::initLogOutput(log_output, millis);
+  hexapod::Hexapod.init(_mode == 1);
+
+  //
+  // Mode-specific setup()
+  //
+
+  if (_mode == 0) {
+    normal_setup();
+  }
+  else if (_mode == 1) {
+    setting_setup();
+  }
+
+  Serial.print("Started, mode=");
+  Serial.println(_mode);
+}
+
+void loop() {
+  if (_mode == 0) {
+    normal_loop();
+  }
+  else if (_mode == 1) {
+    setting_loop();
+  }
+}

software/hexapod7697/src/hexapod/base.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <stdexcept>
+
+namespace hexapod {
+
+    class Point3D {
+    public:
+        Point3D() = default;
+        Point3D(float x, float y, float z): x_{x}, y_{y}, z_{z} {
+        }
+
+    public:
+        float x_;
+        float y_;
+        float z_;
+    };
+
+    inline Point3D operator-(const Point3D &lhs, const Point3D &rhs) {
+        return Point3D(lhs.x_ - rhs.x_, lhs.y_ - rhs.y_, lhs.z_ - rhs.z_);
+    };
+
+    inline Point3D operator*(const Point3D &lhs, const float& b) {
+        return Point3D(lhs.x_ * b, lhs.y_ * b, lhs.z_ * b);
+    };
+
+    inline Point3D& operator +=(Point3D& a, const Point3D& b) {
+        a.x_ += b.x_;
+        a.y_ += b.y_;
+        a.z_ += b.z_;
+        return a;
+    }
+
+    inline bool operator ==(const Point3D &a, const Point3D &b) {
+        return (a.x_ == b.x_) && (a.y_ == b.y_) && (a.z_ == b.z_);
+    }
+
+    class Locations {
+    public:
+        Locations() = default;
+        Locations(const Point3D& foreRight, const Point3D& right, const Point3D& hindRight, const Point3D& hindLeft, const Point3D& left, const Point3D& foreLeft):
+            points_{foreRight, right, hindRight, hindLeft, left, foreLeft} {
+        }
+
+        const Point3D& get(int index) const {
+            return points_[index];
+        }
+
+        Locations operator-(const Locations &b) const {
+            return Locations{
+                points_[0] - b.points_[0], points_[1] - b.points_[1], points_[2] - b.points_[2],
+                points_[3] - b.points_[3], points_[4] - b.points_[4], points_[5] - b.points_[5],
+            };
+        }
+
+        Locations operator*(const float& b) const {
+            return Locations{
+                points_[0] * b, points_[1] * b, points_[2] * b,
+                points_[3] * b, points_[4] * b, points_[5] * b,
+            };
+        }
+
+        Locations& operator +=(const Locations& b) {
+            for(auto i=0;i<6;i++)
+                points_[i] += b.points_[i];
+            return *this;
+        }
+    private:
+        Point3D points_[6];
+    };
+}

software/hexapod7697/src/hexapod/config.h

@@ -0,0 +1,22 @@
+#pragma once
+
+namespace hexapod { 
+
+    namespace config {
+        // all below definition use unit: mm
+        const float kLegMountLeftRightX = 29.87;
+        const float kLegMountOtherX = 22.41;
+        const float kLegMountOtherY = 55.41;
+        
+        const float kLegRootToJoint1 = 20.75;
+        const float kLegJoint1ToJoint2 = 28.0;
+        const float kLegJoint2ToJoint3 = 42.6;
+        const float kLegJoint3ToTip = 89.07;
+
+
+        // timing setting. unit: ms
+        const int movementInterval = 5;
+        const int movementSwitchDuration = 150;
+    }
+
+}

software/hexapod7697/src/hexapod/debug.cpp

@@ -0,0 +1,36 @@
+#include "debug.h"
+
+#include <stdio.h>
+#include <stdarg.h>  // For va_start, etc.
+
+namespace {
+    std::function<void(const char*)> _writer = nullptr;
+    std::function<int(void)> _time_func = nullptr;
+}
+
+void _my_log_impl(const char* format, ...)
+{
+    char buffer[256];
+    int pos = 0;
+
+    if(_time_func) {
+        pos = sprintf(buffer, "[%d]", _time_func());
+    }
+
+    if (_writer) {
+        va_list ap;
+        va_start(ap, format);
+        vsprintf(buffer+pos, format, ap);
+        va_end(ap);
+        _writer(buffer);
+    }
+}
+
+namespace hexapod {
+
+    void initLogOutput(std::function<void(const char*)> writer, std::function<int(void)> time_func) {
+        _writer = writer;
+        _time_func = time_func;
+    }
+
+}

software/hexapod7697/src/hexapod/debug.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <functional>
+
+#undef LOG_DEBUG_ON
+#define LOG_INFO_ON
+
+
+#ifdef LOG_DEBUG_ON
+void _my_log_impl(const char* format, ...);
+#define LOG_DEBUG(format, ...) _my_log_impl(format, ##__VA_ARGS__)
+#else
+#define LOG_DEBUG(format, ...) 
+#endif
+
+#ifdef LOG_INFO_ON
+void _my_log_impl(const char* format, ...);
+#define LOG_INFO(format, ...) _my_log_impl(format, ##__VA_ARGS__)
+#else
+#define LOG_INFO(format, ...) 
+#endif
+
+namespace hexapod {
+    void initLogOutput(std::function<void(const char*)> writer, std::function<int(void)> time_func);
+}

software/hexapod7697/src/hexapod/hal/pwm.cpp

@@ -0,0 +1,29 @@
+
+#include <Wire.h>
+#include <Adafruit_PWMServoDriver.h>
+
+#include "pwm.h"
+
+namespace hexapod { namespace hal {
+
+    PCA9685::PCA9685(int i2cAddress) {
+        obj_ = (void*)new Adafruit_PWMServoDriver(i2cAddress);
+    }
+
+    PCA9685::~PCA9685() {
+        delete ((Adafruit_PWMServoDriver*)obj_);
+    }
+
+    void PCA9685::begin() {
+        ((Adafruit_PWMServoDriver*)obj_)->begin();
+    }
+
+    void PCA9685::setPWMFreq(int freq) {
+        ((Adafruit_PWMServoDriver*)obj_)->setPWMFreq(freq);
+    }
+
+    void PCA9685::setPWM(int index, int on, int off) {
+        ((Adafruit_PWMServoDriver*)obj_)->setPWM(index, (uint16_t)on, (uint16_t)off);
+    }
+
+}}

software/hexapod7697/src/hexapod/hal/pwm.h

@@ -0,0 +1,18 @@
+#pragma once
+
+namespace hexapod { namespace hal {
+
+    class PCA9685 {
+    public:
+        PCA9685(int i2cAddress = 0x40);
+        ~PCA9685();
+
+        void begin();
+        void setPWMFreq(int freq);
+        void setPWM(int index, int on, int off);
+
+    private:
+        void* obj_;
+    };
+
+}}

software/hexapod7697/src/hexapod/hexapod.cpp

@@ -0,0 +1,99 @@
+
+#include "hexapod.h"
+#include "servo.h"
+#include "debug.h"
+
+// TBD: move to hal
+#include <LFlash.h>
+
+namespace hexapod {
+
+    HexapodClass Hexapod;
+
+    HexapodClass::HexapodClass(): 
+        legs_{{0}, {1}, {2}, {3}, {4}, {5}}, 
+        movement_{MOVEMENT_STANDBY},
+        mode_{MOVEMENT_STANDBY}
+    {
+
+    }
+
+    void HexapodClass::init(bool setting) {
+        Servo::init();
+
+        LFlash.begin();
+        calibrationLoad();
+
+        // default to standby mode
+        if (!setting)
+            processMovement(MOVEMENT_STANDBY);
+
+        LOG_INFO("Hexapod init done.");
+    }
+
+    void HexapodClass::processMovement(MovementMode mode, int elapsed) {
+        if (mode_ != mode) {
+            mode_ = mode;
+            movement_.setMode(mode_);
+        }
+
+        auto& location = movement_.next(elapsed);
+        for(int i=0;i<6;i++) {
+            legs_[i].moveTip(location.get(i));
+        }
+    }
+
+    void HexapodClass::calibrationSave() {
+
+        short data[6*3*2];
+
+        for(int i=0;i<6;i++) {
+            for(int j=0;j<3;j++) {
+                int offset, scale;
+                legs_[i].get(j)->getParameter(offset, scale);
+                data[2*3*i + 2*j] = (short)offset;
+                data[2*3*i + 2*j + 1] = (short)scale;
+            }
+        }
+
+        LFlash.write(
+            "HEXAPOD",
+            "CALI",
+            LFLASH_RAW_DATA,
+            (const uint8_t *)data,
+            sizeof(data)
+        );
+
+    }
+
+    void HexapodClass::calibrationGet(int legIndex, int partIndex, int& offset, int& scale) {
+        legs_[legIndex].get(partIndex)->getParameter(offset, scale);
+    }
+
+    void HexapodClass::calibrationSet(int legIndex, int partIndex, int offset, int scale) {
+        legs_[legIndex].get(partIndex)->setParameter(offset, scale);
+    }
+
+    void HexapodClass::calibrationTest(int legIndex, int partIndex, float angle) {
+        legs_[legIndex].get(partIndex)->setAngle(angle);
+    }
+
+    void HexapodClass::calibrationLoad() {
+        short data[6*3*2] = {0};
+        uint32_t size = sizeof(data);
+
+        LFlash.read(
+            "HEXAPOD",
+            "CALI",
+            (uint8_t *)data,
+            &size
+        );
+
+        for(int i=0;i<6;i++) {
+            for(int j=0;j<3;j++) {
+                legs_[i].get(j)->setParameter(data[2*3*i + 2*j], data[2*3*i + 2*j + 1], false);
+            }
+        }
+    }
+
+}

software/hexapod7697/src/hexapod/hexapod.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include "movement.h"
+#include "leg.h"
+
+namespace hexapod {
+
+    class HexapodClass {
+    public:
+        HexapodClass();
+
+        // init
+
+        void init(bool setting = false);
+
+        // Movement API
+
+        void processMovement(MovementMode mode, int elapsed = 0);
+
+        // Calibration API
+
+        void calibrationSave(); // write to flash
+        void calibrationGet(int legIndex, int partIndex, int& offset, int& scale);  // read servo setting
+        void calibrationSet(int legIndex, int partIndex, int offset, int scale);    // update servo setting
+        void calibrationTest(int legIndex, int partIndex, float angle);             // test servo setting
+
+    private:
+        void calibrationLoad(); // read from flash
+
+    private:
+        MovementMode mode_;
+        Movement movement_;
+        Leg legs_[6];
+    };
+
+    extern HexapodClass Hexapod;
+}

software/hexapod7697/src/hexapod/leg.cpp

@@ -0,0 +1,201 @@
+#include <Arduino.h>
+
+#include "leg.h"
+#include "config.h"
+#include "debug.h"
+
+#include <cmath>
+
+using namespace hexapod::config;
+
+// Angle (CCW)
+//          0 : + X-axis
+//         90 : + Y-axis
+//        180 : - X-axis
+//        270 : - Y-axis
+
+namespace hexapod {
+
+    namespace {
+
+        // rotate matrix:
+        //  x' = x * cos - y * sin
+        //  y' = x * sin + y * cos
+
+        #define SIN45   0.7071
+        #define COS45   0.7071
+
+        void rotate0(const Point3D& src, Point3D& dest) {
+            dest = src;
+        }
+
+        void rotate45(const Point3D& src, Point3D& dest) {
+            dest.x_ = src.x_ * COS45 - src.y_ * SIN45;
+            dest.y_ = src.x_ * SIN45 + src.y_ * COS45;
+            dest.z_ = src.z_;
+        }
+
+        void rotate135(const Point3D& src, Point3D& dest) {
+            dest.x_ = src.x_ * (-COS45) - src.y_ * SIN45;
+            dest.y_ = src.x_ * SIN45 + src.y_ * (-COS45);
+            dest.z_ = src.z_;
+        }
+
+        void rotate180(const Point3D& src, Point3D& dest) {
+            dest.x_ = -src.x_;
+            dest.y_ = -src.y_;
+            dest.z_ = src.z_;
+        }
+
+        void rotate225(const Point3D& src, Point3D& dest) {
+            dest.x_ = src.x_ * (-COS45) - src.y_ * (-SIN45);
+            dest.y_ = src.x_ * (-SIN45) + src.y_ * (-COS45);
+            dest.z_ = src.z_;
+        }
+
+        void rotate315(const Point3D& src, Point3D& dest) {
+            dest.x_ = src.x_ * COS45 - src.y_ * (-SIN45);
+            dest.y_ = src.x_ * (-SIN45) + src.y_ * COS45;
+            dest.z_ = src.z_;
+        }
+    }
+
+    // Public
+
+    Leg::Leg(int legIndex): index_(legIndex) {
+        switch(legIndex) {
+        case 0: // 45 degree
+            mountPosition_ = Point3D(kLegMountOtherX, kLegMountOtherY, 0);
+            localConv_ = rotate315;
+            worldConv_ = rotate45;
+            break;
+        case 1: // 0 degree
+            mountPosition_ = Point3D(kLegMountLeftRightX, 0, 0);
+            localConv_ = rotate0;
+            worldConv_ = rotate0;
+            break;
+        case 2: // -45 or 315 degree
+            mountPosition_ = Point3D(kLegMountOtherX, -kLegMountOtherY, 0);
+            localConv_ = rotate45;
+            worldConv_ = rotate315;
+            break;
+        case 3: // -135 or 225 degree
+            mountPosition_ = Point3D(-kLegMountOtherX, -kLegMountOtherY, 0);
+            localConv_ = rotate135;
+            worldConv_ = rotate225;
+            break;
+        case 4: // 180 degree
+            mountPosition_ = Point3D(-kLegMountLeftRightX, 0, 0);
+            localConv_ = rotate180;
+            worldConv_ = rotate180;
+            break;
+        case 5: // 135 degree
+            mountPosition_ = Point3D(-kLegMountOtherX, kLegMountOtherY, 0);
+            localConv_ = rotate225;
+            worldConv_ = rotate135;
+            break;
+        }
+
+        for(int i=0;i<3;i++)
+            servos_[i] = new hexapod::Servo(legIndex, i);
+    }
+
+    Leg::~Leg() {
+        for(int i=0;i<3;i++) {
+            delete servos_[i];
+            servos_[i] = nullptr;
+        }
+    }
+
+    void Leg::translateToLocal(const Point3D& world, Point3D& local) {
+        localConv_(world - mountPosition_, local);
+    }
+
+    void Leg::translateToWorld(const Point3D& local, Point3D& world) {
+        worldConv_(local, world);
+        world += mountPosition_;
+    }
+
+    void Leg::setJointAngle(float angle[3]) {
+        Point3D to;
+        _forwardKinematics(angle, to);
+        moveTipLocal(to);
+    }
+
+    void Leg::moveTip(const Point3D& to) {
+        if (to == tipPos_)
+            return;
+
+        Point3D local;
+        translateToLocal(to, local);
+        LOG_DEBUG("leg(%d) moveTip(%f,%f,%f)(%f,%f,%f)", index_, to.x_, to.y_, to.z_, local.x_, local.y_, local.z_);
+        _move(local);
+        tipPos_ = to;
+        tipPosLocal_ = local;
+    }
+
+    const Point3D& Leg::getTipPosition(void) {
+        return tipPos_;
+    }
+
+    void Leg::moveTipLocal(const Point3D& to) {
+        if (to == tipPosLocal_)
+            return;
+
+        Point3D world;
+        translateToWorld(to, world);
+        _move(to);
+        tipPos_ = world;
+        tipPosLocal_ = to;
+    }
+
+    const Point3D& Leg::getTipPositionLocal(void) {
+        return tipPosLocal_;
+    }
+
+    //
+    // Private
+    //
+    const float pi = std::acos(-1);
+    const float hpi = pi/2;
+
+    void Leg::_forwardKinematics(float angle[3], Point3D& out) {
+        float radian[3];
+        for(int i=0; i<3; i++)
+            radian[i] = pi * angle [i] / 180;
+
+        float x = kLegJoint1ToJoint2 + std::cos(radian[1]) * kLegJoint2ToJoint3 + std::cos(radian[1] + radian[2] - hpi) * kLegJoint3ToTip;
+
+        out.x_ = kLegRootToJoint1 + std::cos(radian[0]) * x;
+        out.y_ = std::sin(radian[0]) * x;
+        out.z_ = std::sin(radian[1]) * kLegJoint2ToJoint3 + std::sin(radian[1] + radian[2] - hpi) * kLegJoint3ToTip;
+    }
+
+    void Leg::_inverseKinematics(const Point3D& to, float angles[3]) {
+
+        float x = to.x_ - kLegRootToJoint1;
+        float y = to.y_;
+
+        angles[0] = std::atan2(y, x) * 180 / pi;
+
+        x = std::sqrt(x*x + y*y) - kLegJoint1ToJoint2;
+        y = to.z_;
+        float ar = std::atan2(y, x);
+        float lr2 = x*x + y*y;
+        float lr = std::sqrt(lr2);
+        float a1 = std::acos((lr2 + kLegJoint2ToJoint3*kLegJoint2ToJoint3 - kLegJoint3ToTip*kLegJoint3ToTip)/(2*kLegJoint2ToJoint3*lr));
+        float a2 = std::acos((lr2 - kLegJoint2ToJoint3*kLegJoint2ToJoint3 + kLegJoint3ToTip*kLegJoint3ToTip)/(2*kLegJoint3ToTip*lr));
+        angles[1] = (ar + a1) * 180 / pi;
+        angles[2] = 90 - ((a1 + a2)  * 180 / pi);
+    }
+
+    void Leg::_move(const Point3D& to) {
+        float angles[3];
+        _inverseKinematics(to, angles);
+        LOG_DEBUG("leg(%d) move: (%f,%f,%f)", index_, angles[0], angles[1], angles[2]);
+        for(int i=0; i<3; i++) {
+            servos_[i]->setAngle(angles[i]);
+        }
+    }
+
+}

software/hexapod7697/src/hexapod/leg.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "base.h"
+#include "servo.h"
+
+#include <functional>
+
+namespace hexapod {
+
+    class Leg {
+    public:
+        Leg(int legIndex);
+        virtual ~Leg();
+
+        // coordinate system translation
+        void translateToLocal(const Point3D& world, Point3D& local);
+        void translateToWorld(const Point3D& local, Point3D& world);
+
+        void setJointAngle(float angle[3]);
+
+        // world coordinate system (default)
+        void moveTip(const Point3D& to);
+        const Point3D& getTipPosition(void);
+
+        // local coordinate system version
+        void moveTipLocal(const Point3D& to);
+        const Point3D& getTipPositionLocal(void);
+
+        // 
+        Servo* get(int partIndex) {
+            return servos_[partIndex];
+        }
+
+    private:
+        // Local coorinate system
+        static void _forwardKinematics(float angle[3], Point3D& out);
+        static void _inverseKinematics(const Point3D& to, float angles[3]);
+        void _move(const Point3D& to);
+
+    private:
+        int index_;
+        Servo* servos_[3];
+        Point3D mountPosition_;
+        std::function<void(const Point3D& src, Point3D& dest)> localConv_;
+        std::function<void(const Point3D& src, Point3D& dest)> worldConv_;
+
+        Point3D tipPos_;
+        Point3D tipPosLocal_;
+
+    };
+
+}

software/hexapod7697/src/hexapod/movement.cpp

@@ -0,0 +1,67 @@
+#include "movement.h"
+#include "debug.h"
+#include "config.h"
+
+#include <cstdlib>
+
+namespace hexapod {
+
+    extern const MovementTable& standbyTable();
+    extern const MovementTable& forwardTable();
+    extern const MovementTable& backwardTable();
+    extern const MovementTable& turnleftTable();
+    extern const MovementTable& turnrightTable();
+    extern const MovementTable& shiftleftTable();
+    extern const MovementTable& shiftrightTable();
+    extern const MovementTable& rotatexTable();
+    extern const MovementTable& rotateyTable();
+    extern const MovementTable& rotatezTable();
+
+    const MovementTable kTable[MOVEMENT_TOTAL] {
+        standbyTable(),
+        forwardTable(),
+        backwardTable(),
+        turnleftTable(),
+        turnrightTable(),
+        shiftleftTable(),
+        shiftrightTable(),
+        rotatexTable(),
+        rotateyTable(),
+        rotatezTable(),
+    };
+
+    Movement::Movement(MovementMode mode):
+        mode_{mode}, transiting_{false}
+    {
+    }
+
+    void Movement::setMode(MovementMode newMode) {
+        mode_ = newMode;
+
+        const MovementTable& table = kTable[mode_];
+
+        index_ = table.entries[std::rand() % table.entriesCount];
+        remainTime_ = config::movementSwitchDuration > table.stepDuration ? config::movementSwitchDuration : table.stepDuration;
+    }
+
+    const Locations& Movement::next(int elapsed) {
+
+        const MovementTable& table = kTable[mode_];
+
+         if (elapsed <= 0)
+            elapsed = table.stepDuration;
+
+        if (remainTime_ <= 0) {
+            index_ = (index_ + 1)%table.length;
+            remainTime_ = table.stepDuration;
+        }
+        if (elapsed >= remainTime_)
+            elapsed = remainTime_;
+
+        auto ratio = (float)elapsed / remainTime_;
+        position_ += (table.table[index_] - position_)*ratio;
+        remainTime_ -= elapsed;
+
+        return position_;
+    }
+}

software/hexapod7697/src/hexapod/movement.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "base.h"
+
+namespace hexapod {
+
+    enum MovementMode {
+        MOVEMENT_STANDBY = 0,
+
+        MOVEMENT_FORWARD,
+        MOVEMENT_BACKWARD,
+        MOVEMENT_TURNLEFT,
+        MOVEMENT_TURNRIGHT,
+        MOVEMENT_SHIFTLEFT,
+        MOVEMENT_SHIFTRIGHT,
+        MOVEMENT_ROTATEX,
+        MOVEMENT_ROTATEY,
+        MOVEMENT_ROTATEZ,
+
+        MOVEMENT_TOTAL,
+    };
+
+    inline MovementMode operator++ (MovementMode& m, int) {
+        if (m < MOVEMENT_TOTAL)
+            m = static_cast<MovementMode>(static_cast<int>(m) + 1);
+        return m;
+    }
+
+    struct MovementTable {
+        const Locations* table;
+        int length;
+        int stepDuration;
+        const int* entries;
+        int entriesCount;
+    };
+
+    class Movement {
+    public:
+        Movement(MovementMode mode);
+
+        void setMode(MovementMode newMode);
+
+        const Locations& next(int elapsed);
+
+    private:
+        MovementMode mode_;
+        Locations position_;
+        int index_;             // index in mode position table
+        bool transiting_;       // if still in transiting to new mode
+        int remainTime_;
+    };
+
+}

software/hexapod7697/src/hexapod/movement_table.h

@@ -0,0 +1,558 @@
+//
+// This file is generated, dont directly modify content...
+//
+namespace {
+
+const Locations backward_paths[] {
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(4.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(8.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(12.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(16.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(19.02), P6Z+(6.18)}},
+    {{P1X+(0.00), P1Y+(-12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(16.18), P6Z+(11.76)}},
+    {{P1X+(0.00), P1Y+(-8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(11.76), P6Z+(16.18)}},
+    {{P1X+(0.00), P1Y+(-4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(6.18), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}},
+    {{P1X+(0.00), P1Y+(4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-6.18), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-11.76), P6Z+(16.18)}},
+    {{P1X+(0.00), P1Y+(12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-16.18), P6Z+(11.76)}},
+    {{P1X+(0.00), P1Y+(16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-19.02), P6Z+(6.18)}},
+    {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(0.00)}},
+};
+const int backward_entries[] { 0,10 };
+const MovementTable backward_table {backward_paths, 20, 20, backward_entries, 2 };
+
+const Locations forward_paths[] {
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-19.02), P6Z+(6.18)}},
+    {{P1X+(0.00), P1Y+(12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-16.18), P6Z+(11.76)}},
+    {{P1X+(0.00), P1Y+(8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-11.76), P6Z+(16.18)}},
+    {{P1X+(0.00), P1Y+(4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-6.18), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}},
+    {{P1X+(0.00), P1Y+(-4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(6.18), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(-8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(11.76), P6Z+(16.18)}},
+    {{P1X+(0.00), P1Y+(-12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(16.18), P6Z+(11.76)}},
+    {{P1X+(0.00), P1Y+(-16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(19.02), P6Z+(6.18)}},
+    {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(16.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(12.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(8.00), P6Z+(0.00)}},
+    {{P1X+(0.00), P1Y+(-6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(4.00), P6Z+(0.00)}},
+};
+const int forward_entries[] { 0,10 };
+const MovementTable forward_table {forward_paths, 20, 20, forward_entries, 2 };
+
+const Locations rotatex_paths[] {
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.97 + P1Z*-0.26 + 0.00, P1X*0.00 + P1Y*0.26 + P1Z*0.97 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.97 + P2Z*-0.26 + 0.00, P2X*0.00 + P2Y*0.26 + P2Z*0.97 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.97 + P3Z*-0.26 + 0.00, P3X*0.00 + P3Y*0.26 + P3Z*0.97 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.97 + P4Z*-0.26 + 0.00, P4X*0.00 + P4Y*0.26 + P4Z*0.97 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.97 + P5Z*-0.26 + 0.00, P5X*0.00 + P5Y*0.26 + P5Z*0.97 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.97 + P6Z*-0.26 + 0.00, P6X*0.00 + P6Y*0.26 + P6Z*0.97 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + -3.00, P1X*0.00 + P1Y*0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + -3.00, P2X*0.00 + P2Y*0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + -3.00, P3X*0.00 + P3Y*0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + -3.00, P4X*0.00 + P4Y*0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + -3.00, P5X*0.00 + P5Y*0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + -3.00, P6X*0.00 + P6Y*0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.16 + -6.00, P1X*0.00 + P1Y*0.16 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.16 + -6.00, P2X*0.00 + P2Y*0.16 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.16 + -6.00, P3X*0.00 + P3Y*0.16 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.16 + -6.00, P4X*0.00 + P4Y*0.16 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.16 + -6.00, P5X*0.00 + P5Y*0.16 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.16 + -6.00, P6X*0.00 + P6Y*0.16 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.10 + -9.00, P1X*0.00 + P1Y*0.10 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.10 + -9.00, P2X*0.00 + P2Y*0.10 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.10 + -9.00, P3X*0.00 + P3Y*0.10 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.10 + -9.00, P4X*0.00 + P4Y*0.10 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.10 + -9.00, P5X*0.00 + P5Y*0.10 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.10 + -9.00, P6X*0.00 + P6Y*0.10 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + -12.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + -12.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + -12.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + -12.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + -12.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + -12.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.00 + -15.00, P1X*0.00 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.00 + -15.00, P2X*0.00 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.00 + -15.00, P3X*0.00 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.00 + -15.00, P4X*0.00 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.00 + -15.00, P5X*0.00 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.00 + -15.00, P6X*0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.05 + -12.00, P1X*0.00 + P1Y*-0.05 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.05 + -12.00, P2X*0.00 + P2Y*-0.05 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.05 + -12.00, P3X*0.00 + P3Y*-0.05 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.05 + -12.00, P4X*0.00 + P4Y*-0.05 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.05 + -12.00, P5X*0.00 + P5Y*-0.05 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.05 + -12.00, P6X*0.00 + P6Y*-0.05 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.10 + -9.00, P1X*0.00 + P1Y*-0.10 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.10 + -9.00, P2X*0.00 + P2Y*-0.10 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.10 + -9.00, P3X*0.00 + P3Y*-0.10 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.10 + -9.00, P4X*0.00 + P4Y*-0.10 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.10 + -9.00, P5X*0.00 + P5Y*-0.10 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.10 + -9.00, P6X*0.00 + P6Y*-0.10 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.16 + -6.00, P1X*0.00 + P1Y*-0.16 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.16 + -6.00, P2X*0.00 + P2Y*-0.16 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.16 + -6.00, P3X*0.00 + P3Y*-0.16 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.16 + -6.00, P4X*0.00 + P4Y*-0.16 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.16 + -6.00, P5X*0.00 + P5Y*-0.16 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.16 + -6.00, P6X*0.00 + P6Y*-0.16 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + -3.00, P1X*0.00 + P1Y*-0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + -3.00, P2X*0.00 + P2Y*-0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + -3.00, P3X*0.00 + P3Y*-0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + -3.00, P4X*0.00 + P4Y*-0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + -3.00, P5X*0.00 + P5Y*-0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + -3.00, P6X*0.00 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.97 + P1Z*0.26 + 0.00, P1X*0.00 + P1Y*-0.26 + P1Z*0.97 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.97 + P2Z*0.26 + 0.00, P2X*0.00 + P2Y*-0.26 + P2Z*0.97 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.97 + P3Z*0.26 + 0.00, P3X*0.00 + P3Y*-0.26 + P3Z*0.97 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.97 + P4Z*0.26 + 0.00, P4X*0.00 + P4Y*-0.26 + P4Z*0.97 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.97 + P5Z*0.26 + 0.00, P5X*0.00 + P5Y*-0.26 + P5Z*0.97 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.97 + P6Z*0.26 + 0.00, P6X*0.00 + P6Y*-0.26 + P6Z*0.97 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + 3.00, P1X*0.00 + P1Y*-0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + 3.00, P2X*0.00 + P2Y*-0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + 3.00, P3X*0.00 + P3Y*-0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + 3.00, P4X*0.00 + P4Y*-0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + 3.00, P5X*0.00 + P5Y*-0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + 3.00, P6X*0.00 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.16 + 6.00, P1X*0.00 + P1Y*-0.16 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.16 + 6.00, P2X*0.00 + P2Y*-0.16 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.16 + 6.00, P3X*0.00 + P3Y*-0.16 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.16 + 6.00, P4X*0.00 + P4Y*-0.16 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.16 + 6.00, P5X*0.00 + P5Y*-0.16 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.16 + 6.00, P6X*0.00 + P6Y*-0.16 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.10 + 9.00, P1X*0.00 + P1Y*-0.10 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.10 + 9.00, P2X*0.00 + P2Y*-0.10 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.10 + 9.00, P3X*0.00 + P3Y*-0.10 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.10 + 9.00, P4X*0.00 + P4Y*-0.10 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.10 + 9.00, P5X*0.00 + P5Y*-0.10 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.10 + 9.00, P6X*0.00 + P6Y*-0.10 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.05 + 12.00, P1X*0.00 + P1Y*-0.05 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.05 + 12.00, P2X*0.00 + P2Y*-0.05 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.05 + 12.00, P3X*0.00 + P3Y*-0.05 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.05 + 12.00, P4X*0.00 + P4Y*-0.05 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.05 + 12.00, P5X*0.00 + P5Y*-0.05 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.05 + 12.00, P6X*0.00 + P6Y*-0.05 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.00 + 15.00, P1X*0.00 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.00 + 15.00, P2X*0.00 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.00 + 15.00, P3X*0.00 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.00 + 15.00, P4X*0.00 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.00 + 15.00, P5X*0.00 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.00 + 15.00, P6X*0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 12.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + 12.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + 12.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + 12.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + 12.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + 12.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.10 + 9.00, P1X*0.00 + P1Y*0.10 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.10 + 9.00, P2X*0.00 + P2Y*0.10 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.10 + 9.00, P3X*0.00 + P3Y*0.10 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.10 + 9.00, P4X*0.00 + P4Y*0.10 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.10 + 9.00, P5X*0.00 + P5Y*0.10 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.10 + 9.00, P6X*0.00 + P6Y*0.10 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.16 + 6.00, P1X*0.00 + P1Y*0.16 + P1Z*0.99 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.16 + 6.00, P2X*0.00 + P2Y*0.16 + P2Z*0.99 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.16 + 6.00, P3X*0.00 + P3Y*0.16 + P3Z*0.99 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.16 + 6.00, P4X*0.00 + P4Y*0.16 + P4Z*0.99 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.16 + 6.00, P5X*0.00 + P5Y*0.16 + P5Z*0.99 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.16 + 6.00, P6X*0.00 + P6Y*0.16 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + 3.00, P1X*0.00 + P1Y*0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + 3.00, P2X*0.00 + P2Y*0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + 3.00, P3X*0.00 + P3Y*0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + 3.00, P4X*0.00 + P4Y*0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + 3.00, P5X*0.00 + P5Y*0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + 3.00, P6X*0.00 + P6Y*0.21 + P6Z*0.98 + 0.00}},
+};
+const int rotatex_entries[] { 0,10 };
+const MovementTable rotatex_table {rotatex_paths, 20, 50, rotatex_entries, 2 };
+
+const Locations rotatey_paths[] {
+    {{P1X*0.97 + P1Y*0.00 + P1Z*0.26 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.26 + P1Y*0.00 + P1Z*0.97 + 0.00}, 
+     {P2X*0.97 + P2Y*0.00 + P2Z*0.26 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.26 + P2Y*0.00 + P2Z*0.97 + 0.00}, 
+     {P3X*0.97 + P3Y*0.00 + P3Z*0.26 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.26 + P3Y*0.00 + P3Z*0.97 + 0.00}, 
+     {P4X*0.97 + P4Y*0.00 + P4Z*0.26 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.26 + P4Y*0.00 + P4Z*0.97 + 0.00}, 
+     {P5X*0.97 + P5Y*0.00 + P5Z*0.26 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.26 + P5Y*0.00 + P5Z*0.97 + 0.00}, 
+     {P6X*0.97 + P6Y*0.00 + P6Z*0.26 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.26 + P6Y*0.00 + P6Z*0.97 + 0.00}},
+    {{P1X*0.98 + P1Y*0.00 + P1Z*0.21 + -3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.21 + P1Y*0.00 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.00 + P2Z*0.21 + -3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.21 + P2Y*0.00 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.00 + P3Z*0.21 + -3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.21 + P3Y*0.00 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.00 + P4Z*0.21 + -3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.21 + P4Y*0.00 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.00 + P5Z*0.21 + -3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.21 + P5Y*0.00 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.00 + P6Z*0.21 + -3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*0.16 + -6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.16 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*0.16 + -6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.16 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*0.16 + -6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.16 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*0.16 + -6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.16 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*0.16 + -6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.16 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*0.16 + -6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*0.10 + -9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.10 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*0.10 + -9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.10 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*0.10 + -9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.10 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*0.10 + -9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.10 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*0.10 + -9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.10 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*0.10 + -9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.05 + -12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.05 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.05 + -12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.05 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.05 + -12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.05 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.05 + -12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.05 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.05 + -12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.05 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.05 + -12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + -15.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.00 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + -15.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.00 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + -15.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.00 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + -15.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.00 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + -15.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.00 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + -15.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*-0.05 + -12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.05 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*-0.05 + -12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.05 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*-0.05 + -12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.05 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*-0.05 + -12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.05 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*-0.05 + -12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.05 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*-0.05 + -12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*-0.10 + -9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.10 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*-0.10 + -9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.10 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*-0.10 + -9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.10 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*-0.10 + -9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.10 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*-0.10 + -9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.10 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*-0.10 + -9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*-0.16 + -6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.16 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*-0.16 + -6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.16 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*-0.16 + -6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.16 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*-0.16 + -6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.16 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*-0.16 + -6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.16 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*-0.16 + -6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*0.98 + P1Y*0.00 + P1Z*-0.21 + -3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.21 + P1Y*0.00 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.00 + P2Z*-0.21 + -3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.21 + P2Y*0.00 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.00 + P3Z*-0.21 + -3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.21 + P3Y*0.00 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.00 + P4Z*-0.21 + -3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.21 + P4Y*0.00 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.00 + P5Z*-0.21 + -3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.21 + P5Y*0.00 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.00 + P6Z*-0.21 + -3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
+    {{P1X*0.97 + P1Y*0.00 + P1Z*-0.26 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.26 + P1Y*0.00 + P1Z*0.97 + 0.00}, 
+     {P2X*0.97 + P2Y*0.00 + P2Z*-0.26 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.26 + P2Y*0.00 + P2Z*0.97 + 0.00}, 
+     {P3X*0.97 + P3Y*0.00 + P3Z*-0.26 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.26 + P3Y*0.00 + P3Z*0.97 + 0.00}, 
+     {P4X*0.97 + P4Y*0.00 + P4Z*-0.26 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.26 + P4Y*0.00 + P4Z*0.97 + 0.00}, 
+     {P5X*0.97 + P5Y*0.00 + P5Z*-0.26 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.26 + P5Y*0.00 + P5Z*0.97 + 0.00}, 
+     {P6X*0.97 + P6Y*0.00 + P6Z*-0.26 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.26 + P6Y*0.00 + P6Z*0.97 + 0.00}},
+    {{P1X*0.98 + P1Y*0.00 + P1Z*-0.21 + 3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.21 + P1Y*0.00 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.00 + P2Z*-0.21 + 3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.21 + P2Y*0.00 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.00 + P3Z*-0.21 + 3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.21 + P3Y*0.00 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.00 + P4Z*-0.21 + 3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.21 + P4Y*0.00 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.00 + P5Z*-0.21 + 3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.21 + P5Y*0.00 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.00 + P6Z*-0.21 + 3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*-0.16 + 6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.16 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*-0.16 + 6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.16 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*-0.16 + 6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.16 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*-0.16 + 6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.16 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*-0.16 + 6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.16 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*-0.16 + 6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*-0.10 + 9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.10 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*-0.10 + 9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.10 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*-0.10 + 9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.10 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*-0.10 + 9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.10 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*-0.10 + 9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.10 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*-0.10 + 9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*-0.05 + 12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.05 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*-0.05 + 12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.05 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*-0.05 + 12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.05 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*-0.05 + 12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.05 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*-0.05 + 12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.05 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*-0.05 + 12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 15.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.00 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 15.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.00 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 15.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.00 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 15.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.00 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 15.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.00 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 15.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.05 + 12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.05 + P1Y*0.00 + P1Z*1.00 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.05 + 12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.05 + P2Y*0.00 + P2Z*1.00 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.05 + 12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.05 + P3Y*0.00 + P3Z*1.00 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.05 + 12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.05 + P4Y*0.00 + P4Z*1.00 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.05 + 12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.05 + P5Y*0.00 + P5Z*1.00 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.05 + 12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*0.10 + 9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.10 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*0.10 + 9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.10 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*0.10 + 9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.10 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*0.10 + 9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.10 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*0.10 + 9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.10 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*0.10 + 9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*0.99 + P1Y*0.00 + P1Z*0.16 + 6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.16 + P1Y*0.00 + P1Z*0.99 + 0.00}, 
+     {P2X*0.99 + P2Y*0.00 + P2Z*0.16 + 6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.16 + P2Y*0.00 + P2Z*0.99 + 0.00}, 
+     {P3X*0.99 + P3Y*0.00 + P3Z*0.16 + 6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.16 + P3Y*0.00 + P3Z*0.99 + 0.00}, 
+     {P4X*0.99 + P4Y*0.00 + P4Z*0.16 + 6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.16 + P4Y*0.00 + P4Z*0.99 + 0.00}, 
+     {P5X*0.99 + P5Y*0.00 + P5Z*0.16 + 6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.16 + P5Y*0.00 + P5Z*0.99 + 0.00}, 
+     {P6X*0.99 + P6Y*0.00 + P6Z*0.16 + 6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
+    {{P1X*0.98 + P1Y*0.00 + P1Z*0.21 + 3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.21 + P1Y*0.00 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.00 + P2Z*0.21 + 3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.21 + P2Y*0.00 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.00 + P3Z*0.21 + 3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.21 + P3Y*0.00 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.00 + P4Z*0.21 + 3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.21 + P4Y*0.00 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.00 + P5Z*0.21 + 3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.21 + P5Y*0.00 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.00 + P6Z*0.21 + 3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
+};
+const int rotatey_entries[] { 0,10 };
+const MovementTable rotatey_table {rotatey_paths, 20, 50, rotatey_entries, 2 };
+
+const Locations rotatez_paths[] {
+    {{P1X*0.98 + P1Y*0.00 + P1Z*0.22 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.22 + P1Y*0.00 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.00 + P2Z*0.22 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.22 + P2Y*0.00 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.00 + P3Z*0.22 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.22 + P3Y*0.00 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.00 + P4Z*0.22 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.22 + P4Y*0.00 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.00 + P5Z*0.22 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.22 + P5Y*0.00 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.00 + P6Z*0.22 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.22 + P6Y*0.00 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*0.01 + P1Z*0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.07 + 0.00, P1X*-0.21 + P1Y*0.07 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.01 + P2Z*0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.07 + 0.00, P2X*-0.21 + P2Y*0.07 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.01 + P3Z*0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.07 + 0.00, P3X*-0.21 + P3Y*0.07 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.01 + P4Z*0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.07 + 0.00, P4X*-0.21 + P4Y*0.07 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.01 + P5Z*0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.07 + 0.00, P5X*-0.21 + P5Y*0.07 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.01 + P6Z*0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.07 + 0.00, P6X*-0.21 + P6Y*0.07 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*0.02 + P1Z*0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.13 + 0.00, P1X*-0.18 + P1Y*0.13 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.02 + P2Z*0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.13 + 0.00, P2X*-0.18 + P2Y*0.13 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.02 + P3Z*0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.13 + 0.00, P3X*-0.18 + P3Y*0.13 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.02 + P4Z*0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.13 + 0.00, P4X*-0.18 + P4Y*0.13 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.02 + P5Z*0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.13 + 0.00, P5X*-0.18 + P5Y*0.13 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.02 + P6Z*0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.13 + 0.00, P6X*-0.18 + P6Y*0.13 + P6Z*0.98 + 0.00}},
+    {{P1X*0.99 + P1Y*0.02 + P1Z*0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.18 + 0.00, P1X*-0.13 + P1Y*0.18 + P1Z*0.98 + 0.00}, 
+     {P2X*0.99 + P2Y*0.02 + P2Z*0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.18 + 0.00, P2X*-0.13 + P2Y*0.18 + P2Z*0.98 + 0.00}, 
+     {P3X*0.99 + P3Y*0.02 + P3Z*0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.18 + 0.00, P3X*-0.13 + P3Y*0.18 + P3Z*0.98 + 0.00}, 
+     {P4X*0.99 + P4Y*0.02 + P4Z*0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.18 + 0.00, P4X*-0.13 + P4Y*0.18 + P4Z*0.98 + 0.00}, 
+     {P5X*0.99 + P5Y*0.02 + P5Z*0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.18 + 0.00, P5X*-0.13 + P5Y*0.18 + P5Z*0.98 + 0.00}, 
+     {P6X*0.99 + P6Y*0.02 + P6Z*0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.18 + 0.00, P6X*-0.13 + P6Y*0.18 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.01 + P1Z*0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + 0.00, P1X*-0.07 + P1Y*0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.01 + P2Z*0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + 0.00, P2X*-0.07 + P2Y*0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.01 + P3Z*0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + 0.00, P3X*-0.07 + P3Y*0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.01 + P4Z*0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + 0.00, P4X*-0.07 + P4Y*0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.01 + P5Z*0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + 0.00, P5X*-0.07 + P5Y*0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.01 + P6Z*0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + 0.00, P6X*-0.07 + P6Y*0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.22 + 0.00, P1X*-0.00 + P1Y*0.22 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.22 + 0.00, P2X*-0.00 + P2Y*0.22 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.22 + 0.00, P3X*-0.00 + P3Y*0.22 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.22 + 0.00, P4X*-0.00 + P4Y*0.22 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.22 + 0.00, P5X*-0.00 + P5Y*0.22 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.22 + 0.00, P6X*-0.00 + P6Y*0.22 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*-0.01 + P1Z*-0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + 0.00, P1X*0.07 + P1Y*0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*-0.01 + P2Z*-0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + 0.00, P2X*0.07 + P2Y*0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*-0.01 + P3Z*-0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + 0.00, P3X*0.07 + P3Y*0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*-0.01 + P4Z*-0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + 0.00, P4X*0.07 + P4Y*0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*-0.01 + P5Z*-0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + 0.00, P5X*0.07 + P5Y*0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*-0.01 + P6Z*-0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + 0.00, P6X*0.07 + P6Y*0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*0.99 + P1Y*-0.02 + P1Z*-0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.18 + 0.00, P1X*0.13 + P1Y*0.18 + P1Z*0.98 + 0.00}, 
+     {P2X*0.99 + P2Y*-0.02 + P2Z*-0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.18 + 0.00, P2X*0.13 + P2Y*0.18 + P2Z*0.98 + 0.00}, 
+     {P3X*0.99 + P3Y*-0.02 + P3Z*-0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.18 + 0.00, P3X*0.13 + P3Y*0.18 + P3Z*0.98 + 0.00}, 
+     {P4X*0.99 + P4Y*-0.02 + P4Z*-0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.18 + 0.00, P4X*0.13 + P4Y*0.18 + P4Z*0.98 + 0.00}, 
+     {P5X*0.99 + P5Y*-0.02 + P5Z*-0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.18 + 0.00, P5X*0.13 + P5Y*0.18 + P5Z*0.98 + 0.00}, 
+     {P6X*0.99 + P6Y*-0.02 + P6Z*-0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.18 + 0.00, P6X*0.13 + P6Y*0.18 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*-0.02 + P1Z*-0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.13 + 0.00, P1X*0.18 + P1Y*0.13 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*-0.02 + P2Z*-0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.13 + 0.00, P2X*0.18 + P2Y*0.13 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*-0.02 + P3Z*-0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.13 + 0.00, P3X*0.18 + P3Y*0.13 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*-0.02 + P4Z*-0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.13 + 0.00, P4X*0.18 + P4Y*0.13 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*-0.02 + P5Z*-0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.13 + 0.00, P5X*0.18 + P5Y*0.13 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*-0.02 + P6Z*-0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.13 + 0.00, P6X*0.18 + P6Y*0.13 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*-0.01 + P1Z*-0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.07 + 0.00, P1X*0.21 + P1Y*0.07 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*-0.01 + P2Z*-0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.07 + 0.00, P2X*0.21 + P2Y*0.07 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*-0.01 + P3Z*-0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.07 + 0.00, P3X*0.21 + P3Y*0.07 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*-0.01 + P4Z*-0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.07 + 0.00, P4X*0.21 + P4Y*0.07 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*-0.01 + P5Z*-0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.07 + 0.00, P5X*0.21 + P5Y*0.07 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*-0.01 + P6Z*-0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.07 + 0.00, P6X*0.21 + P6Y*0.07 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*-0.00 + P1Z*-0.22 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.00 + 0.00, P1X*0.22 + P1Y*0.00 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*-0.00 + P2Z*-0.22 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.00 + 0.00, P2X*0.22 + P2Y*0.00 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*-0.00 + P3Z*-0.22 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.00 + 0.00, P3X*0.22 + P3Y*0.00 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*-0.00 + P4Z*-0.22 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.00 + 0.00, P4X*0.22 + P4Y*0.00 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*-0.00 + P5Z*-0.22 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.00 + 0.00, P5X*0.22 + P5Y*0.00 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*-0.00 + P6Z*-0.22 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.00 + 0.00, P6X*0.22 + P6Y*0.00 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*0.01 + P1Z*-0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.07 + 0.00, P1X*0.21 + P1Y*-0.07 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.01 + P2Z*-0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.07 + 0.00, P2X*0.21 + P2Y*-0.07 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.01 + P3Z*-0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.07 + 0.00, P3X*0.21 + P3Y*-0.07 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.01 + P4Z*-0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.07 + 0.00, P4X*0.21 + P4Y*-0.07 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.01 + P5Z*-0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.07 + 0.00, P5X*0.21 + P5Y*-0.07 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.01 + P6Z*-0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.07 + 0.00, P6X*0.21 + P6Y*-0.07 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*0.02 + P1Z*-0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.13 + 0.00, P1X*0.18 + P1Y*-0.13 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*0.02 + P2Z*-0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.13 + 0.00, P2X*0.18 + P2Y*-0.13 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*0.02 + P3Z*-0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.13 + 0.00, P3X*0.18 + P3Y*-0.13 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*0.02 + P4Z*-0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.13 + 0.00, P4X*0.18 + P4Y*-0.13 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*0.02 + P5Z*-0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.13 + 0.00, P5X*0.18 + P5Y*-0.13 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*0.02 + P6Z*-0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.13 + 0.00, P6X*0.18 + P6Y*-0.13 + P6Z*0.98 + 0.00}},
+    {{P1X*0.99 + P1Y*0.02 + P1Z*-0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.18 + 0.00, P1X*0.13 + P1Y*-0.18 + P1Z*0.98 + 0.00}, 
+     {P2X*0.99 + P2Y*0.02 + P2Z*-0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.18 + 0.00, P2X*0.13 + P2Y*-0.18 + P2Z*0.98 + 0.00}, 
+     {P3X*0.99 + P3Y*0.02 + P3Z*-0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.18 + 0.00, P3X*0.13 + P3Y*-0.18 + P3Z*0.98 + 0.00}, 
+     {P4X*0.99 + P4Y*0.02 + P4Z*-0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.18 + 0.00, P4X*0.13 + P4Y*-0.18 + P4Z*0.98 + 0.00}, 
+     {P5X*0.99 + P5Y*0.02 + P5Z*-0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.18 + 0.00, P5X*0.13 + P5Y*-0.18 + P5Z*0.98 + 0.00}, 
+     {P6X*0.99 + P6Y*0.02 + P6Z*-0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.18 + 0.00, P6X*0.13 + P6Y*-0.18 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.01 + P1Z*-0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + 0.00, P1X*0.07 + P1Y*-0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.01 + P2Z*-0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + 0.00, P2X*0.07 + P2Y*-0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.01 + P3Z*-0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + 0.00, P3X*0.07 + P3Y*-0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.01 + P4Z*-0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + 0.00, P4X*0.07 + P4Y*-0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.01 + P5Z*-0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + 0.00, P5X*0.07 + P5Y*-0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.01 + P6Z*-0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + 0.00, P6X*0.07 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*0.00 + P1Z*-0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.22 + 0.00, P1X*0.00 + P1Y*-0.22 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*0.00 + P2Z*-0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.22 + 0.00, P2X*0.00 + P2Y*-0.22 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*0.00 + P3Z*-0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.22 + 0.00, P3X*0.00 + P3Y*-0.22 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*0.00 + P4Z*-0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.22 + 0.00, P4X*0.00 + P4Y*-0.22 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*0.00 + P5Z*-0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.22 + 0.00, P5X*0.00 + P5Y*-0.22 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*0.00 + P6Z*-0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.22 + 0.00, P6X*0.00 + P6Y*-0.22 + P6Z*0.98 + 0.00}},
+    {{P1X*1.00 + P1Y*-0.01 + P1Z*0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + 0.00, P1X*-0.07 + P1Y*-0.21 + P1Z*0.98 + 0.00}, 
+     {P2X*1.00 + P2Y*-0.01 + P2Z*0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + 0.00, P2X*-0.07 + P2Y*-0.21 + P2Z*0.98 + 0.00}, 
+     {P3X*1.00 + P3Y*-0.01 + P3Z*0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + 0.00, P3X*-0.07 + P3Y*-0.21 + P3Z*0.98 + 0.00}, 
+     {P4X*1.00 + P4Y*-0.01 + P4Z*0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + 0.00, P4X*-0.07 + P4Y*-0.21 + P4Z*0.98 + 0.00}, 
+     {P5X*1.00 + P5Y*-0.01 + P5Z*0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + 0.00, P5X*-0.07 + P5Y*-0.21 + P5Z*0.98 + 0.00}, 
+     {P6X*1.00 + P6Y*-0.01 + P6Z*0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + 0.00, P6X*-0.07 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
+    {{P1X*0.99 + P1Y*-0.02 + P1Z*0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.18 + 0.00, P1X*-0.13 + P1Y*-0.18 + P1Z*0.98 + 0.00}, 
+     {P2X*0.99 + P2Y*-0.02 + P2Z*0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.18 + 0.00, P2X*-0.13 + P2Y*-0.18 + P2Z*0.98 + 0.00}, 
+     {P3X*0.99 + P3Y*-0.02 + P3Z*0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.18 + 0.00, P3X*-0.13 + P3Y*-0.18 + P3Z*0.98 + 0.00}, 
+     {P4X*0.99 + P4Y*-0.02 + P4Z*0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.18 + 0.00, P4X*-0.13 + P4Y*-0.18 + P4Z*0.98 + 0.00}, 
+     {P5X*0.99 + P5Y*-0.02 + P5Z*0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.18 + 0.00, P5X*-0.13 + P5Y*-0.18 + P5Z*0.98 + 0.00}, 
+     {P6X*0.99 + P6Y*-0.02 + P6Z*0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.18 + 0.00, P6X*-0.13 + P6Y*-0.18 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*-0.02 + P1Z*0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.13 + 0.00, P1X*-0.18 + P1Y*-0.13 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*-0.02 + P2Z*0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.13 + 0.00, P2X*-0.18 + P2Y*-0.13 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*-0.02 + P3Z*0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.13 + 0.00, P3X*-0.18 + P3Y*-0.13 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*-0.02 + P4Z*0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.13 + 0.00, P4X*-0.18 + P4Y*-0.13 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*-0.02 + P5Z*0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.13 + 0.00, P5X*-0.18 + P5Y*-0.13 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*-0.02 + P6Z*0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.13 + 0.00, P6X*-0.18 + P6Y*-0.13 + P6Z*0.98 + 0.00}},
+    {{P1X*0.98 + P1Y*-0.01 + P1Z*0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.07 + 0.00, P1X*-0.21 + P1Y*-0.07 + P1Z*0.98 + 0.00}, 
+     {P2X*0.98 + P2Y*-0.01 + P2Z*0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.07 + 0.00, P2X*-0.21 + P2Y*-0.07 + P2Z*0.98 + 0.00}, 
+     {P3X*0.98 + P3Y*-0.01 + P3Z*0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.07 + 0.00, P3X*-0.21 + P3Y*-0.07 + P3Z*0.98 + 0.00}, 
+     {P4X*0.98 + P4Y*-0.01 + P4Z*0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.07 + 0.00, P4X*-0.21 + P4Y*-0.07 + P4Z*0.98 + 0.00}, 
+     {P5X*0.98 + P5Y*-0.01 + P5Z*0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.07 + 0.00, P5X*-0.21 + P5Y*-0.07 + P5Z*0.98 + 0.00}, 
+     {P6X*0.98 + P6Y*-0.01 + P6Z*0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.07 + 0.00, P6X*-0.21 + P6Y*-0.07 + P6Z*0.98 + 0.00}},
+};
+const int rotatez_entries[] { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19 };
+const MovementTable rotatez_table {rotatez_paths, 20, 50, rotatez_entries, 20 };
+
+const Locations shiftleft_paths[] {
+    {{P1X+(-0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(-6.18), P1Y+(0.00), P1Z+(19.02)}, {P2X+(4.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-6.18), P3Y+(0.00), P3Z+(19.02)}, {P4X+(4.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-6.18), P5Y+(0.00), P5Z+(19.02)}, {P6X+(4.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-11.76), P1Y+(0.00), P1Z+(16.18)}, {P2X+(8.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-11.76), P3Y+(0.00), P3Z+(16.18)}, {P4X+(8.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-11.76), P5Y+(0.00), P5Z+(16.18)}, {P6X+(8.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-16.18), P1Y+(0.00), P1Z+(11.76)}, {P2X+(12.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-16.18), P3Y+(0.00), P3Z+(11.76)}, {P4X+(12.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-16.18), P5Y+(0.00), P5Z+(11.76)}, {P6X+(12.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-19.02), P1Y+(0.00), P1Z+(6.18)}, {P2X+(16.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-19.02), P3Y+(0.00), P3Z+(6.18)}, {P4X+(16.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-19.02), P5Y+(0.00), P5Z+(6.18)}, {P6X+(16.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-16.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(19.02), P2Y+(-0.00), P2Z+(6.18)}, {P3X+(-16.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(19.02), P4Y+(-0.00), P4Z+(6.18)}, {P5X+(-16.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(19.02), P6Y+(-0.00), P6Z+(6.18)}},
+    {{P1X+(-12.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(16.18), P2Y+(-0.00), P2Z+(11.76)}, {P3X+(-12.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(16.18), P4Y+(-0.00), P4Z+(11.76)}, {P5X+(-12.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(16.18), P6Y+(-0.00), P6Z+(11.76)}},
+    {{P1X+(-8.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(11.76), P2Y+(-0.00), P2Z+(16.18)}, {P3X+(-8.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(11.76), P4Y+(-0.00), P4Z+(16.18)}, {P5X+(-8.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(11.76), P6Y+(-0.00), P6Z+(16.18)}},
+    {{P1X+(-4.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(6.18), P2Y+(-0.00), P2Z+(19.02)}, {P3X+(-4.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(6.18), P4Y+(-0.00), P4Z+(19.02)}, {P5X+(-4.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(6.18), P6Y+(-0.00), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(0.00), P6Z+(20.00)}},
+    {{P1X+(4.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-6.18), P2Y+(0.00), P2Z+(19.02)}, {P3X+(4.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-6.18), P4Y+(0.00), P4Z+(19.02)}, {P5X+(4.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-6.18), P6Y+(0.00), P6Z+(19.02)}},
+    {{P1X+(8.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-11.76), P2Y+(0.00), P2Z+(16.18)}, {P3X+(8.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-11.76), P4Y+(0.00), P4Z+(16.18)}, {P5X+(8.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-11.76), P6Y+(0.00), P6Z+(16.18)}},
+    {{P1X+(12.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-16.18), P2Y+(0.00), P2Z+(11.76)}, {P3X+(12.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-16.18), P4Y+(0.00), P4Z+(11.76)}, {P5X+(12.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-16.18), P6Y+(0.00), P6Z+(11.76)}},
+    {{P1X+(16.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-19.02), P2Y+(0.00), P2Z+(6.18)}, {P3X+(16.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-19.02), P4Y+(0.00), P4Z+(6.18)}, {P5X+(16.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-19.02), P6Y+(0.00), P6Z+(6.18)}},
+    {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(19.02), P1Y+(-0.00), P1Z+(6.18)}, {P2X+(-16.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(19.02), P3Y+(-0.00), P3Z+(6.18)}, {P4X+(-16.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(19.02), P5Y+(-0.00), P5Z+(6.18)}, {P6X+(-16.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(16.18), P1Y+(-0.00), P1Z+(11.76)}, {P2X+(-12.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(16.18), P3Y+(-0.00), P3Z+(11.76)}, {P4X+(-12.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(16.18), P5Y+(-0.00), P5Z+(11.76)}, {P6X+(-12.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(11.76), P1Y+(-0.00), P1Z+(16.18)}, {P2X+(-8.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(11.76), P3Y+(-0.00), P3Z+(16.18)}, {P4X+(-8.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(11.76), P5Y+(-0.00), P5Z+(16.18)}, {P6X+(-8.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(6.18), P1Y+(-0.00), P1Z+(19.02)}, {P2X+(-4.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(6.18), P3Y+(-0.00), P3Z+(19.02)}, {P4X+(-4.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(6.18), P5Y+(-0.00), P5Z+(19.02)}, {P6X+(-4.00), P6Y+(0.00), P6Z+(0.00)}},
+};
+const int shiftleft_entries[] { 0,10 };
+const MovementTable shiftleft_table {shiftleft_paths, 20, 20, shiftleft_entries, 2 };
+
+const Locations shiftright_paths[] {
+    {{P1X+(0.00), P1Y+(-0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(6.18), P1Y+(-0.00), P1Z+(19.02)}, {P2X+(-4.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(6.18), P3Y+(-0.00), P3Z+(19.02)}, {P4X+(-4.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(6.18), P5Y+(-0.00), P5Z+(19.02)}, {P6X+(-4.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(11.76), P1Y+(-0.00), P1Z+(16.18)}, {P2X+(-8.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(11.76), P3Y+(-0.00), P3Z+(16.18)}, {P4X+(-8.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(11.76), P5Y+(-0.00), P5Z+(16.18)}, {P6X+(-8.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(16.18), P1Y+(-0.00), P1Z+(11.76)}, {P2X+(-12.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(16.18), P3Y+(-0.00), P3Z+(11.76)}, {P4X+(-12.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(16.18), P5Y+(-0.00), P5Z+(11.76)}, {P6X+(-12.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(19.02), P1Y+(-0.00), P1Z+(6.18)}, {P2X+(-16.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(19.02), P3Y+(-0.00), P3Z+(6.18)}, {P4X+(-16.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(19.02), P5Y+(-0.00), P5Z+(6.18)}, {P6X+(-16.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(16.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-19.02), P2Y+(0.00), P2Z+(6.18)}, {P3X+(16.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-19.02), P4Y+(0.00), P4Z+(6.18)}, {P5X+(16.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-19.02), P6Y+(0.00), P6Z+(6.18)}},
+    {{P1X+(12.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-16.18), P2Y+(0.00), P2Z+(11.76)}, {P3X+(12.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-16.18), P4Y+(0.00), P4Z+(11.76)}, {P5X+(12.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-16.18), P6Y+(0.00), P6Z+(11.76)}},
+    {{P1X+(8.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-11.76), P2Y+(0.00), P2Z+(16.18)}, {P3X+(8.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-11.76), P4Y+(0.00), P4Z+(16.18)}, {P5X+(8.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-11.76), P6Y+(0.00), P6Z+(16.18)}},
+    {{P1X+(4.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-6.18), P2Y+(0.00), P2Z+(19.02)}, {P3X+(4.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-6.18), P4Y+(0.00), P4Z+(19.02)}, {P5X+(4.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-6.18), P6Y+(0.00), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-0.00), P6Z+(20.00)}},
+    {{P1X+(-4.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(6.18), P2Y+(-0.00), P2Z+(19.02)}, {P3X+(-4.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(6.18), P4Y+(-0.00), P4Z+(19.02)}, {P5X+(-4.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(6.18), P6Y+(-0.00), P6Z+(19.02)}},
+    {{P1X+(-8.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(11.76), P2Y+(-0.00), P2Z+(16.18)}, {P3X+(-8.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(11.76), P4Y+(-0.00), P4Z+(16.18)}, {P5X+(-8.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(11.76), P6Y+(-0.00), P6Z+(16.18)}},
+    {{P1X+(-12.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(16.18), P2Y+(-0.00), P2Z+(11.76)}, {P3X+(-12.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(16.18), P4Y+(-0.00), P4Z+(11.76)}, {P5X+(-12.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(16.18), P6Y+(-0.00), P6Z+(11.76)}},
+    {{P1X+(-16.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(19.02), P2Y+(-0.00), P2Z+(6.18)}, {P3X+(-16.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(19.02), P4Y+(-0.00), P4Z+(6.18)}, {P5X+(-16.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(19.02), P6Y+(-0.00), P6Z+(6.18)}},
+    {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-19.02), P1Y+(0.00), P1Z+(6.18)}, {P2X+(16.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-19.02), P3Y+(0.00), P3Z+(6.18)}, {P4X+(16.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-19.02), P5Y+(0.00), P5Z+(6.18)}, {P6X+(16.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-16.18), P1Y+(0.00), P1Z+(11.76)}, {P2X+(12.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-16.18), P3Y+(0.00), P3Z+(11.76)}, {P4X+(12.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-16.18), P5Y+(0.00), P5Z+(11.76)}, {P6X+(12.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-11.76), P1Y+(0.00), P1Z+(16.18)}, {P2X+(8.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-11.76), P3Y+(0.00), P3Z+(16.18)}, {P4X+(8.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-11.76), P5Y+(0.00), P5Z+(16.18)}, {P6X+(8.00), P6Y+(-0.00), P6Z+(0.00)}},
+    {{P1X+(-6.18), P1Y+(0.00), P1Z+(19.02)}, {P2X+(4.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-6.18), P3Y+(0.00), P3Z+(19.02)}, {P4X+(4.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-6.18), P5Y+(0.00), P5Z+(19.02)}, {P6X+(4.00), P6Y+(-0.00), P6Z+(0.00)}},
+};
+const int shiftright_entries[] { 0,10 };
+const MovementTable shiftright_table {shiftright_paths, 20, 20, shiftright_entries, 2 };
+
+const Locations turnleft_paths[] {
+    {{P1X+(-0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(-4.37), P1Y+(4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(4.37), P3Y+(4.37), P3Z+(19.02)}, {P4X+(-2.83), P4Y+(2.83), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(2.83), P6Y+(2.83), P6Z+(0.00)}},
+    {{P1X+(-8.31), P1Y+(8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(8.31), P3Y+(8.31), P3Z+(16.18)}, {P4X+(-5.66), P4Y+(5.66), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(5.66), P6Y+(5.66), P6Z+(0.00)}},
+    {{P1X+(-11.44), P1Y+(11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(11.44), P3Y+(11.44), P3Z+(11.76)}, {P4X+(-8.49), P4Y+(8.49), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(8.49), P6Y+(8.49), P6Z+(0.00)}},
+    {{P1X+(-13.45), P1Y+(13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(13.45), P3Y+(13.45), P3Z+(6.18)}, {P4X+(-11.31), P4Y+(11.31), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(11.31), P6Y+(11.31), P6Z+(0.00)}},
+    {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
+    {{P1X+(-11.31), P1Y+(11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(11.31), P3Y+(11.31), P3Z+(0.00)}, {P4X+(-13.45), P4Y+(13.45), P4Z+(6.18)}, {P5X+(-0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(13.45), P6Y+(13.45), P6Z+(6.18)}},
+    {{P1X+(-8.49), P1Y+(8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(8.49), P3Y+(8.49), P3Z+(0.00)}, {P4X+(-11.44), P4Y+(11.44), P4Z+(11.76)}, {P5X+(-0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(11.44), P6Y+(11.44), P6Z+(11.76)}},
+    {{P1X+(-5.66), P1Y+(5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(5.66), P3Y+(5.66), P3Z+(0.00)}, {P4X+(-8.31), P4Y+(8.31), P4Z+(16.18)}, {P5X+(-0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(8.31), P6Y+(8.31), P6Z+(16.18)}},
+    {{P1X+(-2.83), P1Y+(2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(2.83), P3Y+(2.83), P3Z+(0.00)}, {P4X+(-4.37), P4Y+(4.37), P4Z+(19.02)}, {P5X+(-0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(4.37), P6Y+(4.37), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-0.00), P6Z+(20.00)}},
+    {{P1X+(2.83), P1Y+(-2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(-2.83), P3Y+(-2.83), P3Z+(0.00)}, {P4X+(4.37), P4Y+(-4.37), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(-4.37), P6Y+(-4.37), P6Z+(19.02)}},
+    {{P1X+(5.66), P1Y+(-5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(-5.66), P3Y+(-5.66), P3Z+(0.00)}, {P4X+(8.31), P4Y+(-8.31), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(-8.31), P6Y+(-8.31), P6Z+(16.18)}},
+    {{P1X+(8.49), P1Y+(-8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(-8.49), P3Y+(-8.49), P3Z+(0.00)}, {P4X+(11.44), P4Y+(-11.44), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(-11.44), P6Y+(-11.44), P6Z+(11.76)}},
+    {{P1X+(11.31), P1Y+(-11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(-11.31), P3Y+(-11.31), P3Z+(0.00)}, {P4X+(13.45), P4Y+(-13.45), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(-13.45), P6Y+(-13.45), P6Z+(6.18)}},
+    {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
+    {{P1X+(13.45), P1Y+(-13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(-13.45), P3Y+(-13.45), P3Z+(6.18)}, {P4X+(11.31), P4Y+(-11.31), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(-11.31), P6Y+(-11.31), P6Z+(0.00)}},
+    {{P1X+(11.44), P1Y+(-11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(-11.44), P3Y+(-11.44), P3Z+(11.76)}, {P4X+(8.49), P4Y+(-8.49), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(-8.49), P6Y+(-8.49), P6Z+(0.00)}},
+    {{P1X+(8.31), P1Y+(-8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(-8.31), P3Y+(-8.31), P3Z+(16.18)}, {P4X+(5.66), P4Y+(-5.66), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(-5.66), P6Y+(-5.66), P6Z+(0.00)}},
+    {{P1X+(4.37), P1Y+(-4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(-4.37), P3Y+(-4.37), P3Z+(19.02)}, {P4X+(2.83), P4Y+(-2.83), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(-2.83), P6Y+(-2.83), P6Z+(0.00)}},
+};
+const int turnleft_entries[] { 0,10 };
+const MovementTable turnleft_table {turnleft_paths, 20, 20, turnleft_entries, 2 };
+
+const Locations turnright_paths[] {
+    {{P1X+(0.00), P1Y+(-0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(-0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
+    {{P1X+(4.37), P1Y+(-4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(-4.37), P3Y+(-4.37), P3Z+(19.02)}, {P4X+(2.83), P4Y+(-2.83), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(-2.83), P6Y+(-2.83), P6Z+(0.00)}},
+    {{P1X+(8.31), P1Y+(-8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(-8.31), P3Y+(-8.31), P3Z+(16.18)}, {P4X+(5.66), P4Y+(-5.66), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(-5.66), P6Y+(-5.66), P6Z+(0.00)}},
+    {{P1X+(11.44), P1Y+(-11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(-11.44), P3Y+(-11.44), P3Z+(11.76)}, {P4X+(8.49), P4Y+(-8.49), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(-8.49), P6Y+(-8.49), P6Z+(0.00)}},
+    {{P1X+(13.45), P1Y+(-13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(-13.45), P3Y+(-13.45), P3Z+(6.18)}, {P4X+(11.31), P4Y+(-11.31), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(-11.31), P6Y+(-11.31), P6Z+(0.00)}},
+    {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
+    {{P1X+(11.31), P1Y+(-11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(-11.31), P3Y+(-11.31), P3Z+(0.00)}, {P4X+(13.45), P4Y+(-13.45), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(-13.45), P6Y+(-13.45), P6Z+(6.18)}},
+    {{P1X+(8.49), P1Y+(-8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(-8.49), P3Y+(-8.49), P3Z+(0.00)}, {P4X+(11.44), P4Y+(-11.44), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(-11.44), P6Y+(-11.44), P6Z+(11.76)}},
+    {{P1X+(5.66), P1Y+(-5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(-5.66), P3Y+(-5.66), P3Z+(0.00)}, {P4X+(8.31), P4Y+(-8.31), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(-8.31), P6Y+(-8.31), P6Z+(16.18)}},
+    {{P1X+(2.83), P1Y+(-2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(-2.83), P3Y+(-2.83), P3Z+(0.00)}, {P4X+(4.37), P4Y+(-4.37), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(-4.37), P6Y+(-4.37), P6Z+(19.02)}},
+    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}},
+    {{P1X+(-2.83), P1Y+(2.83), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(2.83), P3Y+(2.83), P3Z+(0.00)}, {P4X+(-4.37), P4Y+(4.37), P4Z+(19.02)}, {P5X+(-0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(4.37), P6Y+(4.37), P6Z+(19.02)}},
+    {{P1X+(-5.66), P1Y+(5.66), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(5.66), P3Y+(5.66), P3Z+(0.00)}, {P4X+(-8.31), P4Y+(8.31), P4Z+(16.18)}, {P5X+(-0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(8.31), P6Y+(8.31), P6Z+(16.18)}},
+    {{P1X+(-8.49), P1Y+(8.49), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(8.49), P3Y+(8.49), P3Z+(0.00)}, {P4X+(-11.44), P4Y+(11.44), P4Z+(11.76)}, {P5X+(-0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(11.44), P6Y+(11.44), P6Z+(11.76)}},
+    {{P1X+(-11.31), P1Y+(11.31), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(11.31), P3Y+(11.31), P3Z+(0.00)}, {P4X+(-13.45), P4Y+(13.45), P4Z+(6.18)}, {P5X+(-0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(13.45), P6Y+(13.45), P6Z+(6.18)}},
+    {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
+    {{P1X+(-13.45), P1Y+(13.45), P1Z+(6.18)}, {P2X+(-0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(13.45), P3Y+(13.45), P3Z+(6.18)}, {P4X+(-11.31), P4Y+(11.31), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(11.31), P6Y+(11.31), P6Z+(0.00)}},
+    {{P1X+(-11.44), P1Y+(11.44), P1Z+(11.76)}, {P2X+(-0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(11.44), P3Y+(11.44), P3Z+(11.76)}, {P4X+(-8.49), P4Y+(8.49), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(8.49), P6Y+(8.49), P6Z+(0.00)}},
+    {{P1X+(-8.31), P1Y+(8.31), P1Z+(16.18)}, {P2X+(-0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(8.31), P3Y+(8.31), P3Z+(16.18)}, {P4X+(-5.66), P4Y+(5.66), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(5.66), P6Y+(5.66), P6Z+(0.00)}},
+    {{P1X+(-4.37), P1Y+(4.37), P1Z+(19.02)}, {P2X+(-0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(4.37), P3Y+(4.37), P3Z+(19.02)}, {P4X+(-2.83), P4Y+(2.83), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(2.83), P6Y+(2.83), P6Z+(0.00)}},
+};
+const int turnright_entries[] { 0,10 };
+const MovementTable turnright_table {turnright_paths, 20, 20, turnright_entries, 2 };
+}
+
+const MovementTable& backwardTable() {
+    return backward_table;
+}
+const MovementTable& forwardTable() {
+    return forward_table;
+}
+const MovementTable& rotatexTable() {
+    return rotatex_table;
+}
+const MovementTable& rotateyTable() {
+    return rotatey_table;
+}
+const MovementTable& rotatezTable() {
+    return rotatez_table;
+}
+const MovementTable& shiftleftTable() {
+    return shiftleft_table;
+}
+const MovementTable& shiftrightTable() {
+    return shiftright_table;
+}
+const MovementTable& turnleftTable() {
+    return turnleft_table;
+}
+const MovementTable& turnrightTable() {
+    return turnright_table;
+}

software/hexapod7697/src/hexapod/movements.cpp

@@ -0,0 +1,59 @@
+#include "movement.h"
+#include "config.h"
+
+using namespace hexapod::config;
+using namespace hexapod;
+
+#define SIN30   0.5
+#define COS30   0.866
+#define SIN45   0.7071
+#define COS45   0.7071
+#define SIN15   0.2588
+#define COS15   0.9659
+
+#define STANDBY_Z   (kLegJoint3ToTip*COS15-kLegJoint2ToJoint3*SIN30)
+#define LEFTRIGHT_X (kLegMountLeftRightX+kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)
+#define OTHER_X (kLegMountOtherX + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*COS45)
+#define OTHER_Y (kLegMountOtherY + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*SIN45)
+
+#define P1X     OTHER_X
+#define P1Y     OTHER_Y
+#define P1Z     -STANDBY_Z
+
+#define P2X     LEFTRIGHT_X
+#define P2Y     0
+#define P2Z     -STANDBY_Z
+
+#define P3X     OTHER_X
+#define P3Y     -OTHER_Y
+#define P3Z     -STANDBY_Z
+
+#define P4X     -OTHER_X
+#define P4Y     -OTHER_Y
+#define P4Z     -STANDBY_Z
+
+#define P5X     -LEFTRIGHT_X
+#define P5Y     0
+#define P5Z     -STANDBY_Z
+
+#define P6X     -OTHER_X
+#define P6Y     OTHER_Y
+#define P6Z     -STANDBY_Z
+
+namespace hexapod {
+
+    namespace {
+        const Locations kStandby {
+            {P1X, P1Y, P1Z}, {P2X, P2Y, P2Z}, {P3X, P3Y, P3Z}, {P4X, P4Y, P4Z}, {P5X, P5Y, P5Z}, {P6X, P6Y, P6Z}
+        };
+        const int zero = 0;
+
+        const MovementTable standay_table {&kStandby, 1, 20, &zero, 1};
+    }
+
+    const MovementTable& standbyTable() {
+        return standay_table;
+    }
+
+    #include "movement_table.h"
+}

software/hexapod7697/src/hexapod/servo.cpp

@@ -0,0 +1,117 @@
+#include "servo.h"
+#include "debug.h"
+
+#include "hal/pwm.h"
+
+namespace hexapod { 
+
+    namespace {
+        // if freq = 240, 1 tick = 1000000 / (240*4096) ~= 1us
+        // if freq = 120, 1 tick = 1000000 / (120*4096) ~= 2us
+        const static int kFrequency = 120;
+        const static int kTickUs = 2;
+        const static int kServoMiddle = 1400;
+        const static int kServoMax = 2000;
+        const static int kServoMin = 800;
+        const static int kServoRange = kServoMax-kServoMiddle;
+
+        auto pwmLeft = hal::PCA9685(0x41);
+        auto pwmRight = hal::PCA9685(0x40);
+        bool pwmInited = false;
+ 
+        void initPWM(void) {
+            if (pwmInited)
+                return;
+
+            pwmLeft.begin();  
+            pwmLeft.setPWMFreq(kFrequency);
+            pwmRight.begin();  
+            pwmRight.setPWMFreq(kFrequency);
+            pwmInited = true;
+        }
+
+        int hexapod2pwm(int legIndex, int partIndex) {
+            switch(legIndex) {
+            case 0:     return 5 + partIndex;
+            case 1:     return 2 + partIndex;
+            case 2:     return 8 + partIndex;
+            case 3:     return 16 + 8 + partIndex;
+            case 4:     return 16 + 2 + partIndex;
+            case 5:     return 16 + 5 + partIndex;
+            default:    return 0;
+            }
+        }
+
+        int pwm2hexapod(int pwm) {
+            if(pwm >= 16+8)
+                return 9 + pwm - (16+8);
+            if(pwm >= 16+5)
+                return 15 + pwm - (16+5);
+            if(pwm >= 16+2)
+                return 12 + pwm - (16+2);
+            if(pwm >= 8)
+                return 6 + pwm - (8);
+            if(pwm >= 5)
+                return 0 + pwm - (5);
+            if(pwm >= 2)
+                return 3 + pwm - (2);
+            return -1;
+        }
+
+    }
+
+    void Servo::init(void) {
+        initPWM();
+    }
+
+    Servo::Servo(int legIndex, int partIndex) {
+        pwmIndex_ = hexapod2pwm(legIndex, partIndex);
+        inverse_ = partIndex == 1 ? true : false;
+        adjust_angle_ = partIndex == 1 ? 15.0 : 0.0;
+        range_ = partIndex == 0 ? 45 : 60;
+        angle_ = 0;
+    }
+
+    void Servo::setAngle(float angle) {
+        initPWM();
+
+        if (angle > range_ + adjust_angle_) {
+            LOG_INFO("exceed[%d][%f]", pwm2hexapod(pwmIndex_), angle);
+            angle = range_;
+        }
+        else if(angle < -range_ + adjust_angle_) {
+            LOG_INFO("exceed[%d][%f]", pwm2hexapod(pwmIndex_), angle);
+            angle = -range_;
+        }
+
+        angle_ = angle;
+
+        hal::PCA9685* pwm;
+        int idx;
+        if (pwmIndex_ < 16) {
+            pwm = &pwmRight;
+            idx = pwmIndex_;
+        }
+        else {
+            pwm = &pwmLeft;
+            idx = pwmIndex_ - 16;
+        }
+        angle -= adjust_angle_;
+        if (inverse_)
+            angle = -angle;
+
+        int us = kServoMiddle + offset_ + angle*(kServoRange/60)*(1+0.01*scale_);
+        if (us > kServoMax)
+            us = kServoMax;
+        else if(us < kServoMin)
+            us = kServoMin;
+
+        pwm->setPWM(idx, 0, us/kTickUs);
+        LOG_DEBUG("setAngle(%.2f, %d)", angle, us);
+    }
+
+    float Servo::getAngle(void) {
+        return angle_;
+    }
+
+}

software/hexapod7697/src/hexapod/servo.h

@@ -0,0 +1,38 @@
+#pragma once
+
+namespace hexapod { 
+
+    class Servo {
+    public:
+        static void init(void);
+
+    public:
+        Servo(int legIndex, int partIndex);
+
+        // angle: 0 means center, range is -60~60
+        void setAngle(float angle);
+        float getAngle(void);
+
+        void getParameter(int& offset, int& scale) {
+            offset = offset_;
+            scale = scale_;
+        }
+
+        void setParameter(int offset, int scale, bool update = true) {
+            offset_ = offset;
+            scale_ = scale;
+            if (update)
+                setAngle(angle_);
+        }
+
+    private:
+        float angle_;
+        int pwmIndex_;
+        bool inverse_;
+        int offset_;
+        int scale_;
+        int range_;
+        float adjust_angle_;
+    };
+
+}

software/hexapod7697/src/linkit_control/ui_controls.cpp

@@ -0,0 +1,83 @@
+#include <Arduino.h>
+#include "ui_controls.h"
+
+namespace {
+  const static int kRepeatPeriod1 = 500;
+  const static int kRepeatPeriod1Count = 4;
+  const static int kRepeatPeriod2 = 100;
+}
+
+void ButtonGroup::addControl(Button &button) {
+  // prevent duplicated entry
+  for (const auto control : controls_) {
+    if (control == &button) {
+      return;
+    }
+  }
+  controls_.push_back(&button);
+};
+
+void ButtonGroup::addControl(Button *button) {
+  // prevent duplicated entry
+  for (const auto control : controls_) {
+    if (control == button) {
+      return;
+    }
+  }
+  controls_.push_back(button);
+};
+
+void ButtonGroup::addControls(std::vector<Button *> lists) {
+  for (auto button : lists) {
+    addControl(button);
+  }
+}
+
+void ButtonGroup::process() {
+  if (!callback_)
+    return;
+
+  if (pressed_) {
+    if (pressed_->isValueChanged() && pressed_->getValue() == 0) {
+      callback_(pressed_->_id, UP);
+      pressed_ = nullptr;
+    }
+    else if (millis() >= time_repeat_) {
+      callback_(pressed_->_id, REPEAT);
+      count_repeat_ += 1;
+      if (count_repeat_ < kRepeatPeriod1Count)
+        time_repeat_ = millis() + kRepeatPeriod1;
+      else
+        time_repeat_ = millis() + kRepeatPeriod2;
+    }
+  }
+  else {
+    for (auto control : controls_) {
+      if (control->isValueChanged() && control->getValue() == 1) {
+        // only 1 button can be pressed at the same time within the same group
+        pressed_ = control;
+        time_repeat_ = millis() + kRepeatPeriod1;
+        count_repeat_ = 0;
+        callback_(control->_id, DOWN);
+        return;
+      }
+    }
+  }
+}
+
+int ButtonGroup::getPressFlag() {
+  int flag = 0;
+  for (auto control : controls_) {
+    if (control->getValue() == 1) {
+      if (control->_id < 32)
+        flag |= (1<<control->_id);
+    }
+  }
+  return flag;
+}
+
+void LRemote_addControls(std::vector<LRemoteUIControl *> lists) {
+  for (auto control : lists) {
+    LRemote.addControl(*control);
+  }
+}

software/hexapod7697/src/linkit_control/ui_controls.h

@@ -0,0 +1,65 @@
+#pragma once
+
+#ifndef __LREMOTE_H__
+#include <LRemote.h>
+#define __LREMOTE_H__
+#endif
+
+class Label : public LRemoteLabel {
+public:
+  Label(const String &text, uint8_t x, uint8_t y, uint8_t w, uint8_t h, RCColorType color = RC_GREY) : LRemoteLabel() {
+    setText(text);
+    setPos(x, y);
+    setSize(w, h);
+    setColor(color);
+  }
+};
+
+
+class Button : public LRemoteButton {
+public:
+  Button(int id, const String &text, uint8_t x, uint8_t y, uint8_t w, uint8_t h, RCColorType color = RC_GREY) : LRemoteButton() {
+    setText(text);
+    setPos(x, y);
+    setSize(w, h);
+    setColor(color);
+    _id = id;
+  }
+
+public:
+  int _id;
+};
+
+#include <vector>
+#include <functional>
+
+class ButtonGroup {
+public:
+
+  enum mode {
+    NONE = 0,
+    DOWN,
+    UP,
+    REPEAT
+  };
+
+  void config(std::function<void(int, mode)> callback) {
+    callback_ = callback;
+  };
+
+  void addControl(Button &button);
+  void addControl(Button *button);
+  void addControls(std::vector<Button *> lists);
+  void process();
+
+  int getPressFlag();
+
+public:
+  std::vector<Button *> controls_;
+  std::function<void(int, mode)> callback_ = nullptr;
+  Button *pressed_ = nullptr;
+  int time_repeat_ = 0;
+  int count_repeat_ = 0;
+};
+
+void LRemote_addControls(std::vector<LRemoteUIControl *> lists);

software/hexapod7697/src/normal_mode.cpp

@@ -0,0 +1,86 @@
+#include <Arduino.h>
+
+#ifndef __LREMOTE_H__
+#include <LRemote.h>
+#define __LREMOTE_H__
+#endif
+
+#include "linkit_control/ui_controls.h"
+#include "hexapod/debug.h"
+#include "hexapod/config.h"
+#include "hexapod/hexapod.h"
+
+#define REACT_DELAY hexapod::config::movementInterval
+
+static Button buttonForward(hexapod::MOVEMENT_FORWARD, "Forward", 1, 1, 1, 1, RC_BLUE);
+static Button buttonBackward(hexapod::MOVEMENT_BACKWARD, "Backward", 1, 3, 1, 1, RC_BLUE);
+static Button buttonTL(hexapod::MOVEMENT_TURNLEFT, "TurnLeft", 0, 2, 1, 1, RC_BLUE);
+static Button buttonTR(hexapod::MOVEMENT_TURNRIGHT, "TurnRight", 2, 2, 1, 1, RC_BLUE);
+static Button buttonSL(hexapod::MOVEMENT_SHIFTLEFT, "ShiftLeft", 0, 3, 1, 1, RC_YELLOW);
+static Button buttonSR(hexapod::MOVEMENT_SHIFTRIGHT, "ShiftRight", 2, 3, 1, 1, RC_YELLOW);
+static Button buttonRotateX(hexapod::MOVEMENT_ROTATEX, "RotateX", 0, 0, 1, 1, RC_YELLOW);
+static Button buttonRotateY(hexapod::MOVEMENT_ROTATEY, "RotateY", 1, 0, 1, 1, RC_YELLOW);
+static Button buttonRotateZ(hexapod::MOVEMENT_ROTATEZ, "RotateZ", 2, 0, 1, 1, RC_YELLOW);
+
+static ButtonGroup btnGroup;
+
+static void log_output(const char* log) {
+  Serial.println(log);
+}
+
+void normal_setup(void) {
+  LRemote.setName("Hexapod");
+  LRemote.setOrientation(RC_PORTRAIT);
+  LRemote.setGrid(3, 5);
+
+  LRemote_addControls({
+    &buttonForward, &buttonBackward,
+    &buttonTL, &buttonTR,
+    &buttonSL, &buttonSR,
+    &buttonRotateX, &buttonRotateY, &buttonRotateZ,
+  });
+  btnGroup.addControls({
+    &buttonForward, &buttonBackward,
+    &buttonTL, &buttonTR,
+    &buttonSL, &buttonSR,
+    &buttonRotateX, &buttonRotateY, &buttonRotateZ,
+  });
+
+  LRemote.begin();
+}
+
+void normal_loop(void) {
+
+  // check if we are connect by some 
+  // BLE central device, e.g. an mobile app
+  if(!LRemote.connected()) {
+    delay(1000-REACT_DELAY);
+  }
+
+  auto t0 = millis();
+
+  // Process the incoming BLE write request
+  // and translate them to control events
+  LRemote.process();  
+
+  auto flag = btnGroup.getPressFlag();
+  auto mode = hexapod::MOVEMENT_STANDBY;
+  for (auto m = hexapod::MOVEMENT_STANDBY; m < hexapod::MOVEMENT_TOTAL; m++) {
+    if (flag & (1<<m)) {
+      mode = m;
+      break;
+    }
+  }
+
+  hexapod::Hexapod.processMovement(mode, REACT_DELAY);
+
+  auto spent = millis() - t0;
+
+  if(spent < REACT_DELAY) {
+    // Serial.println(spent);
+    delay(REACT_DELAY-spent);
+  }
+  else {
+    Serial.println(spent);
+  }
+}

software/hexapod7697/src/normal_mode.h

@@ -0,0 +1,5 @@
+
+#pragma once
+
+void normal_setup(void);
+void normal_loop(void);

software/hexapod7697/src/setting_mode.cpp

@@ -0,0 +1,220 @@
+#include <Arduino.h>
+
+#ifndef __LREMOTE_H__
+#include <LRemote.h>
+#define __LREMOTE_H__
+#endif
+
+#include "linkit_control/ui_controls.h"
+#include "hexapod/debug.h"
+#include "hexapod/hexapod.h"
+
+#define WIDTH 9
+#define HEIGHT 18
+#define L_W WIDTH
+#define L_H 1
+#define B_W 2
+#define B_H 2
+#define T_W 5
+#define T_H 2
+
+#define L_X(g)  0
+#define L_Y(g)  (L_H+T_H)*g
+#define Bl_X(g)  0
+#define Bl_Y(g)  (L_H+T_H)*g+L_H
+#define T_X(g)  B_W
+#define T_Y(g)  (L_H+T_H)*g+L_H
+#define Br_X(g)  B_W+T_W
+#define Br_Y(g)  (L_H+T_H)*g+L_H
+
+//---------------------------------------------------------------------------
+// UI Group: Adjust setting
+
+static Label label1("Select leg", L_X(0), L_Y(0), L_W, L_H);
+static Label text1("", T_X(0), T_Y(0), T_W, T_H, RC_PINK);
+static Button button1l(10, "<", Bl_X(0), Bl_Y(0), B_W, B_H, RC_BLUE);
+static Button button1r(11, ">", Br_X(0), Br_Y(0), B_W, B_H, RC_BLUE);
+
+static Label label2("Select joint", L_X(1), L_Y(1), L_W, L_H);
+static Label text2("", T_X(1), T_Y(1), T_W, T_H, RC_PINK);
+static Button button2l(20, "<", Bl_X(1), Bl_Y(1), B_W, B_H, RC_BLUE);
+static Button button2r(21, ">", Br_X(1), Br_Y(1), B_W, B_H, RC_BLUE);
+
+static Label label3("Servo offset", L_X(2), L_Y(2), L_W, L_H);
+static Label text3("0", T_X(2), T_Y(2), T_W, T_H, RC_PINK);
+static Button button3l(30, "-", Bl_X(2), Bl_Y(2), B_W, B_H, RC_BLUE);
+static Button button3r(31, "+", Br_X(2), Br_Y(2), B_W, B_H, RC_BLUE);
+
+static Label label4("Servo scale", L_X(3), L_Y(3), L_W, L_H);
+static Label text4("0", T_X(3), T_Y(3), T_W, T_H, RC_PINK);
+static Button button4l(40, "-", Bl_X(3), Bl_Y(3), B_W, B_H, RC_BLUE);
+static Button button4r(41, "+", Br_X(3), Br_Y(3), B_W, B_H, RC_BLUE);
+
+static Button buttonSave(60, "Save", 0, Bl_Y(5), WIDTH, B_H, RC_GREEN);
+
+static ButtonGroup btnGroup;
+
+//---------------------------------------------------------------------------
+// UI Group: Test
+
+static Label label5("Servo test", L_X(4), L_Y(4), L_W, L_H);
+static Button button5l(-45, "-45°", 0, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
+static Button button5c(0, "0°", WIDTH/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
+static Button button5r(45, "+45°", WIDTH*2/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
+
+static ButtonGroup btnGroupTest;
+
+//---------------------------------------------------------------------------
+
+const static String _leg_table[] = {
+  "front right",
+  "right",
+  "rear right",
+  "rear left",
+  "left",
+  "front left",
+};
+
+const static String _joint_table[] = {
+  "body",
+  "thigh",
+  "foot",
+};
+
+static int _cur_leg = 0;
+static int _cur_joint = 0;
+static int _cur_offset = 0;
+static int _cur_scale = 0;
+static int _test_angle[3*6] = {0};
+
+void update_text(void) {
+  text1.updateText(_leg_table[_cur_leg]);
+  text2.updateText(_joint_table[_cur_joint]);
+  text3.updateText(String(_cur_offset));
+  text4.updateText(String(1+0.01*_cur_scale));
+}
+
+static uint8_t led_blink_state = 0;
+
+void setting_setup(void) {
+
+  digitalWrite(LED_BUILTIN, HIGH);
+
+  //
+  // initialize config mode UI
+  //
+  LRemote.setName("Hexapod (S)");
+  LRemote.setOrientation(RC_PORTRAIT);
+  LRemote.setGrid(WIDTH, HEIGHT);
+
+  LRemote_addControls({
+    &label1, &text1, &button1l, &button1r,  // row1
+    &label2, &text2, &button2l, &button2r,  // row2
+    &label3, &text3, &button3l, &button3r,  // row3
+    &label4, &text4, &button4l, &button4r,  // row4
+    &label5, &button5l, &button5c, &button5r, // row5
+    &buttonSave                             // row6
+  });
+
+  //
+  // Button group
+  //  
+  btnGroup.addControls({
+    &button1l, &button1r,
+    &button2l, &button2r,
+    &button3l, &button3r,
+    &button4l, &button4r,
+    &buttonSave
+  });
+
+  btnGroup.config([](int btn_id, ButtonGroup::mode mode) {
+    if (mode != ButtonGroup::mode::DOWN && mode != ButtonGroup::mode::REPEAT)
+      return;
+
+    Serial.print("button id: ");
+    Serial.print(String(btn_id));
+    Serial.println(" is triggered");
+
+    int g = btn_id / 10;
+    int lr = btn_id % 10;
+
+    switch(g) {
+    case 1:
+      _cur_leg = (_cur_leg + 6 + (lr ? 1 : -1)) % 6;
+      hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
+      hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
+      break;
+    case 2:
+      _cur_joint = (_cur_joint + 3 + (lr ? 1 : -1)) % 3;
+      hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
+      hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
+      break;
+    case 3:
+      _cur_offset += (lr ? 1 : -1);
+      hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
+      break;
+    case 4:
+      _cur_scale += (lr ? 1 : -1);
+      hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
+      break;
+    case 6:
+      hexapod::Hexapod.calibrationSave();
+    }
+    update_text();
+  });
+
+  //
+  // Button group: Testing
+  //  
+  btnGroupTest.addControls({
+    &button5l, &button5c, &button5r
+  });
+
+  btnGroupTest.config([](int btn_id, ButtonGroup::mode mode) {
+    if (mode != ButtonGroup::mode::DOWN)
+      return;
+    _test_angle[_cur_leg*3+_cur_joint] = btn_id;
+    hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
+    delay(20);
+  });
+
+  //
+  // Ready to go
+  // 
+  hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
+  update_text();
+
+  for(auto i=0;i<6;i++) {
+    for(auto j=0;j<3;j++) {
+      hexapod::Hexapod.calibrationTest(i, j, _test_angle[i*3+j]);
+    }
+  }
+
+  LRemote.begin();
+}
+
+void setting_loop(void) {
+
+  // check if we are connect by some 
+  // BLE central device, e.g. an mobile app
+  if(!LRemote.connected()) {
+    delay(500);
+    led_blink_state = !led_blink_state;
+  } else {
+    // The interval between button down/up
+    // can be very short - e.g. a quick tap
+    // on the screen.
+    // We could lose some event if we
+    // delay something like 100ms.
+    delay(15);
+    led_blink_state = 1;
+  }
+  digitalWrite(LED_BUILTIN, led_blink_state ? HIGH : LOW);
+
+  // Process the incoming BLE write request
+  // and translate them to control events
+  LRemote.process();  
+
+  btnGroup.process();
+  btnGroupTest.process();
+}

software/hexapod7697/src/setting_mode.h

@@ -0,0 +1,5 @@
+
+#pragma once
+
+void setting_setup(void);
+void setting_loop(void);

software/pathTool/src/main.py

@@ -0,0 +1,95 @@
+import argparse
+import logging
+import os
+import sys
+
+def collectPath(sub_folder):
+    scripts = {}
+    for script_name in [f[:-3] for f in sorted(os.listdir(sub_folder)) if f.endswith('.py') and os.path.isfile(os.path.join(sub_folder, f))]:
+        module = __import__(script_name)
+        if not hasattr(module, 'path_generator'):
+            continue
+        scripts[script_name] = module.path_generator
+
+    return scripts
+
+def show_detail(path, result):
+    print("path:{}:".format(path))
+    for i, p in enumerate(result):
+        print("{:2d}  {:5.2f}, {:5.2f}, {:5.2f}".format(i, p[0], p[1], p[2]))
+
+def generate_c_body(path, params):
+    data, mode, dur, entries = params
+    result = "\nconst Locations {}_paths[] {{\n".format(path)
+
+    if mode == "shift":
+        # data: float[6][N][3]
+
+        assert(len(data) == 6)
+
+        count = len(data[0])
+        for i in range(count):
+            result += "    {" + ", ".join(
+                "{{P{idx}X+({x:.2f}), P{idx}Y+({y:.2f}), P{idx}Z+({z:.2f})}}".format(x=data[j][i][0], y=data[j][i][1], z=data[j][i][2], idx=j+1) 
+                for j in range(6)
+            ) + "},\n"
+
+    elif mode == "matrix":
+        # data: np.matrix[N]
+
+        count = len(data)
+        for i in range(count):
+            result += "    {" + ", \n     ".join(
+                "{{P{idx}X*{e00:.2f} + P{idx}Y*{e01:.2f} + P{idx}Z*{e02:.2f} + {e03:.2f}, P{idx}X*{e10:.2f} + P{idx}Y*{e11:.2f} + P{idx}Z*{e12:.2f} + {e13:.2f}, P{idx}X*{e20:.2f} + P{idx}Y*{e21:.2f} + P{idx}Z*{e22:.2f} + {e23:.2f}}}".format(
+                    e00=data[i].item((0,0)), e01=data[i].item((0,1)), e02=data[i].item((0,2)), e03=data[i].item((0,3)),
+                    e10=data[i].item((1,0)), e11=data[i].item((1,1)), e12=data[i].item((1,2)), e13=data[i].item((1,3)),
+                    e20=data[i].item((2,0)), e21=data[i].item((2,1)), e22=data[i].item((2,2)), e23=data[i].item((2,3)),
+                    idx=j+1) 
+                for j in range(6)
+            ) + "},\n"
+
+    else:
+        raise RuntimeError("Generation mode: {} not supported".format(mode))
+
+    result += "};\n"
+    result += "const int {}_entries[] {{ {} }};\n".format(path, ",".join(str(e) for e in entries))
+    result += "const MovementTable {name}_table {{{name}_paths, {count}, {dur}, {name}_entries, {ecount} }};".format(name=path, count=count, dur=dur, ecount=len(entries))
+    return result
+
+def generate_c_def(path):
+    return """const MovementTable& {name}Table() {{
+    return {name}_table;
+}}""".format(name=path)
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description='pathTool: generate Hexapod path')
+    parser.add_argument('--pathDir', metavar='DIR',  dest='path_dir', default='path',
+                        help='path script directory (default: {})'.format('path'))
+    parser.add_argument('--outPath', metavar='PATH',  dest='out_path', default='output/movement_table.h',
+                        help='path script directory (default: {})'.format('output/movement_table.h'))
+    args = parser.parse_args()
+
+    sys.path.insert(0, args.path_dir)
+
+    # find available path generator
+    paths = collectPath(args.path_dir)
+
+    # generate all paths
+    results = {path: generator() for path, generator in paths.items()}
+
+    # output results
+    with open(args.out_path, "w") as f:
+        print("//", file=f)
+        print("// This file is generated, dont directly modify content...", file=f)
+        print("//", file=f)
+        print("namespace {", file=f)
+        for path, data in results.items():
+            print(generate_c_body(path, data), file=f)
+        print("}\n", file=f)
+        for path in results:
+            print(generate_c_def(path), file=f)
+
+    print("Result written to {}".format(args.out_path))
+
+
+

software/pathTool/src/output/movement_table.h

@@ -0,0 +1 @@
+../../../hexapod7697/src/hexapod/movement_table.h

software/pathTool/src/path/backward.py

@@ -0,0 +1,15 @@
+from collections import deque
+
+from lib import semicircle_generator
+from forward import g_steps, g_radius
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps, reverse=True)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)

software/pathTool/src/path/forward.py

@@ -0,0 +1,19 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
+    

software/pathTool/src/path/lib.py

@@ -0,0 +1,85 @@
+from collections import deque
+import math
+import numpy as np
+
+pi = math.acos(-1)
+
+def semicircle_generator(radius, steps, reverse=False):
+    assert (steps % 4) == 0
+    halfsteps = int(steps/2)
+
+    step_angle = pi / halfsteps
+
+    result = []
+
+    # first half, move backward (only y change)
+    for i in range(halfsteps):
+        result.append((0, radius - i*radius*2/(halfsteps), 0))
+
+    # second half, move forward in semicircle shape (y, z change)
+    for i in range(halfsteps):
+        angle = pi - step_angle*i
+        y = radius * math.cos(angle)
+        z = radius * math.sin(angle)
+        result.append((0, y, z))
+
+    result = deque(result)
+    result.rotate(int(steps/4))
+
+    if reverse:
+        result = deque(reversed(result))
+        result.rotate(1)
+
+    return result
+
+def get_rotate_x_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [1, 0, 0, 0],
+        [0, math.cos(angle), -math.sin(angle), 0],
+        [0, math.sin(angle), math.cos(angle), 0],
+        [0, 0, 0, 1],
+    ])
+
+def get_rotate_y_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [math.cos(angle), 0, math.sin(angle), 0],
+        [0, 1, 0, 0],
+        [-math.sin(angle), 0, math.cos(angle), 0],
+        [0, 0, 0, 1],
+    ])
+
+def get_rotate_z_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [math.cos(angle), -math.sin(angle), 0, 0],
+        [math.sin(angle), math.cos(angle), 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 1],
+    ])
+
+def point_rotate_x(pt, angle):
+    ptx = list(pt) + [1]
+    return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
+
+def point_rotate_y(pt, angle):
+    ptx = list(pt) + [1]
+    return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
+
+def point_rotate_z(pt, angle):
+    ptx = list(pt) + [1]
+    return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
+
+def path_rotate_x(path, angle):
+    return [point_rotate_x(p, angle) for p in path]
+
+def path_rotate_y(path, angle):
+    return [point_rotate_y(p, angle) for p in path]
+
+def path_rotate_z(path, angle):
+    return [point_rotate_z(p, angle) for p in path]
+
+if __name__ == '__main__':
+    pt = [0, 1, 0]
+    print(point_rotate_z(pt, 270))

software/pathTool/src/path/rotatex.py

@@ -0,0 +1,42 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_x_matrix as get_matrix
+
+g_steps = 20
+
+swing_angle = 15
+y_radius = 15
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    quarter = int(g_steps/4)
+
+    pi = math.acos(-1)
+
+    result = []
+    step_angle = swing_angle / quarter
+    step_offset = y_radius / quarter
+
+    for i in range(quarter):
+        m = get_matrix(swing_angle - i*step_angle)
+        m[1,3] = -i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(-i*step_angle)
+        m[1,3] = -y_radius + i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle-swing_angle)
+        m[1,3] = i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle)
+        m[1,3] = y_radius-i * step_offset
+        result.append(m)
+
+    return result, "matrix", 50, (0, quarter*2)

software/pathTool/src/path/rotatey.py

@@ -0,0 +1,42 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_y_matrix as get_matrix
+
+g_steps = 20
+
+swing_angle = 15
+x_radius = 15
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    quarter = int(g_steps/4)
+
+    pi = math.acos(-1)
+
+    result = []
+    step_angle = swing_angle / quarter
+    step_offset = x_radius / quarter
+
+    for i in range(quarter):
+        m = get_matrix(swing_angle - i*step_angle)
+        m[0,3] = -i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(-i*step_angle)
+        m[0,3] = -x_radius + i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle-swing_angle)
+        m[0,3] = i * step_offset
+        result.append(m)
+
+    for i in range(quarter):
+        m = get_matrix(i*step_angle)
+        m[0,3] = x_radius-i * step_offset
+        result.append(m)
+
+    return result, "matrix", 50, (0, quarter*2)

software/pathTool/src/path/rotatez.py

@@ -0,0 +1,25 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_x_matrix, get_rotate_y_matrix
+
+g_steps = 20
+
+z_lift = 4.5
+xy_radius = 1
+
+def path_generator():
+
+    pi = math.acos(-1)
+
+    result = []
+    step_angle = 2*pi / g_steps
+    for i in range(g_steps):
+        x = xy_radius * math.cos(i*step_angle)
+        y = xy_radius * math.sin(i*step_angle)
+
+        m = get_rotate_y_matrix(math.atan2(x, z_lift)*180/pi) * get_rotate_x_matrix(math.atan2(y, z_lift)*180/pi)
+        result.append(m)
+
+    return result, "matrix", 50, range(g_steps)

software/pathTool/src/path/shiftleft.py

@@ -0,0 +1,20 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    path = path_rotate_z(path, 90)  # shift 90 degree to make the path "left" shift
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)

software/pathTool/src/path/shiftright.py

@@ -0,0 +1,20 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    path = path_rotate_z(path, 270)  # shift 270 degree to make the path "right" shift
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)

software/pathTool/src/path/turnleft.py

@@ -0,0 +1,27 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [
+        path_rotate_z(path, 45), 
+        path_rotate_z(mir_path, 0), 
+        path_rotate_z(path, 315), 
+        path_rotate_z(mir_path, 225), 
+        path_rotate_z(path, 180), 
+        path_rotate_z(mir_path, 135), 
+    ], "shift", 20, (0, halfsteps)
+    

software/pathTool/src/path/turnright.py

@@ -0,0 +1,27 @@
+
+from collections import deque
+
+from lib import semicircle_generator
+from lib import path_rotate_z
+
+g_steps = 20
+g_radius = 20
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+
+    mir_path = deque(path)
+    mir_path.rotate(halfsteps)
+
+    return [
+        path_rotate_z(path, 45+180), 
+        path_rotate_z(mir_path, 0+180), 
+        path_rotate_z(path, 315+180), 
+        path_rotate_z(mir_path, 225+180), 
+        path_rotate_z(path, 180+180), 
+        path_rotate_z(mir_path, 135+180), 
+    ], "shift", 20, (0, halfsteps)
+