delete old files

software/archive/hexapod7697/.gitignore

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

software/archive/hexapod7697/hexapod7697.ino

@@ -1,84 +0,0 @@
-
-#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/archive/hexapod7697/src/hexapod/base.h

@@ -1,71 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/config.h

@@ -1,22 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/debug.cpp

@@ -1,36 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/debug.h

@@ -1,25 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/hal/pwm.cpp

@@ -1,29 +0,0 @@
-
-#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/archive/hexapod7697/src/hexapod/hal/pwm.h

@@ -1,18 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/hexapod.cpp

@@ -1,99 +0,0 @@
-
-#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/archive/hexapod7697/src/hexapod/hexapod.h

@@ -1,37 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/leg.cpp

@@ -1,201 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/leg.h

@@ -1,52 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/movement.cpp

@@ -1,79 +0,0 @@
-#include "movement.h"
-#include "debug.h"
-#include "config.h"
-
-#include <cstdlib>
-
-namespace hexapod {
-
-    extern const MovementTable& standbyTable();
-    extern const MovementTable& forwardTable();
-    extern const MovementTable& forwardfastTable();
-    extern const MovementTable& backwardTable();
-    extern const MovementTable& turnleftTable();
-    extern const MovementTable& turnrightTable();
-    extern const MovementTable& shiftleftTable();
-    extern const MovementTable& shiftrightTable();
-    extern const MovementTable& climbTable();
-    extern const MovementTable& rotatexTable();
-    extern const MovementTable& rotateyTable();
-    extern const MovementTable& rotatezTable();
-    extern const MovementTable& twistTable();
-
-    const MovementTable kTable[MOVEMENT_TOTAL] {
-        standbyTable(),
-        forwardTable(),
-        forwardfastTable(),
-        backwardTable(),
-        turnleftTable(),
-        turnrightTable(),
-        shiftleftTable(),
-        shiftrightTable(),
-        climbTable(),
-        rotatexTable(),
-        rotateyTable(),
-        rotatezTable(),
-        twistTable(),
-    };
-
-    Movement::Movement(MovementMode mode):
-        mode_{mode}, transiting_{false}
-    {
-    }
-
-    void Movement::setMode(MovementMode newMode) {
-
-        if (!kTable[newMode].entries) {
-            LOG_INFO("Error: null movement of mode(%d)!", newMode);
-            return;
-        }
-
-        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/archive/hexapod7697/src/hexapod/movement.h

@@ -1,56 +0,0 @@
-#pragma once
-
-#include "base.h"
-
-namespace hexapod {
-
-    enum MovementMode {
-        MOVEMENT_STANDBY = 0,
-
-        MOVEMENT_FORWARD,
-        MOVEMENT_FORWARDFAST,
-        MOVEMENT_BACKWARD,
-        MOVEMENT_TURNLEFT,
-        MOVEMENT_TURNRIGHT,
-        MOVEMENT_SHIFTLEFT,
-        MOVEMENT_SHIFTRIGHT,
-        MOVEMENT_CLIMB,
-        MOVEMENT_ROTATEX,
-        MOVEMENT_ROTATEY,
-        MOVEMENT_ROTATEZ,
-        MOVEMENT_TWIST,
-
-        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/archive/hexapod7697/src/hexapod/movement_table.h

@@ -1,742 +0,0 @@
-//
-// This file is generated, dont directly modify content...
-//
-namespace {
-
-const Locations backward_paths[] {
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(5.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(15.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(25.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(23.78), P6Z+(7.73)}},
-    {{P1X+(0.00), P1Y+(-15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(-15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.23), P6Z+(14.69)}},
-    {{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(14.69), P6Z+(20.23)}},
-    {{P1X+(0.00), P1Y+(-5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(-5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(7.73), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}},
-    {{P1X+(0.00), P1Y+(5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-7.73), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-14.69), P6Z+(20.23)}},
-    {{P1X+(0.00), P1Y+(15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.23), P6Z+(14.69)}},
-    {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-23.78), P6Z+(7.73)}},
-    {{P1X+(0.00), P1Y+(25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-25.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(-15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(-15.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(-5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(-5.00), P6Z+(0.00)}},
-};
-const int backward_entries[] { 0,10 };
-const MovementTable backward_table {backward_paths, 20, 20, backward_entries, 2 };
-
-const Locations climb_paths[] {
-    {{P1X+(30.00), P1Y+(0.00), P1Z+(50.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(-30.00)}, {P3X+(30.00), P3Y+(0.00), P3Z+(50.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(-30.00)}, {P5X+(-30.00), P5Y+(0.00), P5Z+(50.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(-30.00)}},
-    {{P1X+(28.53), P1Y+(6.18), P1Z+(46.08)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(-30.00)}, {P3X+(28.53), P3Y+(6.18), P3Z+(46.08)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(-30.00)}, {P5X+(-28.53), P5Y+(6.18), P5Z+(46.08)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(-30.00)}},
-    {{P1X+(24.27), P1Y+(11.76), P1Z+(34.72)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(-30.00)}, {P3X+(24.27), P3Y+(11.76), P3Z+(34.72)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(-30.00)}, {P5X+(-24.27), P5Y+(11.76), P5Z+(34.72)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(-30.00)}},
-    {{P1X+(17.63), P1Y+(16.18), P1Z+(17.02)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(-30.00)}, {P3X+(17.63), P3Y+(16.18), P3Z+(17.02)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(-30.00)}, {P5X+(-17.63), P5Y+(16.18), P5Z+(17.02)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(-30.00)}},
-    {{P1X+(9.27), P1Y+(19.02), P1Z+(-5.28)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(-30.00)}, {P3X+(9.27), P3Y+(19.02), P3Z+(-5.28)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(-30.00)}, {P5X+(-9.27), P5Y+(19.02), P5Z+(-5.28)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(-30.00)}},
-    {{P1X+(0.00), P1Y+(20.00), P1Z+(-30.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(-30.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(-30.00)}, {P4X+(-0.00), P4Y+(-20.00), P4Z+(-30.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(-30.00)}, {P6X+(-0.00), P6Y+(-20.00), P6Z+(-30.00)}},
-    {{P1X+(0.00), P1Y+(16.00), P1Z+(-30.00)}, {P2X+(9.27), P2Y+(-19.02), P2Z+(-5.28)}, {P3X+(0.00), P3Y+(16.00), P3Z+(-30.00)}, {P4X+(-9.27), P4Y+(-19.02), P4Z+(-5.28)}, {P5X+(0.00), P5Y+(16.00), P5Z+(-30.00)}, {P6X+(-9.27), P6Y+(-19.02), P6Z+(-5.28)}},
-    {{P1X+(0.00), P1Y+(12.00), P1Z+(-30.00)}, {P2X+(17.63), P2Y+(-16.18), P2Z+(17.02)}, {P3X+(0.00), P3Y+(12.00), P3Z+(-30.00)}, {P4X+(-17.63), P4Y+(-16.18), P4Z+(17.02)}, {P5X+(0.00), P5Y+(12.00), P5Z+(-30.00)}, {P6X+(-17.63), P6Y+(-16.18), P6Z+(17.02)}},
-    {{P1X+(0.00), P1Y+(8.00), P1Z+(-30.00)}, {P2X+(24.27), P2Y+(-11.76), P2Z+(34.72)}, {P3X+(0.00), P3Y+(8.00), P3Z+(-30.00)}, {P4X+(-24.27), P4Y+(-11.76), P4Z+(34.72)}, {P5X+(0.00), P5Y+(8.00), P5Z+(-30.00)}, {P6X+(-24.27), P6Y+(-11.76), P6Z+(34.72)}},
-    {{P1X+(0.00), P1Y+(4.00), P1Z+(-30.00)}, {P2X+(28.53), P2Y+(-6.18), P2Z+(46.08)}, {P3X+(0.00), P3Y+(4.00), P3Z+(-30.00)}, {P4X+(-28.53), P4Y+(-6.18), P4Z+(46.08)}, {P5X+(0.00), P5Y+(4.00), P5Z+(-30.00)}, {P6X+(-28.53), P6Y+(-6.18), P6Z+(46.08)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(-30.00)}, {P2X+(30.00), P2Y+(0.00), P2Z+(50.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(-30.00)}, {P4X+(-30.00), P4Y+(0.00), P4Z+(50.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(-30.00)}, {P6X+(-30.00), P6Y+(0.00), P6Z+(50.00)}},
-    {{P1X+(0.00), P1Y+(-4.00), P1Z+(-30.00)}, {P2X+(28.53), P2Y+(6.18), P2Z+(46.08)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(-30.00)}, {P4X+(-28.53), P4Y+(6.18), P4Z+(46.08)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(-30.00)}, {P6X+(-28.53), P6Y+(6.18), P6Z+(46.08)}},
-    {{P1X+(0.00), P1Y+(-8.00), P1Z+(-30.00)}, {P2X+(24.27), P2Y+(11.76), P2Z+(34.72)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(-30.00)}, {P4X+(-24.27), P4Y+(11.76), P4Z+(34.72)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(-30.00)}, {P6X+(-24.27), P6Y+(11.76), P6Z+(34.72)}},
-    {{P1X+(0.00), P1Y+(-12.00), P1Z+(-30.00)}, {P2X+(17.63), P2Y+(16.18), P2Z+(17.02)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(-30.00)}, {P4X+(-17.63), P4Y+(16.18), P4Z+(17.02)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(-30.00)}, {P6X+(-17.63), P6Y+(16.18), P6Z+(17.02)}},
-    {{P1X+(0.00), P1Y+(-16.00), P1Z+(-30.00)}, {P2X+(9.27), P2Y+(19.02), P2Z+(-5.28)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(-30.00)}, {P4X+(-9.27), P4Y+(19.02), P4Z+(-5.28)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(-30.00)}, {P6X+(-9.27), P6Y+(19.02), P6Z+(-5.28)}},
-    {{P1X+(0.00), P1Y+(-20.00), P1Z+(-30.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(-30.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(-30.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(-30.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(-30.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(-30.00)}},
-    {{P1X+(9.27), P1Y+(-19.02), P1Z+(-5.28)}, {P2X+(0.00), P2Y+(16.00), P2Z+(-30.00)}, {P3X+(9.27), P3Y+(-19.02), P3Z+(-5.28)}, {P4X+(0.00), P4Y+(16.00), P4Z+(-30.00)}, {P5X+(-9.27), P5Y+(-19.02), P5Z+(-5.28)}, {P6X+(0.00), P6Y+(16.00), P6Z+(-30.00)}},
-    {{P1X+(17.63), P1Y+(-16.18), P1Z+(17.02)}, {P2X+(0.00), P2Y+(12.00), P2Z+(-30.00)}, {P3X+(17.63), P3Y+(-16.18), P3Z+(17.02)}, {P4X+(0.00), P4Y+(12.00), P4Z+(-30.00)}, {P5X+(-17.63), P5Y+(-16.18), P5Z+(17.02)}, {P6X+(0.00), P6Y+(12.00), P6Z+(-30.00)}},
-    {{P1X+(24.27), P1Y+(-11.76), P1Z+(34.72)}, {P2X+(0.00), P2Y+(8.00), P2Z+(-30.00)}, {P3X+(24.27), P3Y+(-11.76), P3Z+(34.72)}, {P4X+(0.00), P4Y+(8.00), P4Z+(-30.00)}, {P5X+(-24.27), P5Y+(-11.76), P5Z+(34.72)}, {P6X+(0.00), P6Y+(8.00), P6Z+(-30.00)}},
-    {{P1X+(28.53), P1Y+(-6.18), P1Z+(46.08)}, {P2X+(0.00), P2Y+(4.00), P2Z+(-30.00)}, {P3X+(28.53), P3Y+(-6.18), P3Z+(46.08)}, {P4X+(0.00), P4Y+(4.00), P4Z+(-30.00)}, {P5X+(-28.53), P5Y+(-6.18), P5Z+(46.08)}, {P6X+(0.00), P6Y+(4.00), P6Z+(-30.00)}},
-};
-const int climb_entries[] { 0,10 };
-const MovementTable climb_table {climb_paths, 20, 30, climb_entries, 2 };
-
-const Locations forward_paths[] {
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(-5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(-5.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(-15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(-15.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-25.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-23.78), P6Z+(7.73)}},
-    {{P1X+(0.00), P1Y+(15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.23), P6Z+(14.69)}},
-    {{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-14.69), P6Z+(20.23)}},
-    {{P1X+(0.00), P1Y+(5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-7.73), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}},
-    {{P1X+(0.00), P1Y+(-5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(-5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(7.73), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(14.69), P6Z+(20.23)}},
-    {{P1X+(0.00), P1Y+(-15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(-15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.23), P6Z+(14.69)}},
-    {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(23.78), P6Z+(7.73)}},
-    {{P1X+(0.00), P1Y+(-25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(25.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(15.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(-7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(5.00), P6Z+(0.00)}},
-};
-const int forward_entries[] { 0,10 };
-const MovementTable forward_table {forward_paths, 20, 20, forward_entries, 2 };
-
-const Locations forwardfast_paths[] {
-    {{P1X+(10.00), P1Y+(0.00), P1Z+(30.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(10.00), P3Y+(0.00), P3Z+(30.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(30.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(9.51), P1Y+(15.45), P1Z+(28.53)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(9.51), P3Y+(15.45), P3Z+(28.53)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(-9.51), P5Y+(15.45), P5Z+(28.53)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}},
-    {{P1X+(8.09), P1Y+(29.39), P1Z+(24.27)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(8.09), P3Y+(29.39), P3Z+(24.27)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(-8.09), P5Y+(29.39), P5Z+(24.27)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
-    {{P1X+(5.88), P1Y+(40.45), P1Z+(17.63)}, {P2X+(0.00), P2Y+(-30.00), P2Z+(0.00)}, {P3X+(5.88), P3Y+(40.45), P3Z+(17.63)}, {P4X+(0.00), P4Y+(-30.00), P4Z+(0.00)}, {P5X+(-5.88), P5Y+(40.45), P5Z+(17.63)}, {P6X+(0.00), P6Y+(-30.00), P6Z+(0.00)}},
-    {{P1X+(3.09), P1Y+(47.55), P1Z+(9.27)}, {P2X+(0.00), P2Y+(-40.00), P2Z+(0.00)}, {P3X+(3.09), P3Y+(47.55), P3Z+(9.27)}, {P4X+(0.00), P4Y+(-40.00), P4Z+(0.00)}, {P5X+(-3.09), P5Y+(47.55), P5Z+(9.27)}, {P6X+(0.00), P6Y+(-40.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(50.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-50.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(50.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(-50.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(50.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-50.00), P6Z+(0.00)}},
-    {{P1X+(0.00), P1Y+(40.00), P1Z+(0.00)}, {P2X+(3.09), P2Y+(-47.55), P2Z+(9.27)}, {P3X+(0.00), P3Y+(40.00), P3Z+(0.00)}, {P4X+(-3.09), P4Y+(-47.55), P4Z+(9.27)}, {P5X+(0.00), P5Y+(40.00), P5Z+(0.00)}, {P6X+(-3.09), P6Y+(-47.55), P6Z+(9.27)}},
-    {{P1X+(0.00), P1Y+(30.00), P1Z+(0.00)}, {P2X+(5.88), P2Y+(-40.45), P2Z+(17.63)}, {P3X+(0.00), P3Y+(30.00), P3Z+(0.00)}, {P4X+(-5.88), P4Y+(-40.45), P4Z+(17.63)}, {P5X+(0.00), P5Y+(30.00), P5Z+(0.00)}, {P6X+(-5.88), P6Y+(-40.45), P6Z+(17.63)}},
-    {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(8.09), P2Y+(-29.39), P2Z+(24.27)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(-8.09), P4Y+(-29.39), P4Z+(24.27)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-8.09), P6Y+(-29.39), P6Z+(24.27)}},
-    {{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(9.51), P2Y+(-15.45), P2Z+(28.53)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(-9.51), P4Y+(-15.45), P4Z+(28.53)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-9.51), P6Y+(-15.45), P6Z+(28.53)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(10.00), P2Y+(0.00), P2Z+(30.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(30.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(30.00)}},
-    {{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(9.51), P2Y+(15.45), P2Z+(28.53)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(-9.51), P4Y+(15.45), P4Z+(28.53)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(-9.51), P6Y+(15.45), P6Z+(28.53)}},
-    {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(8.09), P2Y+(29.39), P2Z+(24.27)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(-8.09), P4Y+(29.39), P4Z+(24.27)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(-8.09), P6Y+(29.39), P6Z+(24.27)}},
-    {{P1X+(0.00), P1Y+(-30.00), P1Z+(0.00)}, {P2X+(5.88), P2Y+(40.45), P2Z+(17.63)}, {P3X+(0.00), P3Y+(-30.00), P3Z+(0.00)}, {P4X+(-5.88), P4Y+(40.45), P4Z+(17.63)}, {P5X+(0.00), P5Y+(-30.00), P5Z+(0.00)}, {P6X+(-5.88), P6Y+(40.45), P6Z+(17.63)}},
-    {{P1X+(0.00), P1Y+(-40.00), P1Z+(0.00)}, {P2X+(3.09), P2Y+(47.55), P2Z+(9.27)}, {P3X+(0.00), P3Y+(-40.00), P3Z+(0.00)}, {P4X+(-3.09), P4Y+(47.55), P4Z+(9.27)}, {P5X+(0.00), P5Y+(-40.00), P5Z+(0.00)}, {P6X+(-3.09), P6Y+(47.55), P6Z+(9.27)}},
-    {{P1X+(0.00), P1Y+(-50.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(50.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-50.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(50.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-50.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(50.00), P6Z+(0.00)}},
-    {{P1X+(3.09), P1Y+(-47.55), P1Z+(9.27)}, {P2X+(0.00), P2Y+(40.00), P2Z+(0.00)}, {P3X+(3.09), P3Y+(-47.55), P3Z+(9.27)}, {P4X+(0.00), P4Y+(40.00), P4Z+(0.00)}, {P5X+(-3.09), P5Y+(-47.55), P5Z+(9.27)}, {P6X+(0.00), P6Y+(40.00), P6Z+(0.00)}},
-    {{P1X+(5.88), P1Y+(-40.45), P1Z+(17.63)}, {P2X+(0.00), P2Y+(30.00), P2Z+(0.00)}, {P3X+(5.88), P3Y+(-40.45), P3Z+(17.63)}, {P4X+(0.00), P4Y+(30.00), P4Z+(0.00)}, {P5X+(-5.88), P5Y+(-40.45), P5Z+(17.63)}, {P6X+(0.00), P6Y+(30.00), P6Z+(0.00)}},
-    {{P1X+(8.09), P1Y+(-29.39), P1Z+(24.27)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(8.09), P3Y+(-29.39), P3Z+(24.27)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(-8.09), P5Y+(-29.39), P5Z+(24.27)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
-    {{P1X+(9.51), P1Y+(-15.45), P1Z+(28.53)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(9.51), P3Y+(-15.45), P3Z+(28.53)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(-9.51), P5Y+(-15.45), P5Z+(28.53)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}},
-};
-const int forwardfast_entries[] { 0,10 };
-const MovementTable forwardfast_table {forwardfast_paths, 20, 20, forwardfast_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+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(-7.73), P1Y+(0.00), P1Z+(23.78)}, {P2X+(5.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-7.73), P3Y+(0.00), P3Z+(23.78)}, {P4X+(5.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-7.73), P5Y+(0.00), P5Z+(23.78)}, {P6X+(5.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-14.69), P1Y+(0.00), P1Z+(20.23)}, {P2X+(10.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-14.69), P3Y+(0.00), P3Z+(20.23)}, {P4X+(10.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-14.69), P5Y+(0.00), P5Z+(20.23)}, {P6X+(10.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-20.23), P1Y+(0.00), P1Z+(14.69)}, {P2X+(15.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.23), P3Y+(0.00), P3Z+(14.69)}, {P4X+(15.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.23), P5Y+(0.00), P5Z+(14.69)}, {P6X+(15.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-23.78), P1Y+(0.00), P1Z+(7.73)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-23.78), P3Y+(0.00), P3Z+(7.73)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-23.78), P5Y+(0.00), P5Z+(7.73)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-25.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(25.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-25.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(25.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-25.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(25.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(23.78), P2Y+(-0.00), P2Z+(7.73)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(23.78), P4Y+(-0.00), P4Z+(7.73)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(23.78), P6Y+(-0.00), P6Z+(7.73)}},
-    {{P1X+(-15.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.23), P2Y+(-0.00), P2Z+(14.69)}, {P3X+(-15.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.23), P4Y+(-0.00), P4Z+(14.69)}, {P5X+(-15.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.23), P6Y+(-0.00), P6Z+(14.69)}},
-    {{P1X+(-10.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(14.69), P2Y+(-0.00), P2Z+(20.23)}, {P3X+(-10.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(14.69), P4Y+(-0.00), P4Z+(20.23)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(14.69), P6Y+(-0.00), P6Z+(20.23)}},
-    {{P1X+(-5.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(7.73), P2Y+(-0.00), P2Z+(23.78)}, {P3X+(-5.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(7.73), P4Y+(-0.00), P4Z+(23.78)}, {P5X+(-5.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(7.73), P6Y+(-0.00), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(0.00), P6Z+(25.00)}},
-    {{P1X+(5.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-7.73), P2Y+(0.00), P2Z+(23.78)}, {P3X+(5.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-7.73), P4Y+(0.00), P4Z+(23.78)}, {P5X+(5.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-7.73), P6Y+(0.00), P6Z+(23.78)}},
-    {{P1X+(10.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-14.69), P2Y+(0.00), P2Z+(20.23)}, {P3X+(10.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-14.69), P4Y+(0.00), P4Z+(20.23)}, {P5X+(10.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-14.69), P6Y+(0.00), P6Z+(20.23)}},
-    {{P1X+(15.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.23), P2Y+(0.00), P2Z+(14.69)}, {P3X+(15.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.23), P4Y+(0.00), P4Z+(14.69)}, {P5X+(15.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.23), P6Y+(0.00), P6Z+(14.69)}},
-    {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-23.78), P2Y+(0.00), P2Z+(7.73)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-23.78), P4Y+(0.00), P4Z+(7.73)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-23.78), P6Y+(0.00), P6Z+(7.73)}},
-    {{P1X+(25.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-25.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(25.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-25.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(25.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-25.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(23.78), P1Y+(-0.00), P1Z+(7.73)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(23.78), P3Y+(-0.00), P3Z+(7.73)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(23.78), P5Y+(-0.00), P5Z+(7.73)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(20.23), P1Y+(-0.00), P1Z+(14.69)}, {P2X+(-15.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.23), P3Y+(-0.00), P3Z+(14.69)}, {P4X+(-15.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.23), P5Y+(-0.00), P5Z+(14.69)}, {P6X+(-15.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(14.69), P1Y+(-0.00), P1Z+(20.23)}, {P2X+(-10.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(14.69), P3Y+(-0.00), P3Z+(20.23)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(14.69), P5Y+(-0.00), P5Z+(20.23)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(7.73), P1Y+(-0.00), P1Z+(23.78)}, {P2X+(-5.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(7.73), P3Y+(-0.00), P3Z+(23.78)}, {P4X+(-5.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(7.73), P5Y+(-0.00), P5Z+(23.78)}, {P6X+(-5.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+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(7.73), P1Y+(-0.00), P1Z+(23.78)}, {P2X+(-5.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(7.73), P3Y+(-0.00), P3Z+(23.78)}, {P4X+(-5.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(7.73), P5Y+(-0.00), P5Z+(23.78)}, {P6X+(-5.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(14.69), P1Y+(-0.00), P1Z+(20.23)}, {P2X+(-10.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(14.69), P3Y+(-0.00), P3Z+(20.23)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(14.69), P5Y+(-0.00), P5Z+(20.23)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(20.23), P1Y+(-0.00), P1Z+(14.69)}, {P2X+(-15.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.23), P3Y+(-0.00), P3Z+(14.69)}, {P4X+(-15.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.23), P5Y+(-0.00), P5Z+(14.69)}, {P6X+(-15.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(23.78), P1Y+(-0.00), P1Z+(7.73)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(23.78), P3Y+(-0.00), P3Z+(7.73)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(23.78), P5Y+(-0.00), P5Z+(7.73)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(25.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-25.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(25.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-25.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(25.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-25.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-23.78), P2Y+(0.00), P2Z+(7.73)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-23.78), P4Y+(0.00), P4Z+(7.73)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-23.78), P6Y+(0.00), P6Z+(7.73)}},
-    {{P1X+(15.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.23), P2Y+(0.00), P2Z+(14.69)}, {P3X+(15.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.23), P4Y+(0.00), P4Z+(14.69)}, {P5X+(15.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.23), P6Y+(0.00), P6Z+(14.69)}},
-    {{P1X+(10.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-14.69), P2Y+(0.00), P2Z+(20.23)}, {P3X+(10.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-14.69), P4Y+(0.00), P4Z+(20.23)}, {P5X+(10.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-14.69), P6Y+(0.00), P6Z+(20.23)}},
-    {{P1X+(5.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-7.73), P2Y+(0.00), P2Z+(23.78)}, {P3X+(5.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-7.73), P4Y+(0.00), P4Z+(23.78)}, {P5X+(5.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-7.73), P6Y+(0.00), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-0.00), P6Z+(25.00)}},
-    {{P1X+(-5.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(7.73), P2Y+(-0.00), P2Z+(23.78)}, {P3X+(-5.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(7.73), P4Y+(-0.00), P4Z+(23.78)}, {P5X+(-5.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(7.73), P6Y+(-0.00), P6Z+(23.78)}},
-    {{P1X+(-10.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(14.69), P2Y+(-0.00), P2Z+(20.23)}, {P3X+(-10.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(14.69), P4Y+(-0.00), P4Z+(20.23)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(14.69), P6Y+(-0.00), P6Z+(20.23)}},
-    {{P1X+(-15.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.23), P2Y+(-0.00), P2Z+(14.69)}, {P3X+(-15.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.23), P4Y+(-0.00), P4Z+(14.69)}, {P5X+(-15.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.23), P6Y+(-0.00), P6Z+(14.69)}},
-    {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(23.78), P2Y+(-0.00), P2Z+(7.73)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(23.78), P4Y+(-0.00), P4Z+(7.73)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(23.78), P6Y+(-0.00), P6Z+(7.73)}},
-    {{P1X+(-25.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(25.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-25.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(25.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-25.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(25.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-23.78), P1Y+(0.00), P1Z+(7.73)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-23.78), P3Y+(0.00), P3Z+(7.73)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-23.78), P5Y+(0.00), P5Z+(7.73)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-20.23), P1Y+(0.00), P1Z+(14.69)}, {P2X+(15.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.23), P3Y+(0.00), P3Z+(14.69)}, {P4X+(15.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.23), P5Y+(0.00), P5Z+(14.69)}, {P6X+(15.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-14.69), P1Y+(0.00), P1Z+(20.23)}, {P2X+(10.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-14.69), P3Y+(0.00), P3Z+(20.23)}, {P4X+(10.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-14.69), P5Y+(0.00), P5Z+(20.23)}, {P6X+(10.00), P6Y+(-0.00), P6Z+(0.00)}},
-    {{P1X+(-7.73), P1Y+(0.00), P1Z+(23.78)}, {P2X+(5.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-7.73), P3Y+(0.00), P3Z+(23.78)}, {P4X+(5.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-7.73), P5Y+(0.00), P5Z+(23.78)}, {P6X+(5.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+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(-5.46), P1Y+(5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(5.46), P3Y+(5.46), P3Z+(23.78)}, {P4X+(-3.54), P4Y+(3.54), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(3.54), P6Y+(3.54), P6Z+(0.00)}},
-    {{P1X+(-10.39), P1Y+(10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(10.39), P3Y+(10.39), P3Z+(20.23)}, {P4X+(-7.07), P4Y+(7.07), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(7.07), P6Y+(7.07), P6Z+(0.00)}},
-    {{P1X+(-14.30), P1Y+(14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(14.30), P3Y+(14.30), P3Z+(14.69)}, {P4X+(-10.61), P4Y+(10.61), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(10.61), P6Y+(10.61), P6Z+(0.00)}},
-    {{P1X+(-16.81), P1Y+(16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(16.81), P3Y+(16.81), P3Z+(7.73)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
-    {{P1X+(-17.68), P1Y+(17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(17.68), P3Y+(17.68), P3Z+(0.00)}, {P4X+(-17.68), P4Y+(17.68), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(17.68), P6Y+(17.68), P6Z+(0.00)}},
-    {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-16.81), P4Y+(16.81), P4Z+(7.73)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(16.81), P6Y+(16.81), P6Z+(7.73)}},
-    {{P1X+(-10.61), P1Y+(10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(10.61), P3Y+(10.61), P3Z+(0.00)}, {P4X+(-14.30), P4Y+(14.30), P4Z+(14.69)}, {P5X+(-0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(14.30), P6Y+(14.30), P6Z+(14.69)}},
-    {{P1X+(-7.07), P1Y+(7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(7.07), P3Y+(7.07), P3Z+(0.00)}, {P4X+(-10.39), P4Y+(10.39), P4Z+(20.23)}, {P5X+(-0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(10.39), P6Y+(10.39), P6Z+(20.23)}},
-    {{P1X+(-3.54), P1Y+(3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(3.54), P3Y+(3.54), P3Z+(0.00)}, {P4X+(-5.46), P4Y+(5.46), P4Z+(23.78)}, {P5X+(-0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(5.46), P6Y+(5.46), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-0.00), P6Z+(25.00)}},
-    {{P1X+(3.54), P1Y+(-3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(-3.54), P3Y+(-3.54), P3Z+(0.00)}, {P4X+(5.46), P4Y+(-5.46), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(-5.46), P6Y+(-5.46), P6Z+(23.78)}},
-    {{P1X+(7.07), P1Y+(-7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(-7.07), P3Y+(-7.07), P3Z+(0.00)}, {P4X+(10.39), P4Y+(-10.39), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-10.39), P6Y+(-10.39), P6Z+(20.23)}},
-    {{P1X+(10.61), P1Y+(-10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(-10.61), P3Y+(-10.61), P3Z+(0.00)}, {P4X+(14.30), P4Y+(-14.30), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(-14.30), P6Y+(-14.30), P6Z+(14.69)}},
-    {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(16.81), P4Y+(-16.81), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-16.81), P6Y+(-16.81), P6Z+(7.73)}},
-    {{P1X+(17.68), P1Y+(-17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(-17.68), P3Y+(-17.68), P3Z+(0.00)}, {P4X+(17.68), P4Y+(-17.68), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(-17.68), P6Y+(-17.68), P6Z+(0.00)}},
-    {{P1X+(16.81), P1Y+(-16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-16.81), P3Y+(-16.81), P3Z+(7.73)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
-    {{P1X+(14.30), P1Y+(-14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(-14.30), P3Y+(-14.30), P3Z+(14.69)}, {P4X+(10.61), P4Y+(-10.61), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(-10.61), P6Y+(-10.61), P6Z+(0.00)}},
-    {{P1X+(10.39), P1Y+(-10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(-10.39), P3Y+(-10.39), P3Z+(20.23)}, {P4X+(7.07), P4Y+(-7.07), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(-7.07), P6Y+(-7.07), P6Z+(0.00)}},
-    {{P1X+(5.46), P1Y+(-5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(-5.46), P3Y+(-5.46), P3Z+(23.78)}, {P4X+(3.54), P4Y+(-3.54), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(-3.54), P6Y+(-3.54), 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+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(-0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
-    {{P1X+(5.46), P1Y+(-5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(-5.46), P3Y+(-5.46), P3Z+(23.78)}, {P4X+(3.54), P4Y+(-3.54), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(-3.54), P6Y+(-3.54), P6Z+(0.00)}},
-    {{P1X+(10.39), P1Y+(-10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(-10.39), P3Y+(-10.39), P3Z+(20.23)}, {P4X+(7.07), P4Y+(-7.07), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(-7.07), P6Y+(-7.07), P6Z+(0.00)}},
-    {{P1X+(14.30), P1Y+(-14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(-14.30), P3Y+(-14.30), P3Z+(14.69)}, {P4X+(10.61), P4Y+(-10.61), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(-10.61), P6Y+(-10.61), P6Z+(0.00)}},
-    {{P1X+(16.81), P1Y+(-16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-16.81), P3Y+(-16.81), P3Z+(7.73)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
-    {{P1X+(17.68), P1Y+(-17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(-17.68), P3Y+(-17.68), P3Z+(0.00)}, {P4X+(17.68), P4Y+(-17.68), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(-17.68), P6Y+(-17.68), P6Z+(0.00)}},
-    {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(16.81), P4Y+(-16.81), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-16.81), P6Y+(-16.81), P6Z+(7.73)}},
-    {{P1X+(10.61), P1Y+(-10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(-10.61), P3Y+(-10.61), P3Z+(0.00)}, {P4X+(14.30), P4Y+(-14.30), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(-14.30), P6Y+(-14.30), P6Z+(14.69)}},
-    {{P1X+(7.07), P1Y+(-7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(-7.07), P3Y+(-7.07), P3Z+(0.00)}, {P4X+(10.39), P4Y+(-10.39), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-10.39), P6Y+(-10.39), P6Z+(20.23)}},
-    {{P1X+(3.54), P1Y+(-3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(-3.54), P3Y+(-3.54), P3Z+(0.00)}, {P4X+(5.46), P4Y+(-5.46), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(-5.46), P6Y+(-5.46), P6Z+(23.78)}},
-    {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}},
-    {{P1X+(-3.54), P1Y+(3.54), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(3.54), P3Y+(3.54), P3Z+(0.00)}, {P4X+(-5.46), P4Y+(5.46), P4Z+(23.78)}, {P5X+(-0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(5.46), P6Y+(5.46), P6Z+(23.78)}},
-    {{P1X+(-7.07), P1Y+(7.07), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(7.07), P3Y+(7.07), P3Z+(0.00)}, {P4X+(-10.39), P4Y+(10.39), P4Z+(20.23)}, {P5X+(-0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(10.39), P6Y+(10.39), P6Z+(20.23)}},
-    {{P1X+(-10.61), P1Y+(10.61), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(10.61), P3Y+(10.61), P3Z+(0.00)}, {P4X+(-14.30), P4Y+(14.30), P4Z+(14.69)}, {P5X+(-0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(14.30), P6Y+(14.30), P6Z+(14.69)}},
-    {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-16.81), P4Y+(16.81), P4Z+(7.73)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(16.81), P6Y+(16.81), P6Z+(7.73)}},
-    {{P1X+(-17.68), P1Y+(17.68), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(17.68), P3Y+(17.68), P3Z+(0.00)}, {P4X+(-17.68), P4Y+(17.68), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(17.68), P6Y+(17.68), P6Z+(0.00)}},
-    {{P1X+(-16.81), P1Y+(16.81), P1Z+(7.73)}, {P2X+(-0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(16.81), P3Y+(16.81), P3Z+(7.73)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
-    {{P1X+(-14.30), P1Y+(14.30), P1Z+(14.69)}, {P2X+(-0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(14.30), P3Y+(14.30), P3Z+(14.69)}, {P4X+(-10.61), P4Y+(10.61), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(10.61), P6Y+(10.61), P6Z+(0.00)}},
-    {{P1X+(-10.39), P1Y+(10.39), P1Z+(20.23)}, {P2X+(-0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(10.39), P3Y+(10.39), P3Z+(20.23)}, {P4X+(-7.07), P4Y+(7.07), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(7.07), P6Y+(7.07), P6Z+(0.00)}},
-    {{P1X+(-5.46), P1Y+(5.46), P1Z+(23.78)}, {P2X+(-0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(5.46), P3Y+(5.46), P3Z+(23.78)}, {P4X+(-3.54), P4Y+(3.54), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(3.54), P6Y+(3.54), P6Z+(0.00)}},
-};
-const int turnright_entries[] { 0,10 };
-const MovementTable turnright_table {turnright_paths, 20, 20, turnright_entries, 2 };
-
-const Locations twist_paths[] {
-    {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 0.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 + 0.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 + 0.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 + 0.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 + 0.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 + 0.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
-    {{P1X*1.00 + P1Y*-0.07 + P1Z*0.00 + 0.00, P1X*0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, 
-     {P2X*1.00 + P2Y*-0.07 + P2Z*0.00 + 0.00, P2X*0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, 
-     {P3X*1.00 + P3Y*-0.07 + P3Z*0.00 + 0.00, P3X*0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, 
-     {P4X*1.00 + P4Y*-0.07 + P4Z*0.00 + 0.00, P4X*0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, 
-     {P5X*1.00 + P5Y*-0.07 + P5Z*0.00 + 0.00, P5X*0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, 
-     {P6X*1.00 + P6Y*-0.07 + P6Z*0.00 + 0.00, P6X*0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
-    {{P1X*0.99 + P1Y*-0.14 + P1Z*0.01 + 0.00, P1X*0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, 
-     {P2X*0.99 + P2Y*-0.14 + P2Z*0.01 + 0.00, P2X*0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, 
-     {P3X*0.99 + P3Y*-0.14 + P3Z*0.01 + 0.00, P3X*0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, 
-     {P4X*0.99 + P4Y*-0.14 + P4Z*0.01 + 0.00, P4X*0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, 
-     {P5X*0.99 + P5Y*-0.14 + P5Z*0.01 + 0.00, P5X*0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, 
-     {P6X*0.99 + P6Y*-0.14 + P6Z*0.01 + 0.00, P6X*0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
-    {{P1X*0.98 + P1Y*-0.21 + P1Z*0.03 + 0.00, P1X*0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, 
-     {P2X*0.98 + P2Y*-0.21 + P2Z*0.03 + 0.00, P2X*0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, 
-     {P3X*0.98 + P3Y*-0.21 + P3Z*0.03 + 0.00, P3X*0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, 
-     {P4X*0.98 + P4Y*-0.21 + P4Z*0.03 + 0.00, P4X*0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, 
-     {P5X*0.98 + P5Y*-0.21 + P5Z*0.03 + 0.00, P5X*0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, 
-     {P6X*0.98 + P6Y*-0.21 + P6Z*0.03 + 0.00, P6X*0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
-    {{P1X*0.96 + P1Y*-0.27 + P1Z*0.05 + 0.00, P1X*0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, 
-     {P2X*0.96 + P2Y*-0.27 + P2Z*0.05 + 0.00, P2X*0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, 
-     {P3X*0.96 + P3Y*-0.27 + P3Z*0.05 + 0.00, P3X*0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, 
-     {P4X*0.96 + P4Y*-0.27 + P4Z*0.05 + 0.00, P4X*0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, 
-     {P5X*0.96 + P5Y*-0.27 + P5Z*0.05 + 0.00, P5X*0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, 
-     {P6X*0.96 + P6Y*-0.27 + P6Z*0.05 + 0.00, P6X*0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
-    {{P1X*0.94 + P1Y*-0.33 + P1Z*0.07 + 0.00, P1X*0.34 + P1Y*0.91 + P1Z*-0.25 + 0.00, P1X*0.02 + P1Y*0.26 + P1Z*0.97 + 0.00}, 
-     {P2X*0.94 + P2Y*-0.33 + P2Z*0.07 + 0.00, P2X*0.34 + P2Y*0.91 + P2Z*-0.25 + 0.00, P2X*0.02 + P2Y*0.26 + P2Z*0.97 + 0.00}, 
-     {P3X*0.94 + P3Y*-0.33 + P3Z*0.07 + 0.00, P3X*0.34 + P3Y*0.91 + P3Z*-0.25 + 0.00, P3X*0.02 + P3Y*0.26 + P3Z*0.97 + 0.00}, 
-     {P4X*0.94 + P4Y*-0.33 + P4Z*0.07 + 0.00, P4X*0.34 + P4Y*0.91 + P4Z*-0.25 + 0.00, P4X*0.02 + P4Y*0.26 + P4Z*0.97 + 0.00}, 
-     {P5X*0.94 + P5Y*-0.33 + P5Z*0.07 + 0.00, P5X*0.34 + P5Y*0.91 + P5Z*-0.25 + 0.00, P5X*0.02 + P5Y*0.26 + P5Z*0.97 + 0.00}, 
-     {P6X*0.94 + P6Y*-0.33 + P6Z*0.07 + 0.00, P6X*0.34 + P6Y*0.91 + P6Z*-0.25 + 0.00, P6X*0.02 + P6Y*0.26 + P6Z*0.97 + 0.00}},
-    {{P1X*0.96 + P1Y*-0.27 + P1Z*0.05 + 0.00, P1X*0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, 
-     {P2X*0.96 + P2Y*-0.27 + P2Z*0.05 + 0.00, P2X*0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, 
-     {P3X*0.96 + P3Y*-0.27 + P3Z*0.05 + 0.00, P3X*0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, 
-     {P4X*0.96 + P4Y*-0.27 + P4Z*0.05 + 0.00, P4X*0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, 
-     {P5X*0.96 + P5Y*-0.27 + P5Z*0.05 + 0.00, P5X*0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, 
-     {P6X*0.96 + P6Y*-0.27 + P6Z*0.05 + 0.00, P6X*0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
-    {{P1X*0.98 + P1Y*-0.21 + P1Z*0.03 + 0.00, P1X*0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, 
-     {P2X*0.98 + P2Y*-0.21 + P2Z*0.03 + 0.00, P2X*0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, 
-     {P3X*0.98 + P3Y*-0.21 + P3Z*0.03 + 0.00, P3X*0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, 
-     {P4X*0.98 + P4Y*-0.21 + P4Z*0.03 + 0.00, P4X*0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, 
-     {P5X*0.98 + P5Y*-0.21 + P5Z*0.03 + 0.00, P5X*0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, 
-     {P6X*0.98 + P6Y*-0.21 + P6Z*0.03 + 0.00, P6X*0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
-    {{P1X*0.99 + P1Y*-0.14 + P1Z*0.01 + 0.00, P1X*0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, 
-     {P2X*0.99 + P2Y*-0.14 + P2Z*0.01 + 0.00, P2X*0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, 
-     {P3X*0.99 + P3Y*-0.14 + P3Z*0.01 + 0.00, P3X*0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, 
-     {P4X*0.99 + P4Y*-0.14 + P4Z*0.01 + 0.00, P4X*0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, 
-     {P5X*0.99 + P5Y*-0.14 + P5Z*0.01 + 0.00, P5X*0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, 
-     {P6X*0.99 + P6Y*-0.14 + P6Z*0.01 + 0.00, P6X*0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
-    {{P1X*1.00 + P1Y*-0.07 + P1Z*0.00 + 0.00, P1X*0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, 
-     {P2X*1.00 + P2Y*-0.07 + P2Z*0.00 + 0.00, P2X*0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, 
-     {P3X*1.00 + P3Y*-0.07 + P3Z*0.00 + 0.00, P3X*0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, 
-     {P4X*1.00 + P4Y*-0.07 + P4Z*0.00 + 0.00, P4X*0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, 
-     {P5X*1.00 + P5Y*-0.07 + P5Z*0.00 + 0.00, P5X*0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, 
-     {P6X*1.00 + P6Y*-0.07 + P6Z*0.00 + 0.00, P6X*0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*0.00 + P6Y*0.09 + 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 + 0.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 + 0.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 + 0.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 + 0.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 + 0.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 + 0.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
-    {{P1X*1.00 + P1Y*0.07 + P1Z*-0.00 + 0.00, P1X*-0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*-0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, 
-     {P2X*1.00 + P2Y*0.07 + P2Z*-0.00 + 0.00, P2X*-0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*-0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, 
-     {P3X*1.00 + P3Y*0.07 + P3Z*-0.00 + 0.00, P3X*-0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*-0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, 
-     {P4X*1.00 + P4Y*0.07 + P4Z*-0.00 + 0.00, P4X*-0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*-0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, 
-     {P5X*1.00 + P5Y*0.07 + P5Z*-0.00 + 0.00, P5X*-0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*-0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, 
-     {P6X*1.00 + P6Y*0.07 + P6Z*-0.00 + 0.00, P6X*-0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*-0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
-    {{P1X*0.99 + P1Y*0.14 + P1Z*-0.01 + 0.00, P1X*-0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*-0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, 
-     {P2X*0.99 + P2Y*0.14 + P2Z*-0.01 + 0.00, P2X*-0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*-0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, 
-     {P3X*0.99 + P3Y*0.14 + P3Z*-0.01 + 0.00, P3X*-0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*-0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, 
-     {P4X*0.99 + P4Y*0.14 + P4Z*-0.01 + 0.00, P4X*-0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*-0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, 
-     {P5X*0.99 + P5Y*0.14 + P5Z*-0.01 + 0.00, P5X*-0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*-0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, 
-     {P6X*0.99 + P6Y*0.14 + P6Z*-0.01 + 0.00, P6X*-0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*-0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
-    {{P1X*0.98 + P1Y*0.21 + P1Z*-0.03 + 0.00, P1X*-0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*-0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, 
-     {P2X*0.98 + P2Y*0.21 + P2Z*-0.03 + 0.00, P2X*-0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*-0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, 
-     {P3X*0.98 + P3Y*0.21 + P3Z*-0.03 + 0.00, P3X*-0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*-0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, 
-     {P4X*0.98 + P4Y*0.21 + P4Z*-0.03 + 0.00, P4X*-0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*-0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, 
-     {P5X*0.98 + P5Y*0.21 + P5Z*-0.03 + 0.00, P5X*-0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*-0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, 
-     {P6X*0.98 + P6Y*0.21 + P6Z*-0.03 + 0.00, P6X*-0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*-0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
-    {{P1X*0.96 + P1Y*0.27 + P1Z*-0.05 + 0.00, P1X*-0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*-0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, 
-     {P2X*0.96 + P2Y*0.27 + P2Z*-0.05 + 0.00, P2X*-0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*-0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, 
-     {P3X*0.96 + P3Y*0.27 + P3Z*-0.05 + 0.00, P3X*-0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*-0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, 
-     {P4X*0.96 + P4Y*0.27 + P4Z*-0.05 + 0.00, P4X*-0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*-0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, 
-     {P5X*0.96 + P5Y*0.27 + P5Z*-0.05 + 0.00, P5X*-0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*-0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, 
-     {P6X*0.96 + P6Y*0.27 + P6Z*-0.05 + 0.00, P6X*-0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*-0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
-    {{P1X*0.94 + P1Y*0.33 + P1Z*-0.07 + 0.00, P1X*-0.34 + P1Y*0.91 + P1Z*-0.25 + 0.00, P1X*-0.02 + P1Y*0.26 + P1Z*0.97 + 0.00}, 
-     {P2X*0.94 + P2Y*0.33 + P2Z*-0.07 + 0.00, P2X*-0.34 + P2Y*0.91 + P2Z*-0.25 + 0.00, P2X*-0.02 + P2Y*0.26 + P2Z*0.97 + 0.00}, 
-     {P3X*0.94 + P3Y*0.33 + P3Z*-0.07 + 0.00, P3X*-0.34 + P3Y*0.91 + P3Z*-0.25 + 0.00, P3X*-0.02 + P3Y*0.26 + P3Z*0.97 + 0.00}, 
-     {P4X*0.94 + P4Y*0.33 + P4Z*-0.07 + 0.00, P4X*-0.34 + P4Y*0.91 + P4Z*-0.25 + 0.00, P4X*-0.02 + P4Y*0.26 + P4Z*0.97 + 0.00}, 
-     {P5X*0.94 + P5Y*0.33 + P5Z*-0.07 + 0.00, P5X*-0.34 + P5Y*0.91 + P5Z*-0.25 + 0.00, P5X*-0.02 + P5Y*0.26 + P5Z*0.97 + 0.00}, 
-     {P6X*0.94 + P6Y*0.33 + P6Z*-0.07 + 0.00, P6X*-0.34 + P6Y*0.91 + P6Z*-0.25 + 0.00, P6X*-0.02 + P6Y*0.26 + P6Z*0.97 + 0.00}},
-    {{P1X*0.96 + P1Y*0.27 + P1Z*-0.05 + 0.00, P1X*-0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*-0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, 
-     {P2X*0.96 + P2Y*0.27 + P2Z*-0.05 + 0.00, P2X*-0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*-0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, 
-     {P3X*0.96 + P3Y*0.27 + P3Z*-0.05 + 0.00, P3X*-0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*-0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, 
-     {P4X*0.96 + P4Y*0.27 + P4Z*-0.05 + 0.00, P4X*-0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*-0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, 
-     {P5X*0.96 + P5Y*0.27 + P5Z*-0.05 + 0.00, P5X*-0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*-0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, 
-     {P6X*0.96 + P6Y*0.27 + P6Z*-0.05 + 0.00, P6X*-0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*-0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
-    {{P1X*0.98 + P1Y*0.21 + P1Z*-0.03 + 0.00, P1X*-0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*-0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, 
-     {P2X*0.98 + P2Y*0.21 + P2Z*-0.03 + 0.00, P2X*-0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*-0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, 
-     {P3X*0.98 + P3Y*0.21 + P3Z*-0.03 + 0.00, P3X*-0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*-0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, 
-     {P4X*0.98 + P4Y*0.21 + P4Z*-0.03 + 0.00, P4X*-0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*-0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, 
-     {P5X*0.98 + P5Y*0.21 + P5Z*-0.03 + 0.00, P5X*-0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*-0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, 
-     {P6X*0.98 + P6Y*0.21 + P6Z*-0.03 + 0.00, P6X*-0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*-0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
-    {{P1X*0.99 + P1Y*0.14 + P1Z*-0.01 + 0.00, P1X*-0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*-0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, 
-     {P2X*0.99 + P2Y*0.14 + P2Z*-0.01 + 0.00, P2X*-0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*-0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, 
-     {P3X*0.99 + P3Y*0.14 + P3Z*-0.01 + 0.00, P3X*-0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*-0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, 
-     {P4X*0.99 + P4Y*0.14 + P4Z*-0.01 + 0.00, P4X*-0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*-0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, 
-     {P5X*0.99 + P5Y*0.14 + P5Z*-0.01 + 0.00, P5X*-0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*-0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, 
-     {P6X*0.99 + P6Y*0.14 + P6Z*-0.01 + 0.00, P6X*-0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*-0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
-    {{P1X*1.00 + P1Y*0.07 + P1Z*-0.00 + 0.00, P1X*-0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*-0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, 
-     {P2X*1.00 + P2Y*0.07 + P2Z*-0.00 + 0.00, P2X*-0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*-0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, 
-     {P3X*1.00 + P3Y*0.07 + P3Z*-0.00 + 0.00, P3X*-0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*-0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, 
-     {P4X*1.00 + P4Y*0.07 + P4Z*-0.00 + 0.00, P4X*-0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*-0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, 
-     {P5X*1.00 + P5Y*0.07 + P5Z*-0.00 + 0.00, P5X*-0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*-0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, 
-     {P6X*1.00 + P6Y*0.07 + P6Z*-0.00 + 0.00, P6X*-0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*-0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
-};
-const int twist_entries[] { 0,10 };
-const MovementTable twist_table {twist_paths, 20, 50, twist_entries, 2 };
-}
-
-const MovementTable& backwardTable() {
-    return backward_table;
-}
-const MovementTable& climbTable() {
-    return climb_table;
-}
-const MovementTable& forwardTable() {
-    return forward_table;
-}
-const MovementTable& forwardfastTable() {
-    return forwardfast_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;
-}
-const MovementTable& twistTable() {
-    return twist_table;
-}

software/archive/hexapod7697/src/hexapod/movements.cpp

@@ -1,59 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/servo.cpp

@@ -1,117 +0,0 @@
-#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/archive/hexapod7697/src/hexapod/servo.h

@@ -1,38 +0,0 @@
-#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/archive/hexapod7697/src/linkit_control/ui_controls.cpp

@@ -1,83 +0,0 @@
-#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/archive/hexapod7697/src/linkit_control/ui_controls.h

@@ -1,65 +0,0 @@
-#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/archive/hexapod7697/src/normal_mode.cpp

@@ -1,94 +0,0 @@
-#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, 3, 1, 1, RC_BLUE);
-static Button buttonRun(hexapod::MOVEMENT_FORWARDFAST, "Run", 1, 2, 1, 1, RC_ORANGE);
-static Button buttonBackward(hexapod::MOVEMENT_BACKWARD, "Backward", 1, 4, 1, 1, RC_BLUE);
-static Button buttonTL(hexapod::MOVEMENT_TURNLEFT, "TurnLeft", 0, 3, 1, 1, RC_GREEN);
-static Button buttonTR(hexapod::MOVEMENT_TURNRIGHT, "TurnRight", 2, 3, 1, 1, RC_GREEN);
-static Button buttonSL(hexapod::MOVEMENT_SHIFTLEFT, "ShiftLeft", 0, 4, 1, 1, RC_BLUE);
-static Button buttonSR(hexapod::MOVEMENT_SHIFTRIGHT, "ShiftRight", 2, 4, 1, 1, RC_BLUE);
-static Button buttonClimb(hexapod::MOVEMENT_CLIMB, "Climb", 2, 2, 1, 1, RC_ORANGE);
-
-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 Button buttonTwist(hexapod::MOVEMENT_TWIST, "Twist", 0, 1, 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,
-    &buttonRun, &buttonClimb,
-    &buttonRotateX, &buttonRotateY, &buttonRotateZ,
-    &buttonTwist,
-  });
-  btnGroup.addControls({
-    &buttonForward, &buttonBackward,
-    &buttonTL, &buttonTR,
-    &buttonSL, &buttonSR,
-    &buttonRun, &buttonClimb,
-    &buttonRotateX, &buttonRotateY, &buttonRotateZ,
-    &buttonTwist,
-  });
-
-  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/archive/hexapod7697/src/normal_mode.h

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

software/archive/hexapod7697/src/setting_mode.cpp

@@ -1,220 +0,0 @@
-#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/archive/hexapod7697/src/setting_mode.h

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