add sch and src
Smallp Tsai
6 years ago
parent
c517f5355a
commit
5ccaab4c49
@ -0,0 +1,52 @@
|
||||
# Electronics
|
||||
|
||||
## Connection Diagram
|
||||
|
||||
|
||||
|
||||
## Bill of Materials
|
||||
|
||||
### PCB
|
||||
|
||||
Name | Thumbnail | Required |
|
||||
-------- | --------- | -------- |
|
||||
main | | 1 |
|
||||
sub | | 2 |
|
||||
|
||||
### Components
|
||||
|
||||
Name | Thumbnail | Required |
|
||||
-------- | --------- | -------- |
|
||||
[Linkit 7697](https://labs.mediatek.com/zh-tw/platform/linkit-7697) | <img width=200 src='https://labs.mediatek.com/images/img_hdk_7697.png'/> | 1 |
|
||||
[mini360 DC-DC](https://www.aliexpress.com/w/wholesale-mini360-dc-dc.html) Buck voltage regulator |  | 7 |
|
||||
[PCA9865](https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf) (TSSOP28)|  | 2 |
|
||||
Resistor: 220 ohm (0805) | | 18 |
|
||||
Resistor: 10K ohm (0805) | | 12 |
|
||||
Resistor: 470 ohm (0805) | | 1 |
|
||||
LED: green or any color (0805) | | 1 |
|
||||
Capacitor: 10 uF (0805) | | 2 |
|
||||
14 pin 2.54mm female header | | 4 |
|
||||
4 pin 2.54mm female header | | 2 |
|
||||
6 pin 2.54mm female header | | 2 |
|
||||
4 pin 2.54mm 90 degress male header | | 2 |
|
||||
6 pin 2.54mm 90 degress male header | | 2 |
|
||||
4 pin 2.54mm male header | | 2 |
|
||||
3 pin 2.54mm male header (black) | | 6 |
|
||||
3 pin 2.54mm male header (yellow) | | 6 |
|
||||
3 pin 2.54mm male header (red) | | 6 |
|
||||
2 pin 2.54mm male header | | 1 |
|
||||
Jumper 2.54mm | | 1 |
|
||||
|
||||
## PCB detail
|
||||
|
||||
> Please use 'Eagle' software to open below schematics
|
||||
|
||||
### Main
|
||||
|
||||
|
||||
|
||||
|
||||
### Sub (x2)
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 228 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 32 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 31 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 60 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 48 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 31 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 57 KiB |
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,29 @@
|
||||
# Software
|
||||
|
||||
Software contain 2 parts, one is software of running on Linkt 7697 (an arduino C++ program), another is path generation (a python program).
|
||||
|
||||
* hexapod7697: an arduino program running on Linkit 7697
|
||||
* pathTool: a python program that generate 3D points header, included by `hexapod7697`
|
||||
|
||||
## hexapod7697
|
||||
|
||||
Files/Folder | Description |
|
||||
------------ | ----------- |
|
||||
src/normal_mode | provide normal calibration (forward/backward/turn left/turn right/rotate/etc...) |
|
||||
src/setting_mode | provide calirbration functions, to make sure servo is correctly aligned. |
|
||||
src/linkit_control/ | UI control helper |
|
||||
src/hexapod | hexapod class |
|
||||
src/hexapod/hal | Hardware Abatraction Layer to running on Linkit 7697 |
|
||||
|
||||
> hexapod7697 use c++11 and STL, you may encounter problem if want to port it on pure Arduino platform.
|
||||
|
||||
## pathTool
|
||||
|
||||
* Python compatibility:
|
||||
* 3.x: ok.
|
||||
* 2.x: not verified.
|
||||
* Required package
|
||||
* numpy: `pip3 install numpy`
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1 @@
|
||||
.vscode/
|
||||
@ -0,0 +1,84 @@
|
||||
|
||||
#include "src/setting_mode.h"
|
||||
#include "src/normal_mode.h"
|
||||
|
||||
#include "src/hexapod/debug.h"
|
||||
#include "src/hexapod/hexapod.h"
|
||||
|
||||
//
|
||||
// mode
|
||||
// 0: normal mode
|
||||
// 1: setting mode (for servo calibration)
|
||||
//
|
||||
static int _mode = 0;
|
||||
|
||||
static void log_output(const char* log) {
|
||||
Serial.println(log);
|
||||
}
|
||||
|
||||
//
|
||||
// wait for N secs, enter setting mode if pressed USR button
|
||||
//
|
||||
void boot_wait(int wait_for = 2000) {
|
||||
digitalWrite(LED_BUILTIN, HIGH);
|
||||
|
||||
pinMode(6, INPUT);
|
||||
while (wait_for > 0) {
|
||||
if (HIGH == digitalRead(6)) {
|
||||
_mode = 1;
|
||||
break;
|
||||
}
|
||||
delay(100);
|
||||
wait_for -= 100;
|
||||
}
|
||||
|
||||
digitalWrite(LED_BUILTIN, LOW);
|
||||
}
|
||||
|
||||
//
|
||||
// Do common setup and mode-specific setup
|
||||
//
|
||||
void setup() {
|
||||
//Initialize serial and wait for port to open:
|
||||
Serial.begin(115200);
|
||||
Serial.println("Starting...");
|
||||
|
||||
//
|
||||
// Common setup()
|
||||
//
|
||||
|
||||
// Initialize GPIO
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
|
||||
// Workaround for beta PCB
|
||||
pinMode(2, INPUT);
|
||||
pinMode(3, INPUT);
|
||||
|
||||
boot_wait();
|
||||
|
||||
hexapod::initLogOutput(log_output, millis);
|
||||
hexapod::Hexapod.init(_mode == 1);
|
||||
|
||||
//
|
||||
// Mode-specific setup()
|
||||
//
|
||||
|
||||
if (_mode == 0) {
|
||||
normal_setup();
|
||||
}
|
||||
else if (_mode == 1) {
|
||||
setting_setup();
|
||||
}
|
||||
|
||||
Serial.print("Started, mode=");
|
||||
Serial.println(_mode);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (_mode == 0) {
|
||||
normal_loop();
|
||||
}
|
||||
else if (_mode == 1) {
|
||||
setting_loop();
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,71 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
class Point3D {
|
||||
public:
|
||||
Point3D() = default;
|
||||
Point3D(float x, float y, float z): x_{x}, y_{y}, z_{z} {
|
||||
}
|
||||
|
||||
public:
|
||||
float x_;
|
||||
float y_;
|
||||
float z_;
|
||||
};
|
||||
|
||||
inline Point3D operator-(const Point3D &lhs, const Point3D &rhs) {
|
||||
return Point3D(lhs.x_ - rhs.x_, lhs.y_ - rhs.y_, lhs.z_ - rhs.z_);
|
||||
};
|
||||
|
||||
inline Point3D operator*(const Point3D &lhs, const float& b) {
|
||||
return Point3D(lhs.x_ * b, lhs.y_ * b, lhs.z_ * b);
|
||||
};
|
||||
|
||||
inline Point3D& operator +=(Point3D& a, const Point3D& b) {
|
||||
a.x_ += b.x_;
|
||||
a.y_ += b.y_;
|
||||
a.z_ += b.z_;
|
||||
return a;
|
||||
}
|
||||
|
||||
inline bool operator ==(const Point3D &a, const Point3D &b) {
|
||||
return (a.x_ == b.x_) && (a.y_ == b.y_) && (a.z_ == b.z_);
|
||||
}
|
||||
|
||||
class Locations {
|
||||
public:
|
||||
Locations() = default;
|
||||
Locations(const Point3D& foreRight, const Point3D& right, const Point3D& hindRight, const Point3D& hindLeft, const Point3D& left, const Point3D& foreLeft):
|
||||
points_{foreRight, right, hindRight, hindLeft, left, foreLeft} {
|
||||
}
|
||||
|
||||
const Point3D& get(int index) const {
|
||||
return points_[index];
|
||||
}
|
||||
|
||||
Locations operator-(const Locations &b) const {
|
||||
return Locations{
|
||||
points_[0] - b.points_[0], points_[1] - b.points_[1], points_[2] - b.points_[2],
|
||||
points_[3] - b.points_[3], points_[4] - b.points_[4], points_[5] - b.points_[5],
|
||||
};
|
||||
}
|
||||
|
||||
Locations operator*(const float& b) const {
|
||||
return Locations{
|
||||
points_[0] * b, points_[1] * b, points_[2] * b,
|
||||
points_[3] * b, points_[4] * b, points_[5] * b,
|
||||
};
|
||||
}
|
||||
|
||||
Locations& operator +=(const Locations& b) {
|
||||
for(auto i=0;i<6;i++)
|
||||
points_[i] += b.points_[i];
|
||||
return *this;
|
||||
}
|
||||
private:
|
||||
Point3D points_[6];
|
||||
};
|
||||
}
|
||||
@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
namespace config {
|
||||
// all below definition use unit: mm
|
||||
const float kLegMountLeftRightX = 29.87;
|
||||
const float kLegMountOtherX = 22.41;
|
||||
const float kLegMountOtherY = 55.41;
|
||||
|
||||
const float kLegRootToJoint1 = 20.75;
|
||||
const float kLegJoint1ToJoint2 = 28.0;
|
||||
const float kLegJoint2ToJoint3 = 42.6;
|
||||
const float kLegJoint3ToTip = 89.07;
|
||||
|
||||
|
||||
// timing setting. unit: ms
|
||||
const int movementInterval = 5;
|
||||
const int movementSwitchDuration = 150;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,36 @@
|
||||
#include "debug.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h> // For va_start, etc.
|
||||
|
||||
namespace {
|
||||
std::function<void(const char*)> _writer = nullptr;
|
||||
std::function<int(void)> _time_func = nullptr;
|
||||
}
|
||||
|
||||
void _my_log_impl(const char* format, ...)
|
||||
{
|
||||
char buffer[256];
|
||||
int pos = 0;
|
||||
|
||||
if(_time_func) {
|
||||
pos = sprintf(buffer, "[%d]", _time_func());
|
||||
}
|
||||
|
||||
if (_writer) {
|
||||
va_list ap;
|
||||
va_start(ap, format);
|
||||
vsprintf(buffer+pos, format, ap);
|
||||
va_end(ap);
|
||||
_writer(buffer);
|
||||
}
|
||||
}
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
void initLogOutput(std::function<void(const char*)> writer, std::function<int(void)> time_func) {
|
||||
_writer = writer;
|
||||
_time_func = time_func;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,25 @@
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
|
||||
#undef LOG_DEBUG_ON
|
||||
#define LOG_INFO_ON
|
||||
|
||||
|
||||
#ifdef LOG_DEBUG_ON
|
||||
void _my_log_impl(const char* format, ...);
|
||||
#define LOG_DEBUG(format, ...) _my_log_impl(format, ##__VA_ARGS__)
|
||||
#else
|
||||
#define LOG_DEBUG(format, ...)
|
||||
#endif
|
||||
|
||||
#ifdef LOG_INFO_ON
|
||||
void _my_log_impl(const char* format, ...);
|
||||
#define LOG_INFO(format, ...) _my_log_impl(format, ##__VA_ARGS__)
|
||||
#else
|
||||
#define LOG_INFO(format, ...)
|
||||
#endif
|
||||
|
||||
namespace hexapod {
|
||||
void initLogOutput(std::function<void(const char*)> writer, std::function<int(void)> time_func);
|
||||
}
|
||||
@ -0,0 +1,29 @@
|
||||
|
||||
#include <Wire.h>
|
||||
#include <Adafruit_PWMServoDriver.h>
|
||||
|
||||
#include "pwm.h"
|
||||
|
||||
namespace hexapod { namespace hal {
|
||||
|
||||
PCA9685::PCA9685(int i2cAddress) {
|
||||
obj_ = (void*)new Adafruit_PWMServoDriver(i2cAddress);
|
||||
}
|
||||
|
||||
PCA9685::~PCA9685() {
|
||||
delete ((Adafruit_PWMServoDriver*)obj_);
|
||||
}
|
||||
|
||||
void PCA9685::begin() {
|
||||
((Adafruit_PWMServoDriver*)obj_)->begin();
|
||||
}
|
||||
|
||||
void PCA9685::setPWMFreq(int freq) {
|
||||
((Adafruit_PWMServoDriver*)obj_)->setPWMFreq(freq);
|
||||
}
|
||||
|
||||
void PCA9685::setPWM(int index, int on, int off) {
|
||||
((Adafruit_PWMServoDriver*)obj_)->setPWM(index, (uint16_t)on, (uint16_t)off);
|
||||
}
|
||||
|
||||
}}
|
||||
@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
namespace hexapod { namespace hal {
|
||||
|
||||
class PCA9685 {
|
||||
public:
|
||||
PCA9685(int i2cAddress = 0x40);
|
||||
~PCA9685();
|
||||
|
||||
void begin();
|
||||
void setPWMFreq(int freq);
|
||||
void setPWM(int index, int on, int off);
|
||||
|
||||
private:
|
||||
void* obj_;
|
||||
};
|
||||
|
||||
}}
|
||||
@ -0,0 +1,99 @@
|
||||
|
||||
#include "hexapod.h"
|
||||
#include "servo.h"
|
||||
#include "debug.h"
|
||||
|
||||
// TBD: move to hal
|
||||
#include <LFlash.h>
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
HexapodClass Hexapod;
|
||||
|
||||
HexapodClass::HexapodClass():
|
||||
legs_{{0}, {1}, {2}, {3}, {4}, {5}},
|
||||
movement_{MOVEMENT_STANDBY},
|
||||
mode_{MOVEMENT_STANDBY}
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void HexapodClass::init(bool setting) {
|
||||
Servo::init();
|
||||
|
||||
LFlash.begin();
|
||||
calibrationLoad();
|
||||
|
||||
// default to standby mode
|
||||
if (!setting)
|
||||
processMovement(MOVEMENT_STANDBY);
|
||||
|
||||
LOG_INFO("Hexapod init done.");
|
||||
}
|
||||
|
||||
void HexapodClass::processMovement(MovementMode mode, int elapsed) {
|
||||
if (mode_ != mode) {
|
||||
mode_ = mode;
|
||||
movement_.setMode(mode_);
|
||||
}
|
||||
|
||||
auto& location = movement_.next(elapsed);
|
||||
for(int i=0;i<6;i++) {
|
||||
legs_[i].moveTip(location.get(i));
|
||||
}
|
||||
}
|
||||
|
||||
void HexapodClass::calibrationSave() {
|
||||
|
||||
short data[6*3*2];
|
||||
|
||||
for(int i=0;i<6;i++) {
|
||||
for(int j=0;j<3;j++) {
|
||||
int offset, scale;
|
||||
legs_[i].get(j)->getParameter(offset, scale);
|
||||
data[2*3*i + 2*j] = (short)offset;
|
||||
data[2*3*i + 2*j + 1] = (short)scale;
|
||||
}
|
||||
}
|
||||
|
||||
LFlash.write(
|
||||
"HEXAPOD",
|
||||
"CALI",
|
||||
LFLASH_RAW_DATA,
|
||||
(const uint8_t *)data,
|
||||
sizeof(data)
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
void HexapodClass::calibrationGet(int legIndex, int partIndex, int& offset, int& scale) {
|
||||
legs_[legIndex].get(partIndex)->getParameter(offset, scale);
|
||||
}
|
||||
|
||||
void HexapodClass::calibrationSet(int legIndex, int partIndex, int offset, int scale) {
|
||||
legs_[legIndex].get(partIndex)->setParameter(offset, scale);
|
||||
}
|
||||
|
||||
void HexapodClass::calibrationTest(int legIndex, int partIndex, float angle) {
|
||||
legs_[legIndex].get(partIndex)->setAngle(angle);
|
||||
}
|
||||
|
||||
void HexapodClass::calibrationLoad() {
|
||||
short data[6*3*2] = {0};
|
||||
uint32_t size = sizeof(data);
|
||||
|
||||
LFlash.read(
|
||||
"HEXAPOD",
|
||||
"CALI",
|
||||
(uint8_t *)data,
|
||||
&size
|
||||
);
|
||||
|
||||
for(int i=0;i<6;i++) {
|
||||
for(int j=0;j<3;j++) {
|
||||
legs_[i].get(j)->setParameter(data[2*3*i + 2*j], data[2*3*i + 2*j + 1], false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include "movement.h"
|
||||
#include "leg.h"
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
class HexapodClass {
|
||||
public:
|
||||
HexapodClass();
|
||||
|
||||
// init
|
||||
|
||||
void init(bool setting = false);
|
||||
|
||||
// Movement API
|
||||
|
||||
void processMovement(MovementMode mode, int elapsed = 0);
|
||||
|
||||
// Calibration API
|
||||
|
||||
void calibrationSave(); // write to flash
|
||||
void calibrationGet(int legIndex, int partIndex, int& offset, int& scale); // read servo setting
|
||||
void calibrationSet(int legIndex, int partIndex, int offset, int scale); // update servo setting
|
||||
void calibrationTest(int legIndex, int partIndex, float angle); // test servo setting
|
||||
|
||||
private:
|
||||
void calibrationLoad(); // read from flash
|
||||
|
||||
private:
|
||||
MovementMode mode_;
|
||||
Movement movement_;
|
||||
Leg legs_[6];
|
||||
};
|
||||
|
||||
extern HexapodClass Hexapod;
|
||||
}
|
||||
@ -0,0 +1,201 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "leg.h"
|
||||
#include "config.h"
|
||||
#include "debug.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace hexapod::config;
|
||||
|
||||
// Angle (CCW)
|
||||
// 0 : + X-axis
|
||||
// 90 : + Y-axis
|
||||
// 180 : - X-axis
|
||||
// 270 : - Y-axis
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
namespace {
|
||||
|
||||
// rotate matrix:
|
||||
// x' = x * cos - y * sin
|
||||
// y' = x * sin + y * cos
|
||||
|
||||
#define SIN45 0.7071
|
||||
#define COS45 0.7071
|
||||
|
||||
void rotate0(const Point3D& src, Point3D& dest) {
|
||||
dest = src;
|
||||
}
|
||||
|
||||
void rotate45(const Point3D& src, Point3D& dest) {
|
||||
dest.x_ = src.x_ * COS45 - src.y_ * SIN45;
|
||||
dest.y_ = src.x_ * SIN45 + src.y_ * COS45;
|
||||
dest.z_ = src.z_;
|
||||
}
|
||||
|
||||
void rotate135(const Point3D& src, Point3D& dest) {
|
||||
dest.x_ = src.x_ * (-COS45) - src.y_ * SIN45;
|
||||
dest.y_ = src.x_ * SIN45 + src.y_ * (-COS45);
|
||||
dest.z_ = src.z_;
|
||||
}
|
||||
|
||||
void rotate180(const Point3D& src, Point3D& dest) {
|
||||
dest.x_ = -src.x_;
|
||||
dest.y_ = -src.y_;
|
||||
dest.z_ = src.z_;
|
||||
}
|
||||
|
||||
void rotate225(const Point3D& src, Point3D& dest) {
|
||||
dest.x_ = src.x_ * (-COS45) - src.y_ * (-SIN45);
|
||||
dest.y_ = src.x_ * (-SIN45) + src.y_ * (-COS45);
|
||||
dest.z_ = src.z_;
|
||||
}
|
||||
|
||||
void rotate315(const Point3D& src, Point3D& dest) {
|
||||
dest.x_ = src.x_ * COS45 - src.y_ * (-SIN45);
|
||||
dest.y_ = src.x_ * (-SIN45) + src.y_ * COS45;
|
||||
dest.z_ = src.z_;
|
||||
}
|
||||
}
|
||||
|
||||
// Public
|
||||
|
||||
Leg::Leg(int legIndex): index_(legIndex) {
|
||||
switch(legIndex) {
|
||||
case 0: // 45 degree
|
||||
mountPosition_ = Point3D(kLegMountOtherX, kLegMountOtherY, 0);
|
||||
localConv_ = rotate315;
|
||||
worldConv_ = rotate45;
|
||||
break;
|
||||
case 1: // 0 degree
|
||||
mountPosition_ = Point3D(kLegMountLeftRightX, 0, 0);
|
||||
localConv_ = rotate0;
|
||||
worldConv_ = rotate0;
|
||||
break;
|
||||
case 2: // -45 or 315 degree
|
||||
mountPosition_ = Point3D(kLegMountOtherX, -kLegMountOtherY, 0);
|
||||
localConv_ = rotate45;
|
||||
worldConv_ = rotate315;
|
||||
break;
|
||||
case 3: // -135 or 225 degree
|
||||
mountPosition_ = Point3D(-kLegMountOtherX, -kLegMountOtherY, 0);
|
||||
localConv_ = rotate135;
|
||||
worldConv_ = rotate225;
|
||||
break;
|
||||
case 4: // 180 degree
|
||||
mountPosition_ = Point3D(-kLegMountLeftRightX, 0, 0);
|
||||
localConv_ = rotate180;
|
||||
worldConv_ = rotate180;
|
||||
break;
|
||||
case 5: // 135 degree
|
||||
mountPosition_ = Point3D(-kLegMountOtherX, kLegMountOtherY, 0);
|
||||
localConv_ = rotate225;
|
||||
worldConv_ = rotate135;
|
||||
break;
|
||||
}
|
||||
|
||||
for(int i=0;i<3;i++)
|
||||
servos_[i] = new hexapod::Servo(legIndex, i);
|
||||
}
|
||||
|
||||
Leg::~Leg() {
|
||||
for(int i=0;i<3;i++) {
|
||||
delete servos_[i];
|
||||
servos_[i] = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void Leg::translateToLocal(const Point3D& world, Point3D& local) {
|
||||
localConv_(world - mountPosition_, local);
|
||||
}
|
||||
|
||||
void Leg::translateToWorld(const Point3D& local, Point3D& world) {
|
||||
worldConv_(local, world);
|
||||
world += mountPosition_;
|
||||
}
|
||||
|
||||
void Leg::setJointAngle(float angle[3]) {
|
||||
Point3D to;
|
||||
_forwardKinematics(angle, to);
|
||||
moveTipLocal(to);
|
||||
}
|
||||
|
||||
void Leg::moveTip(const Point3D& to) {
|
||||
if (to == tipPos_)
|
||||
return;
|
||||
|
||||
Point3D local;
|
||||
translateToLocal(to, local);
|
||||
LOG_DEBUG("leg(%d) moveTip(%f,%f,%f)(%f,%f,%f)", index_, to.x_, to.y_, to.z_, local.x_, local.y_, local.z_);
|
||||
_move(local);
|
||||
tipPos_ = to;
|
||||
tipPosLocal_ = local;
|
||||
}
|
||||
|
||||
const Point3D& Leg::getTipPosition(void) {
|
||||
return tipPos_;
|
||||
}
|
||||
|
||||
void Leg::moveTipLocal(const Point3D& to) {
|
||||
if (to == tipPosLocal_)
|
||||
return;
|
||||
|
||||
Point3D world;
|
||||
translateToWorld(to, world);
|
||||
_move(to);
|
||||
tipPos_ = world;
|
||||
tipPosLocal_ = to;
|
||||
}
|
||||
|
||||
const Point3D& Leg::getTipPositionLocal(void) {
|
||||
return tipPosLocal_;
|
||||
}
|
||||
|
||||
//
|
||||
// Private
|
||||
//
|
||||
const float pi = std::acos(-1);
|
||||
const float hpi = pi/2;
|
||||
|
||||
void Leg::_forwardKinematics(float angle[3], Point3D& out) {
|
||||
float radian[3];
|
||||
for(int i=0; i<3; i++)
|
||||
radian[i] = pi * angle [i] / 180;
|
||||
|
||||
float x = kLegJoint1ToJoint2 + std::cos(radian[1]) * kLegJoint2ToJoint3 + std::cos(radian[1] + radian[2] - hpi) * kLegJoint3ToTip;
|
||||
|
||||
out.x_ = kLegRootToJoint1 + std::cos(radian[0]) * x;
|
||||
out.y_ = std::sin(radian[0]) * x;
|
||||
out.z_ = std::sin(radian[1]) * kLegJoint2ToJoint3 + std::sin(radian[1] + radian[2] - hpi) * kLegJoint3ToTip;
|
||||
}
|
||||
|
||||
void Leg::_inverseKinematics(const Point3D& to, float angles[3]) {
|
||||
|
||||
float x = to.x_ - kLegRootToJoint1;
|
||||
float y = to.y_;
|
||||
|
||||
angles[0] = std::atan2(y, x) * 180 / pi;
|
||||
|
||||
x = std::sqrt(x*x + y*y) - kLegJoint1ToJoint2;
|
||||
y = to.z_;
|
||||
float ar = std::atan2(y, x);
|
||||
float lr2 = x*x + y*y;
|
||||
float lr = std::sqrt(lr2);
|
||||
float a1 = std::acos((lr2 + kLegJoint2ToJoint3*kLegJoint2ToJoint3 - kLegJoint3ToTip*kLegJoint3ToTip)/(2*kLegJoint2ToJoint3*lr));
|
||||
float a2 = std::acos((lr2 - kLegJoint2ToJoint3*kLegJoint2ToJoint3 + kLegJoint3ToTip*kLegJoint3ToTip)/(2*kLegJoint3ToTip*lr));
|
||||
angles[1] = (ar + a1) * 180 / pi;
|
||||
angles[2] = 90 - ((a1 + a2) * 180 / pi);
|
||||
}
|
||||
|
||||
void Leg::_move(const Point3D& to) {
|
||||
float angles[3];
|
||||
_inverseKinematics(to, angles);
|
||||
LOG_DEBUG("leg(%d) move: (%f,%f,%f)", index_, angles[0], angles[1], angles[2]);
|
||||
for(int i=0; i<3; i++) {
|
||||
servos_[i]->setAngle(angles[i]);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#include "base.h"
|
||||
#include "servo.h"
|
||||
|
||||
#include <functional>
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
class Leg {
|
||||
public:
|
||||
Leg(int legIndex);
|
||||
virtual ~Leg();
|
||||
|
||||
// coordinate system translation
|
||||
void translateToLocal(const Point3D& world, Point3D& local);
|
||||
void translateToWorld(const Point3D& local, Point3D& world);
|
||||
|
||||
void setJointAngle(float angle[3]);
|
||||
|
||||
// world coordinate system (default)
|
||||
void moveTip(const Point3D& to);
|
||||
const Point3D& getTipPosition(void);
|
||||
|
||||
// local coordinate system version
|
||||
void moveTipLocal(const Point3D& to);
|
||||
const Point3D& getTipPositionLocal(void);
|
||||
|
||||
//
|
||||
Servo* get(int partIndex) {
|
||||
return servos_[partIndex];
|
||||
}
|
||||
|
||||
private:
|
||||
// Local coorinate system
|
||||
static void _forwardKinematics(float angle[3], Point3D& out);
|
||||
static void _inverseKinematics(const Point3D& to, float angles[3]);
|
||||
void _move(const Point3D& to);
|
||||
|
||||
private:
|
||||
int index_;
|
||||
Servo* servos_[3];
|
||||
Point3D mountPosition_;
|
||||
std::function<void(const Point3D& src, Point3D& dest)> localConv_;
|
||||
std::function<void(const Point3D& src, Point3D& dest)> worldConv_;
|
||||
|
||||
Point3D tipPos_;
|
||||
Point3D tipPosLocal_;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
@ -0,0 +1,67 @@
|
||||
#include "movement.h"
|
||||
#include "debug.h"
|
||||
#include "config.h"
|
||||
|
||||
#include <cstdlib>
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
extern const MovementTable& standbyTable();
|
||||
extern const MovementTable& forwardTable();
|
||||
extern const MovementTable& backwardTable();
|
||||
extern const MovementTable& turnleftTable();
|
||||
extern const MovementTable& turnrightTable();
|
||||
extern const MovementTable& shiftleftTable();
|
||||
extern const MovementTable& shiftrightTable();
|
||||
extern const MovementTable& rotatexTable();
|
||||
extern const MovementTable& rotateyTable();
|
||||
extern const MovementTable& rotatezTable();
|
||||
|
||||
const MovementTable kTable[MOVEMENT_TOTAL] {
|
||||
standbyTable(),
|
||||
forwardTable(),
|
||||
backwardTable(),
|
||||
turnleftTable(),
|
||||
turnrightTable(),
|
||||
shiftleftTable(),
|
||||
shiftrightTable(),
|
||||
rotatexTable(),
|
||||
rotateyTable(),
|
||||
rotatezTable(),
|
||||
};
|
||||
|
||||
Movement::Movement(MovementMode mode):
|
||||
mode_{mode}, transiting_{false}
|
||||
{
|
||||
}
|
||||
|
||||
void Movement::setMode(MovementMode newMode) {
|
||||
mode_ = newMode;
|
||||
|
||||
const MovementTable& table = kTable[mode_];
|
||||
|
||||
index_ = table.entries[std::rand() % table.entriesCount];
|
||||
remainTime_ = config::movementSwitchDuration > table.stepDuration ? config::movementSwitchDuration : table.stepDuration;
|
||||
}
|
||||
|
||||
const Locations& Movement::next(int elapsed) {
|
||||
|
||||
const MovementTable& table = kTable[mode_];
|
||||
|
||||
if (elapsed <= 0)
|
||||
elapsed = table.stepDuration;
|
||||
|
||||
if (remainTime_ <= 0) {
|
||||
index_ = (index_ + 1)%table.length;
|
||||
remainTime_ = table.stepDuration;
|
||||
}
|
||||
if (elapsed >= remainTime_)
|
||||
elapsed = remainTime_;
|
||||
|
||||
auto ratio = (float)elapsed / remainTime_;
|
||||
position_ += (table.table[index_] - position_)*ratio;
|
||||
remainTime_ -= elapsed;
|
||||
|
||||
return position_;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,53 @@
|
||||
#pragma once
|
||||
|
||||
#include "base.h"
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
enum MovementMode {
|
||||
MOVEMENT_STANDBY = 0,
|
||||
|
||||
MOVEMENT_FORWARD,
|
||||
MOVEMENT_BACKWARD,
|
||||
MOVEMENT_TURNLEFT,
|
||||
MOVEMENT_TURNRIGHT,
|
||||
MOVEMENT_SHIFTLEFT,
|
||||
MOVEMENT_SHIFTRIGHT,
|
||||
MOVEMENT_ROTATEX,
|
||||
MOVEMENT_ROTATEY,
|
||||
MOVEMENT_ROTATEZ,
|
||||
|
||||
MOVEMENT_TOTAL,
|
||||
};
|
||||
|
||||
inline MovementMode operator++ (MovementMode& m, int) {
|
||||
if (m < MOVEMENT_TOTAL)
|
||||
m = static_cast<MovementMode>(static_cast<int>(m) + 1);
|
||||
return m;
|
||||
}
|
||||
|
||||
struct MovementTable {
|
||||
const Locations* table;
|
||||
int length;
|
||||
int stepDuration;
|
||||
const int* entries;
|
||||
int entriesCount;
|
||||
};
|
||||
|
||||
class Movement {
|
||||
public:
|
||||
Movement(MovementMode mode);
|
||||
|
||||
void setMode(MovementMode newMode);
|
||||
|
||||
const Locations& next(int elapsed);
|
||||
|
||||
private:
|
||||
MovementMode mode_;
|
||||
Locations position_;
|
||||
int index_; // index in mode position table
|
||||
bool transiting_; // if still in transiting to new mode
|
||||
int remainTime_;
|
||||
};
|
||||
|
||||
}
|
||||
@ -0,0 +1,558 @@
|
||||
//
|
||||
// This file is generated, dont directly modify content...
|
||||
//
|
||||
namespace {
|
||||
|
||||
const Locations backward_paths[] {
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(4.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(8.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(12.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(16.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(19.02), P6Z+(6.18)}},
|
||||
{{P1X+(0.00), P1Y+(-12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(16.18), P6Z+(11.76)}},
|
||||
{{P1X+(0.00), P1Y+(-8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(11.76), P6Z+(16.18)}},
|
||||
{{P1X+(0.00), P1Y+(-4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(6.18), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}},
|
||||
{{P1X+(0.00), P1Y+(4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-6.18), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-11.76), P6Z+(16.18)}},
|
||||
{{P1X+(0.00), P1Y+(12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-16.18), P6Z+(11.76)}},
|
||||
{{P1X+(0.00), P1Y+(16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-19.02), P6Z+(6.18)}},
|
||||
{{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(0.00)}},
|
||||
};
|
||||
const int backward_entries[] { 0,10 };
|
||||
const MovementTable backward_table {backward_paths, 20, 20, backward_entries, 2 };
|
||||
|
||||
const Locations forward_paths[] {
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-19.02), P6Z+(6.18)}},
|
||||
{{P1X+(0.00), P1Y+(12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-16.18), P6Z+(11.76)}},
|
||||
{{P1X+(0.00), P1Y+(8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-11.76), P6Z+(16.18)}},
|
||||
{{P1X+(0.00), P1Y+(4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-6.18), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}},
|
||||
{{P1X+(0.00), P1Y+(-4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(6.18), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(-8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(11.76), P6Z+(16.18)}},
|
||||
{{P1X+(0.00), P1Y+(-12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(16.18), P6Z+(11.76)}},
|
||||
{{P1X+(0.00), P1Y+(-16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(19.02), P6Z+(6.18)}},
|
||||
{{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(16.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(12.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(8.00), P6Z+(0.00)}},
|
||||
{{P1X+(0.00), P1Y+(-6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(4.00), P6Z+(0.00)}},
|
||||
};
|
||||
const int forward_entries[] { 0,10 };
|
||||
const MovementTable forward_table {forward_paths, 20, 20, forward_entries, 2 };
|
||||
|
||||
const Locations rotatex_paths[] {
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.97 + P1Z*-0.26 + 0.00, P1X*0.00 + P1Y*0.26 + P1Z*0.97 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.97 + P2Z*-0.26 + 0.00, P2X*0.00 + P2Y*0.26 + P2Z*0.97 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.97 + P3Z*-0.26 + 0.00, P3X*0.00 + P3Y*0.26 + P3Z*0.97 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.97 + P4Z*-0.26 + 0.00, P4X*0.00 + P4Y*0.26 + P4Z*0.97 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.97 + P5Z*-0.26 + 0.00, P5X*0.00 + P5Y*0.26 + P5Z*0.97 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.97 + P6Z*-0.26 + 0.00, P6X*0.00 + P6Y*0.26 + P6Z*0.97 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + -3.00, P1X*0.00 + P1Y*0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + -3.00, P2X*0.00 + P2Y*0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + -3.00, P3X*0.00 + P3Y*0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + -3.00, P4X*0.00 + P4Y*0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + -3.00, P5X*0.00 + P5Y*0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + -3.00, P6X*0.00 + P6Y*0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.16 + -6.00, P1X*0.00 + P1Y*0.16 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.16 + -6.00, P2X*0.00 + P2Y*0.16 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.16 + -6.00, P3X*0.00 + P3Y*0.16 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.16 + -6.00, P4X*0.00 + P4Y*0.16 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.16 + -6.00, P5X*0.00 + P5Y*0.16 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.16 + -6.00, P6X*0.00 + P6Y*0.16 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.10 + -9.00, P1X*0.00 + P1Y*0.10 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.10 + -9.00, P2X*0.00 + P2Y*0.10 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.10 + -9.00, P3X*0.00 + P3Y*0.10 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.10 + -9.00, P4X*0.00 + P4Y*0.10 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.10 + -9.00, P5X*0.00 + P5Y*0.10 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.10 + -9.00, P6X*0.00 + P6Y*0.10 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + -12.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + -12.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + -12.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + -12.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + -12.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + -12.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.00 + -15.00, P1X*0.00 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.00 + -15.00, P2X*0.00 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.00 + -15.00, P3X*0.00 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.00 + -15.00, P4X*0.00 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.00 + -15.00, P5X*0.00 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.00 + -15.00, P6X*0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.05 + -12.00, P1X*0.00 + P1Y*-0.05 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.05 + -12.00, P2X*0.00 + P2Y*-0.05 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.05 + -12.00, P3X*0.00 + P3Y*-0.05 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.05 + -12.00, P4X*0.00 + P4Y*-0.05 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.05 + -12.00, P5X*0.00 + P5Y*-0.05 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.05 + -12.00, P6X*0.00 + P6Y*-0.05 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.10 + -9.00, P1X*0.00 + P1Y*-0.10 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.10 + -9.00, P2X*0.00 + P2Y*-0.10 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.10 + -9.00, P3X*0.00 + P3Y*-0.10 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.10 + -9.00, P4X*0.00 + P4Y*-0.10 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.10 + -9.00, P5X*0.00 + P5Y*-0.10 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.10 + -9.00, P6X*0.00 + P6Y*-0.10 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.16 + -6.00, P1X*0.00 + P1Y*-0.16 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.16 + -6.00, P2X*0.00 + P2Y*-0.16 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.16 + -6.00, P3X*0.00 + P3Y*-0.16 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.16 + -6.00, P4X*0.00 + P4Y*-0.16 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.16 + -6.00, P5X*0.00 + P5Y*-0.16 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.16 + -6.00, P6X*0.00 + P6Y*-0.16 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + -3.00, P1X*0.00 + P1Y*-0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + -3.00, P2X*0.00 + P2Y*-0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + -3.00, P3X*0.00 + P3Y*-0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + -3.00, P4X*0.00 + P4Y*-0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + -3.00, P5X*0.00 + P5Y*-0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + -3.00, P6X*0.00 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.97 + P1Z*0.26 + 0.00, P1X*0.00 + P1Y*-0.26 + P1Z*0.97 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.97 + P2Z*0.26 + 0.00, P2X*0.00 + P2Y*-0.26 + P2Z*0.97 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.97 + P3Z*0.26 + 0.00, P3X*0.00 + P3Y*-0.26 + P3Z*0.97 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.97 + P4Z*0.26 + 0.00, P4X*0.00 + P4Y*-0.26 + P4Z*0.97 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.97 + P5Z*0.26 + 0.00, P5X*0.00 + P5Y*-0.26 + P5Z*0.97 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.97 + P6Z*0.26 + 0.00, P6X*0.00 + P6Y*-0.26 + P6Z*0.97 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + 3.00, P1X*0.00 + P1Y*-0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + 3.00, P2X*0.00 + P2Y*-0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + 3.00, P3X*0.00 + P3Y*-0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + 3.00, P4X*0.00 + P4Y*-0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + 3.00, P5X*0.00 + P5Y*-0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + 3.00, P6X*0.00 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.16 + 6.00, P1X*0.00 + P1Y*-0.16 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.16 + 6.00, P2X*0.00 + P2Y*-0.16 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.16 + 6.00, P3X*0.00 + P3Y*-0.16 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.16 + 6.00, P4X*0.00 + P4Y*-0.16 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.16 + 6.00, P5X*0.00 + P5Y*-0.16 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.16 + 6.00, P6X*0.00 + P6Y*-0.16 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.10 + 9.00, P1X*0.00 + P1Y*-0.10 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.10 + 9.00, P2X*0.00 + P2Y*-0.10 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.10 + 9.00, P3X*0.00 + P3Y*-0.10 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.10 + 9.00, P4X*0.00 + P4Y*-0.10 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.10 + 9.00, P5X*0.00 + P5Y*-0.10 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.10 + 9.00, P6X*0.00 + P6Y*-0.10 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.05 + 12.00, P1X*0.00 + P1Y*-0.05 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.05 + 12.00, P2X*0.00 + P2Y*-0.05 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.05 + 12.00, P3X*0.00 + P3Y*-0.05 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.05 + 12.00, P4X*0.00 + P4Y*-0.05 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.05 + 12.00, P5X*0.00 + P5Y*-0.05 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.05 + 12.00, P6X*0.00 + P6Y*-0.05 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.00 + 15.00, P1X*0.00 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.00 + 15.00, P2X*0.00 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.00 + 15.00, P3X*0.00 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.00 + 15.00, P4X*0.00 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.00 + 15.00, P5X*0.00 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.00 + 15.00, P6X*0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 12.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + 12.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + 12.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + 12.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + 12.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + 12.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.10 + 9.00, P1X*0.00 + P1Y*0.10 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.10 + 9.00, P2X*0.00 + P2Y*0.10 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.10 + 9.00, P3X*0.00 + P3Y*0.10 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.10 + 9.00, P4X*0.00 + P4Y*0.10 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.10 + 9.00, P5X*0.00 + P5Y*0.10 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.10 + 9.00, P6X*0.00 + P6Y*0.10 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.16 + 6.00, P1X*0.00 + P1Y*0.16 + P1Z*0.99 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.16 + 6.00, P2X*0.00 + P2Y*0.16 + P2Z*0.99 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.16 + 6.00, P3X*0.00 + P3Y*0.16 + P3Z*0.99 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.16 + 6.00, P4X*0.00 + P4Y*0.16 + P4Z*0.99 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.16 + 6.00, P5X*0.00 + P5Y*0.16 + P5Z*0.99 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.16 + 6.00, P6X*0.00 + P6Y*0.16 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + 3.00, P1X*0.00 + P1Y*0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + 3.00, P2X*0.00 + P2Y*0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + 3.00, P3X*0.00 + P3Y*0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + 3.00, P4X*0.00 + P4Y*0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + 3.00, P5X*0.00 + P5Y*0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + 3.00, P6X*0.00 + P6Y*0.21 + P6Z*0.98 + 0.00}},
|
||||
};
|
||||
const int rotatex_entries[] { 0,10 };
|
||||
const MovementTable rotatex_table {rotatex_paths, 20, 50, rotatex_entries, 2 };
|
||||
|
||||
const Locations rotatey_paths[] {
|
||||
{{P1X*0.97 + P1Y*0.00 + P1Z*0.26 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.26 + P1Y*0.00 + P1Z*0.97 + 0.00},
|
||||
{P2X*0.97 + P2Y*0.00 + P2Z*0.26 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.26 + P2Y*0.00 + P2Z*0.97 + 0.00},
|
||||
{P3X*0.97 + P3Y*0.00 + P3Z*0.26 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.26 + P3Y*0.00 + P3Z*0.97 + 0.00},
|
||||
{P4X*0.97 + P4Y*0.00 + P4Z*0.26 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.26 + P4Y*0.00 + P4Z*0.97 + 0.00},
|
||||
{P5X*0.97 + P5Y*0.00 + P5Z*0.26 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.26 + P5Y*0.00 + P5Z*0.97 + 0.00},
|
||||
{P6X*0.97 + P6Y*0.00 + P6Z*0.26 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.26 + P6Y*0.00 + P6Z*0.97 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.00 + P1Z*0.21 + -3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.21 + P1Y*0.00 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.00 + P2Z*0.21 + -3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.21 + P2Y*0.00 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.00 + P3Z*0.21 + -3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.21 + P3Y*0.00 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.00 + P4Z*0.21 + -3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.21 + P4Y*0.00 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.00 + P5Z*0.21 + -3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.21 + P5Y*0.00 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.00 + P6Z*0.21 + -3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*0.16 + -6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.16 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*0.16 + -6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.16 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*0.16 + -6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.16 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*0.16 + -6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.16 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*0.16 + -6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.16 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*0.16 + -6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*0.10 + -9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.10 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*0.10 + -9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.10 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*0.10 + -9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.10 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*0.10 + -9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.10 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*0.10 + -9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.10 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*0.10 + -9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.05 + -12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.05 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.05 + -12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.05 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.05 + -12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.05 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.05 + -12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.05 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.05 + -12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.05 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.05 + -12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + -15.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.00 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + -15.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.00 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + -15.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.00 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + -15.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.00 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + -15.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.00 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + -15.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*-0.05 + -12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.05 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*-0.05 + -12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.05 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*-0.05 + -12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.05 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*-0.05 + -12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.05 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*-0.05 + -12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.05 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*-0.05 + -12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*-0.10 + -9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.10 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*-0.10 + -9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.10 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*-0.10 + -9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.10 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*-0.10 + -9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.10 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*-0.10 + -9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.10 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*-0.10 + -9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*-0.16 + -6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.16 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*-0.16 + -6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.16 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*-0.16 + -6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.16 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*-0.16 + -6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.16 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*-0.16 + -6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.16 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*-0.16 + -6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.00 + P1Z*-0.21 + -3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.21 + P1Y*0.00 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.00 + P2Z*-0.21 + -3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.21 + P2Y*0.00 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.00 + P3Z*-0.21 + -3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.21 + P3Y*0.00 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.00 + P4Z*-0.21 + -3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.21 + P4Y*0.00 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.00 + P5Z*-0.21 + -3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.21 + P5Y*0.00 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.00 + P6Z*-0.21 + -3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.97 + P1Y*0.00 + P1Z*-0.26 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.26 + P1Y*0.00 + P1Z*0.97 + 0.00},
|
||||
{P2X*0.97 + P2Y*0.00 + P2Z*-0.26 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.26 + P2Y*0.00 + P2Z*0.97 + 0.00},
|
||||
{P3X*0.97 + P3Y*0.00 + P3Z*-0.26 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.26 + P3Y*0.00 + P3Z*0.97 + 0.00},
|
||||
{P4X*0.97 + P4Y*0.00 + P4Z*-0.26 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.26 + P4Y*0.00 + P4Z*0.97 + 0.00},
|
||||
{P5X*0.97 + P5Y*0.00 + P5Z*-0.26 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.26 + P5Y*0.00 + P5Z*0.97 + 0.00},
|
||||
{P6X*0.97 + P6Y*0.00 + P6Z*-0.26 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.26 + P6Y*0.00 + P6Z*0.97 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.00 + P1Z*-0.21 + 3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.21 + P1Y*0.00 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.00 + P2Z*-0.21 + 3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.21 + P2Y*0.00 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.00 + P3Z*-0.21 + 3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.21 + P3Y*0.00 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.00 + P4Z*-0.21 + 3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.21 + P4Y*0.00 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.00 + P5Z*-0.21 + 3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.21 + P5Y*0.00 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.00 + P6Z*-0.21 + 3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*-0.16 + 6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.16 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*-0.16 + 6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.16 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*-0.16 + 6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.16 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*-0.16 + 6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.16 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*-0.16 + 6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.16 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*-0.16 + 6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*-0.10 + 9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.10 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*-0.10 + 9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.10 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*-0.10 + 9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.10 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*-0.10 + 9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.10 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*-0.10 + 9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.10 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*-0.10 + 9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*-0.05 + 12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*0.05 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*-0.05 + 12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*0.05 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*-0.05 + 12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*0.05 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*-0.05 + 12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*0.05 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*-0.05 + 12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*0.05 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*-0.05 + 12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 15.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.00 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 15.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.00 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 15.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.00 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 15.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.00 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 15.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.00 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 15.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.00 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.05 + 12.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.05 + P1Y*0.00 + P1Z*1.00 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.05 + 12.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.05 + P2Y*0.00 + P2Z*1.00 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.05 + 12.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.05 + P3Y*0.00 + P3Z*1.00 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.05 + 12.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.05 + P4Y*0.00 + P4Z*1.00 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.05 + 12.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.05 + P5Y*0.00 + P5Z*1.00 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.05 + 12.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.05 + P6Y*0.00 + P6Z*1.00 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*0.10 + 9.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.10 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*0.10 + 9.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.10 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*0.10 + 9.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.10 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*0.10 + 9.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.10 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*0.10 + 9.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.10 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*0.10 + 9.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.10 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.00 + P1Z*0.16 + 6.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.16 + P1Y*0.00 + P1Z*0.99 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.00 + P2Z*0.16 + 6.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.16 + P2Y*0.00 + P2Z*0.99 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.00 + P3Z*0.16 + 6.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.16 + P3Y*0.00 + P3Z*0.99 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.00 + P4Z*0.16 + 6.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.16 + P4Y*0.00 + P4Z*0.99 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.00 + P5Z*0.16 + 6.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.16 + P5Y*0.00 + P5Z*0.99 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.00 + P6Z*0.16 + 6.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.16 + P6Y*0.00 + P6Z*0.99 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.00 + P1Z*0.21 + 3.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.21 + P1Y*0.00 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.00 + P2Z*0.21 + 3.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.21 + P2Y*0.00 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.00 + P3Z*0.21 + 3.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.21 + P3Y*0.00 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.00 + P4Z*0.21 + 3.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.21 + P4Y*0.00 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.00 + P5Z*0.21 + 3.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.21 + P5Y*0.00 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.00 + P6Z*0.21 + 3.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.21 + P6Y*0.00 + P6Z*0.98 + 0.00}},
|
||||
};
|
||||
const int rotatey_entries[] { 0,10 };
|
||||
const MovementTable rotatey_table {rotatey_paths, 20, 50, rotatey_entries, 2 };
|
||||
|
||||
const Locations rotatez_paths[] {
|
||||
{{P1X*0.98 + P1Y*0.00 + P1Z*0.22 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.00 + 0.00, P1X*-0.22 + P1Y*0.00 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.00 + P2Z*0.22 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.00 + 0.00, P2X*-0.22 + P2Y*0.00 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.00 + P3Z*0.22 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.00 + 0.00, P3X*-0.22 + P3Y*0.00 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.00 + P4Z*0.22 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.00 + 0.00, P4X*-0.22 + P4Y*0.00 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.00 + P5Z*0.22 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.00 + 0.00, P5X*-0.22 + P5Y*0.00 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.00 + P6Z*0.22 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.00 + 0.00, P6X*-0.22 + P6Y*0.00 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.01 + P1Z*0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.07 + 0.00, P1X*-0.21 + P1Y*0.07 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.01 + P2Z*0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.07 + 0.00, P2X*-0.21 + P2Y*0.07 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.01 + P3Z*0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.07 + 0.00, P3X*-0.21 + P3Y*0.07 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.01 + P4Z*0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.07 + 0.00, P4X*-0.21 + P4Y*0.07 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.01 + P5Z*0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.07 + 0.00, P5X*-0.21 + P5Y*0.07 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.01 + P6Z*0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.07 + 0.00, P6X*-0.21 + P6Y*0.07 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.02 + P1Z*0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.13 + 0.00, P1X*-0.18 + P1Y*0.13 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.02 + P2Z*0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.13 + 0.00, P2X*-0.18 + P2Y*0.13 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.02 + P3Z*0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.13 + 0.00, P3X*-0.18 + P3Y*0.13 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.02 + P4Z*0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.13 + 0.00, P4X*-0.18 + P4Y*0.13 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.02 + P5Z*0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.13 + 0.00, P5X*-0.18 + P5Y*0.13 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.02 + P6Z*0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.13 + 0.00, P6X*-0.18 + P6Y*0.13 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.02 + P1Z*0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.18 + 0.00, P1X*-0.13 + P1Y*0.18 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.02 + P2Z*0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.18 + 0.00, P2X*-0.13 + P2Y*0.18 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.02 + P3Z*0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.18 + 0.00, P3X*-0.13 + P3Y*0.18 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.02 + P4Z*0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.18 + 0.00, P4X*-0.13 + P4Y*0.18 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.02 + P5Z*0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.18 + 0.00, P5X*-0.13 + P5Y*0.18 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.02 + P6Z*0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.18 + 0.00, P6X*-0.13 + P6Y*0.18 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.01 + P1Z*0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + 0.00, P1X*-0.07 + P1Y*0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.01 + P2Z*0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + 0.00, P2X*-0.07 + P2Y*0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.01 + P3Z*0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + 0.00, P3X*-0.07 + P3Y*0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.01 + P4Z*0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + 0.00, P4X*-0.07 + P4Y*0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.01 + P5Z*0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + 0.00, P5X*-0.07 + P5Y*0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.01 + P6Z*0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + 0.00, P6X*-0.07 + P6Y*0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.22 + 0.00, P1X*-0.00 + P1Y*0.22 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.22 + 0.00, P2X*-0.00 + P2Y*0.22 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.22 + 0.00, P3X*-0.00 + P3Y*0.22 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.22 + 0.00, P4X*-0.00 + P4Y*0.22 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.22 + 0.00, P5X*-0.00 + P5Y*0.22 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.22 + 0.00, P6X*-0.00 + P6Y*0.22 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*-0.01 + P1Z*-0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.21 + 0.00, P1X*0.07 + P1Y*0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*-0.01 + P2Z*-0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.21 + 0.00, P2X*0.07 + P2Y*0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*-0.01 + P3Z*-0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.21 + 0.00, P3X*0.07 + P3Y*0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*-0.01 + P4Z*-0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.21 + 0.00, P4X*0.07 + P4Y*0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*-0.01 + P5Z*-0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.21 + 0.00, P5X*0.07 + P5Y*0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*-0.01 + P6Z*-0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.21 + 0.00, P6X*0.07 + P6Y*0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*-0.02 + P1Z*-0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*-0.18 + 0.00, P1X*0.13 + P1Y*0.18 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.99 + P2Y*-0.02 + P2Z*-0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*-0.18 + 0.00, P2X*0.13 + P2Y*0.18 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.99 + P3Y*-0.02 + P3Z*-0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*-0.18 + 0.00, P3X*0.13 + P3Y*0.18 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.99 + P4Y*-0.02 + P4Z*-0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*-0.18 + 0.00, P4X*0.13 + P4Y*0.18 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.99 + P5Y*-0.02 + P5Z*-0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*-0.18 + 0.00, P5X*0.13 + P5Y*0.18 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.99 + P6Y*-0.02 + P6Z*-0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*-0.18 + 0.00, P6X*0.13 + P6Y*0.18 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*-0.02 + P1Z*-0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*-0.13 + 0.00, P1X*0.18 + P1Y*0.13 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*-0.02 + P2Z*-0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*-0.13 + 0.00, P2X*0.18 + P2Y*0.13 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*-0.02 + P3Z*-0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*-0.13 + 0.00, P3X*0.18 + P3Y*0.13 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*-0.02 + P4Z*-0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*-0.13 + 0.00, P4X*0.18 + P4Y*0.13 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*-0.02 + P5Z*-0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*-0.13 + 0.00, P5X*0.18 + P5Y*0.13 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*-0.02 + P6Z*-0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*-0.13 + 0.00, P6X*0.18 + P6Y*0.13 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*-0.01 + P1Z*-0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.07 + 0.00, P1X*0.21 + P1Y*0.07 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*-0.01 + P2Z*-0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.07 + 0.00, P2X*0.21 + P2Y*0.07 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*-0.01 + P3Z*-0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.07 + 0.00, P3X*0.21 + P3Y*0.07 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*-0.01 + P4Z*-0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.07 + 0.00, P4X*0.21 + P4Y*0.07 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*-0.01 + P5Z*-0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.07 + 0.00, P5X*0.21 + P5Y*0.07 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*-0.01 + P6Z*-0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.07 + 0.00, P6X*0.21 + P6Y*0.07 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*-0.00 + P1Z*-0.22 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.00 + 0.00, P1X*0.22 + P1Y*0.00 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*-0.00 + P2Z*-0.22 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.00 + 0.00, P2X*0.22 + P2Y*0.00 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*-0.00 + P3Z*-0.22 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.00 + 0.00, P3X*0.22 + P3Y*0.00 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*-0.00 + P4Z*-0.22 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.00 + 0.00, P4X*0.22 + P4Y*0.00 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*-0.00 + P5Z*-0.22 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.00 + 0.00, P5X*0.22 + P5Y*0.00 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*-0.00 + P6Z*-0.22 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.00 + 0.00, P6X*0.22 + P6Y*0.00 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.01 + P1Z*-0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.07 + 0.00, P1X*0.21 + P1Y*-0.07 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.01 + P2Z*-0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.07 + 0.00, P2X*0.21 + P2Y*-0.07 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.01 + P3Z*-0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.07 + 0.00, P3X*0.21 + P3Y*-0.07 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.01 + P4Z*-0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.07 + 0.00, P4X*0.21 + P4Y*-0.07 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.01 + P5Z*-0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.07 + 0.00, P5X*0.21 + P5Y*-0.07 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.01 + P6Z*-0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.07 + 0.00, P6X*0.21 + P6Y*-0.07 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*0.02 + P1Z*-0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.13 + 0.00, P1X*0.18 + P1Y*-0.13 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*0.02 + P2Z*-0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.13 + 0.00, P2X*0.18 + P2Y*-0.13 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*0.02 + P3Z*-0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.13 + 0.00, P3X*0.18 + P3Y*-0.13 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*0.02 + P4Z*-0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.13 + 0.00, P4X*0.18 + P4Y*-0.13 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*0.02 + P5Z*-0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.13 + 0.00, P5X*0.18 + P5Y*-0.13 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*0.02 + P6Z*-0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.13 + 0.00, P6X*0.18 + P6Y*-0.13 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*0.02 + P1Z*-0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.18 + 0.00, P1X*0.13 + P1Y*-0.18 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.99 + P2Y*0.02 + P2Z*-0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.18 + 0.00, P2X*0.13 + P2Y*-0.18 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.99 + P3Y*0.02 + P3Z*-0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.18 + 0.00, P3X*0.13 + P3Y*-0.18 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.99 + P4Y*0.02 + P4Z*-0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.18 + 0.00, P4X*0.13 + P4Y*-0.18 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.99 + P5Y*0.02 + P5Z*-0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.18 + 0.00, P5X*0.13 + P5Y*-0.18 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.99 + P6Y*0.02 + P6Z*-0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.18 + 0.00, P6X*0.13 + P6Y*-0.18 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.01 + P1Z*-0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + 0.00, P1X*0.07 + P1Y*-0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.01 + P2Z*-0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + 0.00, P2X*0.07 + P2Y*-0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.01 + P3Z*-0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + 0.00, P3X*0.07 + P3Y*-0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.01 + P4Z*-0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + 0.00, P4X*0.07 + P4Y*-0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.01 + P5Z*-0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + 0.00, P5X*0.07 + P5Y*-0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.01 + P6Z*-0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + 0.00, P6X*0.07 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*0.00 + P1Z*-0.00 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.22 + 0.00, P1X*0.00 + P1Y*-0.22 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*0.00 + P2Z*-0.00 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.22 + 0.00, P2X*0.00 + P2Y*-0.22 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*0.00 + P3Z*-0.00 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.22 + 0.00, P3X*0.00 + P3Y*-0.22 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*0.00 + P4Z*-0.00 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.22 + 0.00, P4X*0.00 + P4Y*-0.22 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*0.00 + P5Z*-0.00 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.22 + 0.00, P5X*0.00 + P5Y*-0.22 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*0.00 + P6Z*-0.00 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.22 + 0.00, P6X*0.00 + P6Y*-0.22 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*1.00 + P1Y*-0.01 + P1Z*0.07 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.21 + 0.00, P1X*-0.07 + P1Y*-0.21 + P1Z*0.98 + 0.00},
|
||||
{P2X*1.00 + P2Y*-0.01 + P2Z*0.07 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.21 + 0.00, P2X*-0.07 + P2Y*-0.21 + P2Z*0.98 + 0.00},
|
||||
{P3X*1.00 + P3Y*-0.01 + P3Z*0.07 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.21 + 0.00, P3X*-0.07 + P3Y*-0.21 + P3Z*0.98 + 0.00},
|
||||
{P4X*1.00 + P4Y*-0.01 + P4Z*0.07 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.21 + 0.00, P4X*-0.07 + P4Y*-0.21 + P4Z*0.98 + 0.00},
|
||||
{P5X*1.00 + P5Y*-0.01 + P5Z*0.07 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.21 + 0.00, P5X*-0.07 + P5Y*-0.21 + P5Z*0.98 + 0.00},
|
||||
{P6X*1.00 + P6Y*-0.01 + P6Z*0.07 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.21 + 0.00, P6X*-0.07 + P6Y*-0.21 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.99 + P1Y*-0.02 + P1Z*0.13 + 0.00, P1X*0.00 + P1Y*0.98 + P1Z*0.18 + 0.00, P1X*-0.13 + P1Y*-0.18 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.99 + P2Y*-0.02 + P2Z*0.13 + 0.00, P2X*0.00 + P2Y*0.98 + P2Z*0.18 + 0.00, P2X*-0.13 + P2Y*-0.18 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.99 + P3Y*-0.02 + P3Z*0.13 + 0.00, P3X*0.00 + P3Y*0.98 + P3Z*0.18 + 0.00, P3X*-0.13 + P3Y*-0.18 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.99 + P4Y*-0.02 + P4Z*0.13 + 0.00, P4X*0.00 + P4Y*0.98 + P4Z*0.18 + 0.00, P4X*-0.13 + P4Y*-0.18 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.99 + P5Y*-0.02 + P5Z*0.13 + 0.00, P5X*0.00 + P5Y*0.98 + P5Z*0.18 + 0.00, P5X*-0.13 + P5Y*-0.18 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.99 + P6Y*-0.02 + P6Z*0.13 + 0.00, P6X*0.00 + P6Y*0.98 + P6Z*0.18 + 0.00, P6X*-0.13 + P6Y*-0.18 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*-0.02 + P1Z*0.18 + 0.00, P1X*0.00 + P1Y*0.99 + P1Z*0.13 + 0.00, P1X*-0.18 + P1Y*-0.13 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*-0.02 + P2Z*0.18 + 0.00, P2X*0.00 + P2Y*0.99 + P2Z*0.13 + 0.00, P2X*-0.18 + P2Y*-0.13 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*-0.02 + P3Z*0.18 + 0.00, P3X*0.00 + P3Y*0.99 + P3Z*0.13 + 0.00, P3X*-0.18 + P3Y*-0.13 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*-0.02 + P4Z*0.18 + 0.00, P4X*0.00 + P4Y*0.99 + P4Z*0.13 + 0.00, P4X*-0.18 + P4Y*-0.13 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*-0.02 + P5Z*0.18 + 0.00, P5X*0.00 + P5Y*0.99 + P5Z*0.13 + 0.00, P5X*-0.18 + P5Y*-0.13 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*-0.02 + P6Z*0.18 + 0.00, P6X*0.00 + P6Y*0.99 + P6Z*0.13 + 0.00, P6X*-0.18 + P6Y*-0.13 + P6Z*0.98 + 0.00}},
|
||||
{{P1X*0.98 + P1Y*-0.01 + P1Z*0.21 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*0.07 + 0.00, P1X*-0.21 + P1Y*-0.07 + P1Z*0.98 + 0.00},
|
||||
{P2X*0.98 + P2Y*-0.01 + P2Z*0.21 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*0.07 + 0.00, P2X*-0.21 + P2Y*-0.07 + P2Z*0.98 + 0.00},
|
||||
{P3X*0.98 + P3Y*-0.01 + P3Z*0.21 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*0.07 + 0.00, P3X*-0.21 + P3Y*-0.07 + P3Z*0.98 + 0.00},
|
||||
{P4X*0.98 + P4Y*-0.01 + P4Z*0.21 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*0.07 + 0.00, P4X*-0.21 + P4Y*-0.07 + P4Z*0.98 + 0.00},
|
||||
{P5X*0.98 + P5Y*-0.01 + P5Z*0.21 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*0.07 + 0.00, P5X*-0.21 + P5Y*-0.07 + P5Z*0.98 + 0.00},
|
||||
{P6X*0.98 + P6Y*-0.01 + P6Z*0.21 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*0.07 + 0.00, P6X*-0.21 + P6Y*-0.07 + P6Z*0.98 + 0.00}},
|
||||
};
|
||||
const int rotatez_entries[] { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19 };
|
||||
const MovementTable rotatez_table {rotatez_paths, 20, 50, rotatez_entries, 20 };
|
||||
|
||||
const Locations shiftleft_paths[] {
|
||||
{{P1X+(-0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-6.18), P1Y+(0.00), P1Z+(19.02)}, {P2X+(4.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-6.18), P3Y+(0.00), P3Z+(19.02)}, {P4X+(4.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-6.18), P5Y+(0.00), P5Z+(19.02)}, {P6X+(4.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-11.76), P1Y+(0.00), P1Z+(16.18)}, {P2X+(8.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-11.76), P3Y+(0.00), P3Z+(16.18)}, {P4X+(8.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-11.76), P5Y+(0.00), P5Z+(16.18)}, {P6X+(8.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-16.18), P1Y+(0.00), P1Z+(11.76)}, {P2X+(12.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-16.18), P3Y+(0.00), P3Z+(11.76)}, {P4X+(12.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-16.18), P5Y+(0.00), P5Z+(11.76)}, {P6X+(12.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-19.02), P1Y+(0.00), P1Z+(6.18)}, {P2X+(16.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-19.02), P3Y+(0.00), P3Z+(6.18)}, {P4X+(16.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-19.02), P5Y+(0.00), P5Z+(6.18)}, {P6X+(16.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-16.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(19.02), P2Y+(-0.00), P2Z+(6.18)}, {P3X+(-16.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(19.02), P4Y+(-0.00), P4Z+(6.18)}, {P5X+(-16.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(19.02), P6Y+(-0.00), P6Z+(6.18)}},
|
||||
{{P1X+(-12.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(16.18), P2Y+(-0.00), P2Z+(11.76)}, {P3X+(-12.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(16.18), P4Y+(-0.00), P4Z+(11.76)}, {P5X+(-12.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(16.18), P6Y+(-0.00), P6Z+(11.76)}},
|
||||
{{P1X+(-8.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(11.76), P2Y+(-0.00), P2Z+(16.18)}, {P3X+(-8.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(11.76), P4Y+(-0.00), P4Z+(16.18)}, {P5X+(-8.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(11.76), P6Y+(-0.00), P6Z+(16.18)}},
|
||||
{{P1X+(-4.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(6.18), P2Y+(-0.00), P2Z+(19.02)}, {P3X+(-4.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(6.18), P4Y+(-0.00), P4Z+(19.02)}, {P5X+(-4.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(6.18), P6Y+(-0.00), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(0.00), P6Z+(20.00)}},
|
||||
{{P1X+(4.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-6.18), P2Y+(0.00), P2Z+(19.02)}, {P3X+(4.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-6.18), P4Y+(0.00), P4Z+(19.02)}, {P5X+(4.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-6.18), P6Y+(0.00), P6Z+(19.02)}},
|
||||
{{P1X+(8.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-11.76), P2Y+(0.00), P2Z+(16.18)}, {P3X+(8.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-11.76), P4Y+(0.00), P4Z+(16.18)}, {P5X+(8.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-11.76), P6Y+(0.00), P6Z+(16.18)}},
|
||||
{{P1X+(12.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-16.18), P2Y+(0.00), P2Z+(11.76)}, {P3X+(12.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-16.18), P4Y+(0.00), P4Z+(11.76)}, {P5X+(12.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-16.18), P6Y+(0.00), P6Z+(11.76)}},
|
||||
{{P1X+(16.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-19.02), P2Y+(0.00), P2Z+(6.18)}, {P3X+(16.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-19.02), P4Y+(0.00), P4Z+(6.18)}, {P5X+(16.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-19.02), P6Y+(0.00), P6Z+(6.18)}},
|
||||
{{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(19.02), P1Y+(-0.00), P1Z+(6.18)}, {P2X+(-16.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(19.02), P3Y+(-0.00), P3Z+(6.18)}, {P4X+(-16.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(19.02), P5Y+(-0.00), P5Z+(6.18)}, {P6X+(-16.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(16.18), P1Y+(-0.00), P1Z+(11.76)}, {P2X+(-12.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(16.18), P3Y+(-0.00), P3Z+(11.76)}, {P4X+(-12.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(16.18), P5Y+(-0.00), P5Z+(11.76)}, {P6X+(-12.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(11.76), P1Y+(-0.00), P1Z+(16.18)}, {P2X+(-8.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(11.76), P3Y+(-0.00), P3Z+(16.18)}, {P4X+(-8.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(11.76), P5Y+(-0.00), P5Z+(16.18)}, {P6X+(-8.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(6.18), P1Y+(-0.00), P1Z+(19.02)}, {P2X+(-4.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(6.18), P3Y+(-0.00), P3Z+(19.02)}, {P4X+(-4.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(6.18), P5Y+(-0.00), P5Z+(19.02)}, {P6X+(-4.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
};
|
||||
const int shiftleft_entries[] { 0,10 };
|
||||
const MovementTable shiftleft_table {shiftleft_paths, 20, 20, shiftleft_entries, 2 };
|
||||
|
||||
const Locations shiftright_paths[] {
|
||||
{{P1X+(0.00), P1Y+(-0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(6.18), P1Y+(-0.00), P1Z+(19.02)}, {P2X+(-4.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(6.18), P3Y+(-0.00), P3Z+(19.02)}, {P4X+(-4.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(6.18), P5Y+(-0.00), P5Z+(19.02)}, {P6X+(-4.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(11.76), P1Y+(-0.00), P1Z+(16.18)}, {P2X+(-8.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(11.76), P3Y+(-0.00), P3Z+(16.18)}, {P4X+(-8.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(11.76), P5Y+(-0.00), P5Z+(16.18)}, {P6X+(-8.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(16.18), P1Y+(-0.00), P1Z+(11.76)}, {P2X+(-12.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(16.18), P3Y+(-0.00), P3Z+(11.76)}, {P4X+(-12.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(16.18), P5Y+(-0.00), P5Z+(11.76)}, {P6X+(-12.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(19.02), P1Y+(-0.00), P1Z+(6.18)}, {P2X+(-16.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(19.02), P3Y+(-0.00), P3Z+(6.18)}, {P4X+(-16.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(19.02), P5Y+(-0.00), P5Z+(6.18)}, {P6X+(-16.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(16.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-19.02), P2Y+(0.00), P2Z+(6.18)}, {P3X+(16.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-19.02), P4Y+(0.00), P4Z+(6.18)}, {P5X+(16.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-19.02), P6Y+(0.00), P6Z+(6.18)}},
|
||||
{{P1X+(12.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-16.18), P2Y+(0.00), P2Z+(11.76)}, {P3X+(12.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-16.18), P4Y+(0.00), P4Z+(11.76)}, {P5X+(12.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-16.18), P6Y+(0.00), P6Z+(11.76)}},
|
||||
{{P1X+(8.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-11.76), P2Y+(0.00), P2Z+(16.18)}, {P3X+(8.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-11.76), P4Y+(0.00), P4Z+(16.18)}, {P5X+(8.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-11.76), P6Y+(0.00), P6Z+(16.18)}},
|
||||
{{P1X+(4.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-6.18), P2Y+(0.00), P2Z+(19.02)}, {P3X+(4.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-6.18), P4Y+(0.00), P4Z+(19.02)}, {P5X+(4.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-6.18), P6Y+(0.00), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-0.00), P6Z+(20.00)}},
|
||||
{{P1X+(-4.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(6.18), P2Y+(-0.00), P2Z+(19.02)}, {P3X+(-4.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(6.18), P4Y+(-0.00), P4Z+(19.02)}, {P5X+(-4.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(6.18), P6Y+(-0.00), P6Z+(19.02)}},
|
||||
{{P1X+(-8.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(11.76), P2Y+(-0.00), P2Z+(16.18)}, {P3X+(-8.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(11.76), P4Y+(-0.00), P4Z+(16.18)}, {P5X+(-8.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(11.76), P6Y+(-0.00), P6Z+(16.18)}},
|
||||
{{P1X+(-12.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(16.18), P2Y+(-0.00), P2Z+(11.76)}, {P3X+(-12.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(16.18), P4Y+(-0.00), P4Z+(11.76)}, {P5X+(-12.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(16.18), P6Y+(-0.00), P6Z+(11.76)}},
|
||||
{{P1X+(-16.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(19.02), P2Y+(-0.00), P2Z+(6.18)}, {P3X+(-16.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(19.02), P4Y+(-0.00), P4Z+(6.18)}, {P5X+(-16.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(19.02), P6Y+(-0.00), P6Z+(6.18)}},
|
||||
{{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-19.02), P1Y+(0.00), P1Z+(6.18)}, {P2X+(16.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-19.02), P3Y+(0.00), P3Z+(6.18)}, {P4X+(16.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-19.02), P5Y+(0.00), P5Z+(6.18)}, {P6X+(16.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-16.18), P1Y+(0.00), P1Z+(11.76)}, {P2X+(12.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-16.18), P3Y+(0.00), P3Z+(11.76)}, {P4X+(12.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-16.18), P5Y+(0.00), P5Z+(11.76)}, {P6X+(12.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-11.76), P1Y+(0.00), P1Z+(16.18)}, {P2X+(8.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-11.76), P3Y+(0.00), P3Z+(16.18)}, {P4X+(8.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-11.76), P5Y+(0.00), P5Z+(16.18)}, {P6X+(8.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-6.18), P1Y+(0.00), P1Z+(19.02)}, {P2X+(4.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-6.18), P3Y+(0.00), P3Z+(19.02)}, {P4X+(4.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-6.18), P5Y+(0.00), P5Z+(19.02)}, {P6X+(4.00), P6Y+(-0.00), P6Z+(0.00)}},
|
||||
};
|
||||
const int shiftright_entries[] { 0,10 };
|
||||
const MovementTable shiftright_table {shiftright_paths, 20, 20, shiftright_entries, 2 };
|
||||
|
||||
const Locations turnleft_paths[] {
|
||||
{{P1X+(-0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(-4.37), P1Y+(4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(4.37), P3Y+(4.37), P3Z+(19.02)}, {P4X+(-2.83), P4Y+(2.83), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(2.83), P6Y+(2.83), P6Z+(0.00)}},
|
||||
{{P1X+(-8.31), P1Y+(8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(8.31), P3Y+(8.31), P3Z+(16.18)}, {P4X+(-5.66), P4Y+(5.66), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(5.66), P6Y+(5.66), P6Z+(0.00)}},
|
||||
{{P1X+(-11.44), P1Y+(11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(11.44), P3Y+(11.44), P3Z+(11.76)}, {P4X+(-8.49), P4Y+(8.49), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(8.49), P6Y+(8.49), P6Z+(0.00)}},
|
||||
{{P1X+(-13.45), P1Y+(13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(13.45), P3Y+(13.45), P3Z+(6.18)}, {P4X+(-11.31), P4Y+(11.31), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(11.31), P6Y+(11.31), P6Z+(0.00)}},
|
||||
{{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
|
||||
{{P1X+(-11.31), P1Y+(11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(11.31), P3Y+(11.31), P3Z+(0.00)}, {P4X+(-13.45), P4Y+(13.45), P4Z+(6.18)}, {P5X+(-0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(13.45), P6Y+(13.45), P6Z+(6.18)}},
|
||||
{{P1X+(-8.49), P1Y+(8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(8.49), P3Y+(8.49), P3Z+(0.00)}, {P4X+(-11.44), P4Y+(11.44), P4Z+(11.76)}, {P5X+(-0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(11.44), P6Y+(11.44), P6Z+(11.76)}},
|
||||
{{P1X+(-5.66), P1Y+(5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(5.66), P3Y+(5.66), P3Z+(0.00)}, {P4X+(-8.31), P4Y+(8.31), P4Z+(16.18)}, {P5X+(-0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(8.31), P6Y+(8.31), P6Z+(16.18)}},
|
||||
{{P1X+(-2.83), P1Y+(2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(2.83), P3Y+(2.83), P3Z+(0.00)}, {P4X+(-4.37), P4Y+(4.37), P4Z+(19.02)}, {P5X+(-0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(4.37), P6Y+(4.37), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-0.00), P6Z+(20.00)}},
|
||||
{{P1X+(2.83), P1Y+(-2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(-2.83), P3Y+(-2.83), P3Z+(0.00)}, {P4X+(4.37), P4Y+(-4.37), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(-4.37), P6Y+(-4.37), P6Z+(19.02)}},
|
||||
{{P1X+(5.66), P1Y+(-5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(-5.66), P3Y+(-5.66), P3Z+(0.00)}, {P4X+(8.31), P4Y+(-8.31), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(-8.31), P6Y+(-8.31), P6Z+(16.18)}},
|
||||
{{P1X+(8.49), P1Y+(-8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(-8.49), P3Y+(-8.49), P3Z+(0.00)}, {P4X+(11.44), P4Y+(-11.44), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(-11.44), P6Y+(-11.44), P6Z+(11.76)}},
|
||||
{{P1X+(11.31), P1Y+(-11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(-11.31), P3Y+(-11.31), P3Z+(0.00)}, {P4X+(13.45), P4Y+(-13.45), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(-13.45), P6Y+(-13.45), P6Z+(6.18)}},
|
||||
{{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
|
||||
{{P1X+(13.45), P1Y+(-13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(-13.45), P3Y+(-13.45), P3Z+(6.18)}, {P4X+(11.31), P4Y+(-11.31), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(-11.31), P6Y+(-11.31), P6Z+(0.00)}},
|
||||
{{P1X+(11.44), P1Y+(-11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(-11.44), P3Y+(-11.44), P3Z+(11.76)}, {P4X+(8.49), P4Y+(-8.49), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(-8.49), P6Y+(-8.49), P6Z+(0.00)}},
|
||||
{{P1X+(8.31), P1Y+(-8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(-8.31), P3Y+(-8.31), P3Z+(16.18)}, {P4X+(5.66), P4Y+(-5.66), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(-5.66), P6Y+(-5.66), P6Z+(0.00)}},
|
||||
{{P1X+(4.37), P1Y+(-4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(-4.37), P3Y+(-4.37), P3Z+(19.02)}, {P4X+(2.83), P4Y+(-2.83), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(-2.83), P6Y+(-2.83), P6Z+(0.00)}},
|
||||
};
|
||||
const int turnleft_entries[] { 0,10 };
|
||||
const MovementTable turnleft_table {turnleft_paths, 20, 20, turnleft_entries, 2 };
|
||||
|
||||
const Locations turnright_paths[] {
|
||||
{{P1X+(0.00), P1Y+(-0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(-0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
|
||||
{{P1X+(4.37), P1Y+(-4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(-4.37), P3Y+(-4.37), P3Z+(19.02)}, {P4X+(2.83), P4Y+(-2.83), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(-2.83), P6Y+(-2.83), P6Z+(0.00)}},
|
||||
{{P1X+(8.31), P1Y+(-8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(-8.31), P3Y+(-8.31), P3Z+(16.18)}, {P4X+(5.66), P4Y+(-5.66), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(-5.66), P6Y+(-5.66), P6Z+(0.00)}},
|
||||
{{P1X+(11.44), P1Y+(-11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(-11.44), P3Y+(-11.44), P3Z+(11.76)}, {P4X+(8.49), P4Y+(-8.49), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(-8.49), P6Y+(-8.49), P6Z+(0.00)}},
|
||||
{{P1X+(13.45), P1Y+(-13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(-13.45), P3Y+(-13.45), P3Z+(6.18)}, {P4X+(11.31), P4Y+(-11.31), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(-11.31), P6Y+(-11.31), P6Z+(0.00)}},
|
||||
{{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
|
||||
{{P1X+(11.31), P1Y+(-11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(-11.31), P3Y+(-11.31), P3Z+(0.00)}, {P4X+(13.45), P4Y+(-13.45), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(-13.45), P6Y+(-13.45), P6Z+(6.18)}},
|
||||
{{P1X+(8.49), P1Y+(-8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(-8.49), P3Y+(-8.49), P3Z+(0.00)}, {P4X+(11.44), P4Y+(-11.44), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(-11.44), P6Y+(-11.44), P6Z+(11.76)}},
|
||||
{{P1X+(5.66), P1Y+(-5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(-5.66), P3Y+(-5.66), P3Z+(0.00)}, {P4X+(8.31), P4Y+(-8.31), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(-8.31), P6Y+(-8.31), P6Z+(16.18)}},
|
||||
{{P1X+(2.83), P1Y+(-2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(-2.83), P3Y+(-2.83), P3Z+(0.00)}, {P4X+(4.37), P4Y+(-4.37), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(-4.37), P6Y+(-4.37), P6Z+(19.02)}},
|
||||
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}},
|
||||
{{P1X+(-2.83), P1Y+(2.83), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(2.83), P3Y+(2.83), P3Z+(0.00)}, {P4X+(-4.37), P4Y+(4.37), P4Z+(19.02)}, {P5X+(-0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(4.37), P6Y+(4.37), P6Z+(19.02)}},
|
||||
{{P1X+(-5.66), P1Y+(5.66), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(5.66), P3Y+(5.66), P3Z+(0.00)}, {P4X+(-8.31), P4Y+(8.31), P4Z+(16.18)}, {P5X+(-0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(8.31), P6Y+(8.31), P6Z+(16.18)}},
|
||||
{{P1X+(-8.49), P1Y+(8.49), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(8.49), P3Y+(8.49), P3Z+(0.00)}, {P4X+(-11.44), P4Y+(11.44), P4Z+(11.76)}, {P5X+(-0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(11.44), P6Y+(11.44), P6Z+(11.76)}},
|
||||
{{P1X+(-11.31), P1Y+(11.31), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(11.31), P3Y+(11.31), P3Z+(0.00)}, {P4X+(-13.45), P4Y+(13.45), P4Z+(6.18)}, {P5X+(-0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(13.45), P6Y+(13.45), P6Z+(6.18)}},
|
||||
{{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
|
||||
{{P1X+(-13.45), P1Y+(13.45), P1Z+(6.18)}, {P2X+(-0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(13.45), P3Y+(13.45), P3Z+(6.18)}, {P4X+(-11.31), P4Y+(11.31), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(11.31), P6Y+(11.31), P6Z+(0.00)}},
|
||||
{{P1X+(-11.44), P1Y+(11.44), P1Z+(11.76)}, {P2X+(-0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(11.44), P3Y+(11.44), P3Z+(11.76)}, {P4X+(-8.49), P4Y+(8.49), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(8.49), P6Y+(8.49), P6Z+(0.00)}},
|
||||
{{P1X+(-8.31), P1Y+(8.31), P1Z+(16.18)}, {P2X+(-0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(8.31), P3Y+(8.31), P3Z+(16.18)}, {P4X+(-5.66), P4Y+(5.66), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(5.66), P6Y+(5.66), P6Z+(0.00)}},
|
||||
{{P1X+(-4.37), P1Y+(4.37), P1Z+(19.02)}, {P2X+(-0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(4.37), P3Y+(4.37), P3Z+(19.02)}, {P4X+(-2.83), P4Y+(2.83), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(2.83), P6Y+(2.83), P6Z+(0.00)}},
|
||||
};
|
||||
const int turnright_entries[] { 0,10 };
|
||||
const MovementTable turnright_table {turnright_paths, 20, 20, turnright_entries, 2 };
|
||||
}
|
||||
|
||||
const MovementTable& backwardTable() {
|
||||
return backward_table;
|
||||
}
|
||||
const MovementTable& forwardTable() {
|
||||
return forward_table;
|
||||
}
|
||||
const MovementTable& rotatexTable() {
|
||||
return rotatex_table;
|
||||
}
|
||||
const MovementTable& rotateyTable() {
|
||||
return rotatey_table;
|
||||
}
|
||||
const MovementTable& rotatezTable() {
|
||||
return rotatez_table;
|
||||
}
|
||||
const MovementTable& shiftleftTable() {
|
||||
return shiftleft_table;
|
||||
}
|
||||
const MovementTable& shiftrightTable() {
|
||||
return shiftright_table;
|
||||
}
|
||||
const MovementTable& turnleftTable() {
|
||||
return turnleft_table;
|
||||
}
|
||||
const MovementTable& turnrightTable() {
|
||||
return turnright_table;
|
||||
}
|
||||
@ -0,0 +1,59 @@
|
||||
#include "movement.h"
|
||||
#include "config.h"
|
||||
|
||||
using namespace hexapod::config;
|
||||
using namespace hexapod;
|
||||
|
||||
#define SIN30 0.5
|
||||
#define COS30 0.866
|
||||
#define SIN45 0.7071
|
||||
#define COS45 0.7071
|
||||
#define SIN15 0.2588
|
||||
#define COS15 0.9659
|
||||
|
||||
#define STANDBY_Z (kLegJoint3ToTip*COS15-kLegJoint2ToJoint3*SIN30)
|
||||
#define LEFTRIGHT_X (kLegMountLeftRightX+kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)
|
||||
#define OTHER_X (kLegMountOtherX + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*COS45)
|
||||
#define OTHER_Y (kLegMountOtherY + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*SIN45)
|
||||
|
||||
#define P1X OTHER_X
|
||||
#define P1Y OTHER_Y
|
||||
#define P1Z -STANDBY_Z
|
||||
|
||||
#define P2X LEFTRIGHT_X
|
||||
#define P2Y 0
|
||||
#define P2Z -STANDBY_Z
|
||||
|
||||
#define P3X OTHER_X
|
||||
#define P3Y -OTHER_Y
|
||||
#define P3Z -STANDBY_Z
|
||||
|
||||
#define P4X -OTHER_X
|
||||
#define P4Y -OTHER_Y
|
||||
#define P4Z -STANDBY_Z
|
||||
|
||||
#define P5X -LEFTRIGHT_X
|
||||
#define P5Y 0
|
||||
#define P5Z -STANDBY_Z
|
||||
|
||||
#define P6X -OTHER_X
|
||||
#define P6Y OTHER_Y
|
||||
#define P6Z -STANDBY_Z
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
namespace {
|
||||
const Locations kStandby {
|
||||
{P1X, P1Y, P1Z}, {P2X, P2Y, P2Z}, {P3X, P3Y, P3Z}, {P4X, P4Y, P4Z}, {P5X, P5Y, P5Z}, {P6X, P6Y, P6Z}
|
||||
};
|
||||
const int zero = 0;
|
||||
|
||||
const MovementTable standay_table {&kStandby, 1, 20, &zero, 1};
|
||||
}
|
||||
|
||||
const MovementTable& standbyTable() {
|
||||
return standay_table;
|
||||
}
|
||||
|
||||
#include "movement_table.h"
|
||||
}
|
||||
@ -0,0 +1,117 @@
|
||||
#include "servo.h"
|
||||
#include "debug.h"
|
||||
|
||||
#include "hal/pwm.h"
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
namespace {
|
||||
// if freq = 240, 1 tick = 1000000 / (240*4096) ~= 1us
|
||||
// if freq = 120, 1 tick = 1000000 / (120*4096) ~= 2us
|
||||
const static int kFrequency = 120;
|
||||
const static int kTickUs = 2;
|
||||
const static int kServoMiddle = 1400;
|
||||
const static int kServoMax = 2000;
|
||||
const static int kServoMin = 800;
|
||||
const static int kServoRange = kServoMax-kServoMiddle;
|
||||
|
||||
auto pwmLeft = hal::PCA9685(0x41);
|
||||
auto pwmRight = hal::PCA9685(0x40);
|
||||
bool pwmInited = false;
|
||||
|
||||
void initPWM(void) {
|
||||
if (pwmInited)
|
||||
return;
|
||||
|
||||
pwmLeft.begin();
|
||||
pwmLeft.setPWMFreq(kFrequency);
|
||||
pwmRight.begin();
|
||||
pwmRight.setPWMFreq(kFrequency);
|
||||
pwmInited = true;
|
||||
}
|
||||
|
||||
int hexapod2pwm(int legIndex, int partIndex) {
|
||||
switch(legIndex) {
|
||||
case 0: return 5 + partIndex;
|
||||
case 1: return 2 + partIndex;
|
||||
case 2: return 8 + partIndex;
|
||||
case 3: return 16 + 8 + partIndex;
|
||||
case 4: return 16 + 2 + partIndex;
|
||||
case 5: return 16 + 5 + partIndex;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
int pwm2hexapod(int pwm) {
|
||||
if(pwm >= 16+8)
|
||||
return 9 + pwm - (16+8);
|
||||
if(pwm >= 16+5)
|
||||
return 15 + pwm - (16+5);
|
||||
if(pwm >= 16+2)
|
||||
return 12 + pwm - (16+2);
|
||||
if(pwm >= 8)
|
||||
return 6 + pwm - (8);
|
||||
if(pwm >= 5)
|
||||
return 0 + pwm - (5);
|
||||
if(pwm >= 2)
|
||||
return 3 + pwm - (2);
|
||||
return -1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Servo::init(void) {
|
||||
initPWM();
|
||||
}
|
||||
|
||||
Servo::Servo(int legIndex, int partIndex) {
|
||||
pwmIndex_ = hexapod2pwm(legIndex, partIndex);
|
||||
inverse_ = partIndex == 1 ? true : false;
|
||||
adjust_angle_ = partIndex == 1 ? 15.0 : 0.0;
|
||||
range_ = partIndex == 0 ? 45 : 60;
|
||||
angle_ = 0;
|
||||
}
|
||||
|
||||
void Servo::setAngle(float angle) {
|
||||
initPWM();
|
||||
|
||||
if (angle > range_ + adjust_angle_) {
|
||||
LOG_INFO("exceed[%d][%f]", pwm2hexapod(pwmIndex_), angle);
|
||||
angle = range_;
|
||||
}
|
||||
else if(angle < -range_ + adjust_angle_) {
|
||||
LOG_INFO("exceed[%d][%f]", pwm2hexapod(pwmIndex_), angle);
|
||||
angle = -range_;
|
||||
}
|
||||
|
||||
angle_ = angle;
|
||||
|
||||
hal::PCA9685* pwm;
|
||||
int idx;
|
||||
if (pwmIndex_ < 16) {
|
||||
pwm = &pwmRight;
|
||||
idx = pwmIndex_;
|
||||
}
|
||||
else {
|
||||
pwm = &pwmLeft;
|
||||
idx = pwmIndex_ - 16;
|
||||
}
|
||||
angle -= adjust_angle_;
|
||||
if (inverse_)
|
||||
angle = -angle;
|
||||
|
||||
int us = kServoMiddle + offset_ + angle*(kServoRange/60)*(1+0.01*scale_);
|
||||
if (us > kServoMax)
|
||||
us = kServoMax;
|
||||
else if(us < kServoMin)
|
||||
us = kServoMin;
|
||||
|
||||
pwm->setPWM(idx, 0, us/kTickUs);
|
||||
LOG_DEBUG("setAngle(%.2f, %d)", angle, us);
|
||||
}
|
||||
|
||||
float Servo::getAngle(void) {
|
||||
return angle_;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
namespace hexapod {
|
||||
|
||||
class Servo {
|
||||
public:
|
||||
static void init(void);
|
||||
|
||||
public:
|
||||
Servo(int legIndex, int partIndex);
|
||||
|
||||
// angle: 0 means center, range is -60~60
|
||||
void setAngle(float angle);
|
||||
float getAngle(void);
|
||||
|
||||
void getParameter(int& offset, int& scale) {
|
||||
offset = offset_;
|
||||
scale = scale_;
|
||||
}
|
||||
|
||||
void setParameter(int offset, int scale, bool update = true) {
|
||||
offset_ = offset;
|
||||
scale_ = scale;
|
||||
if (update)
|
||||
setAngle(angle_);
|
||||
}
|
||||
|
||||
private:
|
||||
float angle_;
|
||||
int pwmIndex_;
|
||||
bool inverse_;
|
||||
int offset_;
|
||||
int scale_;
|
||||
int range_;
|
||||
float adjust_angle_;
|
||||
};
|
||||
|
||||
}
|
||||
@ -0,0 +1,83 @@
|
||||
#include <Arduino.h>
|
||||
#include "ui_controls.h"
|
||||
|
||||
namespace {
|
||||
const static int kRepeatPeriod1 = 500;
|
||||
const static int kRepeatPeriod1Count = 4;
|
||||
const static int kRepeatPeriod2 = 100;
|
||||
}
|
||||
|
||||
void ButtonGroup::addControl(Button &button) {
|
||||
// prevent duplicated entry
|
||||
for (const auto control : controls_) {
|
||||
if (control == &button) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
controls_.push_back(&button);
|
||||
};
|
||||
|
||||
void ButtonGroup::addControl(Button *button) {
|
||||
// prevent duplicated entry
|
||||
for (const auto control : controls_) {
|
||||
if (control == button) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
controls_.push_back(button);
|
||||
};
|
||||
|
||||
void ButtonGroup::addControls(std::vector<Button *> lists) {
|
||||
for (auto button : lists) {
|
||||
addControl(button);
|
||||
}
|
||||
}
|
||||
|
||||
void ButtonGroup::process() {
|
||||
if (!callback_)
|
||||
return;
|
||||
|
||||
if (pressed_) {
|
||||
if (pressed_->isValueChanged() && pressed_->getValue() == 0) {
|
||||
callback_(pressed_->_id, UP);
|
||||
pressed_ = nullptr;
|
||||
}
|
||||
else if (millis() >= time_repeat_) {
|
||||
callback_(pressed_->_id, REPEAT);
|
||||
count_repeat_ += 1;
|
||||
if (count_repeat_ < kRepeatPeriod1Count)
|
||||
time_repeat_ = millis() + kRepeatPeriod1;
|
||||
else
|
||||
time_repeat_ = millis() + kRepeatPeriod2;
|
||||
}
|
||||
}
|
||||
else {
|
||||
for (auto control : controls_) {
|
||||
if (control->isValueChanged() && control->getValue() == 1) {
|
||||
// only 1 button can be pressed at the same time within the same group
|
||||
pressed_ = control;
|
||||
time_repeat_ = millis() + kRepeatPeriod1;
|
||||
count_repeat_ = 0;
|
||||
callback_(control->_id, DOWN);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int ButtonGroup::getPressFlag() {
|
||||
int flag = 0;
|
||||
for (auto control : controls_) {
|
||||
if (control->getValue() == 1) {
|
||||
if (control->_id < 32)
|
||||
flag |= (1<<control->_id);
|
||||
}
|
||||
}
|
||||
return flag;
|
||||
}
|
||||
|
||||
void LRemote_addControls(std::vector<LRemoteUIControl *> lists) {
|
||||
for (auto control : lists) {
|
||||
LRemote.addControl(*control);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef __LREMOTE_H__
|
||||
#include <LRemote.h>
|
||||
#define __LREMOTE_H__
|
||||
#endif
|
||||
|
||||
class Label : public LRemoteLabel {
|
||||
public:
|
||||
Label(const String &text, uint8_t x, uint8_t y, uint8_t w, uint8_t h, RCColorType color = RC_GREY) : LRemoteLabel() {
|
||||
setText(text);
|
||||
setPos(x, y);
|
||||
setSize(w, h);
|
||||
setColor(color);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class Button : public LRemoteButton {
|
||||
public:
|
||||
Button(int id, const String &text, uint8_t x, uint8_t y, uint8_t w, uint8_t h, RCColorType color = RC_GREY) : LRemoteButton() {
|
||||
setText(text);
|
||||
setPos(x, y);
|
||||
setSize(w, h);
|
||||
setColor(color);
|
||||
_id = id;
|
||||
}
|
||||
|
||||
public:
|
||||
int _id;
|
||||
};
|
||||
|
||||
#include <vector>
|
||||
#include <functional>
|
||||
|
||||
class ButtonGroup {
|
||||
public:
|
||||
|
||||
enum mode {
|
||||
NONE = 0,
|
||||
DOWN,
|
||||
UP,
|
||||
REPEAT
|
||||
};
|
||||
|
||||
void config(std::function<void(int, mode)> callback) {
|
||||
callback_ = callback;
|
||||
};
|
||||
|
||||
void addControl(Button &button);
|
||||
void addControl(Button *button);
|
||||
void addControls(std::vector<Button *> lists);
|
||||
void process();
|
||||
|
||||
int getPressFlag();
|
||||
|
||||
public:
|
||||
std::vector<Button *> controls_;
|
||||
std::function<void(int, mode)> callback_ = nullptr;
|
||||
Button *pressed_ = nullptr;
|
||||
int time_repeat_ = 0;
|
||||
int count_repeat_ = 0;
|
||||
};
|
||||
|
||||
void LRemote_addControls(std::vector<LRemoteUIControl *> lists);
|
||||
@ -0,0 +1,86 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
#ifndef __LREMOTE_H__
|
||||
#include <LRemote.h>
|
||||
#define __LREMOTE_H__
|
||||
#endif
|
||||
|
||||
#include "linkit_control/ui_controls.h"
|
||||
#include "hexapod/debug.h"
|
||||
#include "hexapod/config.h"
|
||||
#include "hexapod/hexapod.h"
|
||||
|
||||
#define REACT_DELAY hexapod::config::movementInterval
|
||||
|
||||
static Button buttonForward(hexapod::MOVEMENT_FORWARD, "Forward", 1, 1, 1, 1, RC_BLUE);
|
||||
static Button buttonBackward(hexapod::MOVEMENT_BACKWARD, "Backward", 1, 3, 1, 1, RC_BLUE);
|
||||
static Button buttonTL(hexapod::MOVEMENT_TURNLEFT, "TurnLeft", 0, 2, 1, 1, RC_BLUE);
|
||||
static Button buttonTR(hexapod::MOVEMENT_TURNRIGHT, "TurnRight", 2, 2, 1, 1, RC_BLUE);
|
||||
static Button buttonSL(hexapod::MOVEMENT_SHIFTLEFT, "ShiftLeft", 0, 3, 1, 1, RC_YELLOW);
|
||||
static Button buttonSR(hexapod::MOVEMENT_SHIFTRIGHT, "ShiftRight", 2, 3, 1, 1, RC_YELLOW);
|
||||
static Button buttonRotateX(hexapod::MOVEMENT_ROTATEX, "RotateX", 0, 0, 1, 1, RC_YELLOW);
|
||||
static Button buttonRotateY(hexapod::MOVEMENT_ROTATEY, "RotateY", 1, 0, 1, 1, RC_YELLOW);
|
||||
static Button buttonRotateZ(hexapod::MOVEMENT_ROTATEZ, "RotateZ", 2, 0, 1, 1, RC_YELLOW);
|
||||
|
||||
static ButtonGroup btnGroup;
|
||||
|
||||
static void log_output(const char* log) {
|
||||
Serial.println(log);
|
||||
}
|
||||
|
||||
void normal_setup(void) {
|
||||
LRemote.setName("Hexapod");
|
||||
LRemote.setOrientation(RC_PORTRAIT);
|
||||
LRemote.setGrid(3, 5);
|
||||
|
||||
LRemote_addControls({
|
||||
&buttonForward, &buttonBackward,
|
||||
&buttonTL, &buttonTR,
|
||||
&buttonSL, &buttonSR,
|
||||
&buttonRotateX, &buttonRotateY, &buttonRotateZ,
|
||||
});
|
||||
btnGroup.addControls({
|
||||
&buttonForward, &buttonBackward,
|
||||
&buttonTL, &buttonTR,
|
||||
&buttonSL, &buttonSR,
|
||||
&buttonRotateX, &buttonRotateY, &buttonRotateZ,
|
||||
});
|
||||
|
||||
LRemote.begin();
|
||||
}
|
||||
|
||||
void normal_loop(void) {
|
||||
|
||||
// check if we are connect by some
|
||||
// BLE central device, e.g. an mobile app
|
||||
if(!LRemote.connected()) {
|
||||
delay(1000-REACT_DELAY);
|
||||
}
|
||||
|
||||
auto t0 = millis();
|
||||
|
||||
// Process the incoming BLE write request
|
||||
// and translate them to control events
|
||||
LRemote.process();
|
||||
|
||||
auto flag = btnGroup.getPressFlag();
|
||||
auto mode = hexapod::MOVEMENT_STANDBY;
|
||||
for (auto m = hexapod::MOVEMENT_STANDBY; m < hexapod::MOVEMENT_TOTAL; m++) {
|
||||
if (flag & (1<<m)) {
|
||||
mode = m;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
hexapod::Hexapod.processMovement(mode, REACT_DELAY);
|
||||
|
||||
auto spent = millis() - t0;
|
||||
|
||||
if(spent < REACT_DELAY) {
|
||||
// Serial.println(spent);
|
||||
delay(REACT_DELAY-spent);
|
||||
}
|
||||
else {
|
||||
Serial.println(spent);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,5 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
void normal_setup(void);
|
||||
void normal_loop(void);
|
||||
@ -0,0 +1,220 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
#ifndef __LREMOTE_H__
|
||||
#include <LRemote.h>
|
||||
#define __LREMOTE_H__
|
||||
#endif
|
||||
|
||||
#include "linkit_control/ui_controls.h"
|
||||
#include "hexapod/debug.h"
|
||||
#include "hexapod/hexapod.h"
|
||||
|
||||
#define WIDTH 9
|
||||
#define HEIGHT 18
|
||||
#define L_W WIDTH
|
||||
#define L_H 1
|
||||
#define B_W 2
|
||||
#define B_H 2
|
||||
#define T_W 5
|
||||
#define T_H 2
|
||||
|
||||
#define L_X(g) 0
|
||||
#define L_Y(g) (L_H+T_H)*g
|
||||
#define Bl_X(g) 0
|
||||
#define Bl_Y(g) (L_H+T_H)*g+L_H
|
||||
#define T_X(g) B_W
|
||||
#define T_Y(g) (L_H+T_H)*g+L_H
|
||||
#define Br_X(g) B_W+T_W
|
||||
#define Br_Y(g) (L_H+T_H)*g+L_H
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
// UI Group: Adjust setting
|
||||
|
||||
static Label label1("Select leg", L_X(0), L_Y(0), L_W, L_H);
|
||||
static Label text1("", T_X(0), T_Y(0), T_W, T_H, RC_PINK);
|
||||
static Button button1l(10, "<", Bl_X(0), Bl_Y(0), B_W, B_H, RC_BLUE);
|
||||
static Button button1r(11, ">", Br_X(0), Br_Y(0), B_W, B_H, RC_BLUE);
|
||||
|
||||
static Label label2("Select joint", L_X(1), L_Y(1), L_W, L_H);
|
||||
static Label text2("", T_X(1), T_Y(1), T_W, T_H, RC_PINK);
|
||||
static Button button2l(20, "<", Bl_X(1), Bl_Y(1), B_W, B_H, RC_BLUE);
|
||||
static Button button2r(21, ">", Br_X(1), Br_Y(1), B_W, B_H, RC_BLUE);
|
||||
|
||||
static Label label3("Servo offset", L_X(2), L_Y(2), L_W, L_H);
|
||||
static Label text3("0", T_X(2), T_Y(2), T_W, T_H, RC_PINK);
|
||||
static Button button3l(30, "-", Bl_X(2), Bl_Y(2), B_W, B_H, RC_BLUE);
|
||||
static Button button3r(31, "+", Br_X(2), Br_Y(2), B_W, B_H, RC_BLUE);
|
||||
|
||||
static Label label4("Servo scale", L_X(3), L_Y(3), L_W, L_H);
|
||||
static Label text4("0", T_X(3), T_Y(3), T_W, T_H, RC_PINK);
|
||||
static Button button4l(40, "-", Bl_X(3), Bl_Y(3), B_W, B_H, RC_BLUE);
|
||||
static Button button4r(41, "+", Br_X(3), Br_Y(3), B_W, B_H, RC_BLUE);
|
||||
|
||||
static Button buttonSave(60, "Save", 0, Bl_Y(5), WIDTH, B_H, RC_GREEN);
|
||||
|
||||
static ButtonGroup btnGroup;
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
// UI Group: Test
|
||||
|
||||
static Label label5("Servo test", L_X(4), L_Y(4), L_W, L_H);
|
||||
static Button button5l(-45, "-45°", 0, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
|
||||
static Button button5c(0, "0°", WIDTH/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
|
||||
static Button button5r(45, "+45°", WIDTH*2/3, Bl_Y(4), WIDTH/3, B_H, RC_YELLOW);
|
||||
|
||||
static ButtonGroup btnGroupTest;
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
|
||||
const static String _leg_table[] = {
|
||||
"front right",
|
||||
"right",
|
||||
"rear right",
|
||||
"rear left",
|
||||
"left",
|
||||
"front left",
|
||||
};
|
||||
|
||||
const static String _joint_table[] = {
|
||||
"body",
|
||||
"thigh",
|
||||
"foot",
|
||||
};
|
||||
|
||||
static int _cur_leg = 0;
|
||||
static int _cur_joint = 0;
|
||||
static int _cur_offset = 0;
|
||||
static int _cur_scale = 0;
|
||||
static int _test_angle[3*6] = {0};
|
||||
|
||||
void update_text(void) {
|
||||
text1.updateText(_leg_table[_cur_leg]);
|
||||
text2.updateText(_joint_table[_cur_joint]);
|
||||
text3.updateText(String(_cur_offset));
|
||||
text4.updateText(String(1+0.01*_cur_scale));
|
||||
}
|
||||
|
||||
static uint8_t led_blink_state = 0;
|
||||
|
||||
void setting_setup(void) {
|
||||
|
||||
digitalWrite(LED_BUILTIN, HIGH);
|
||||
|
||||
//
|
||||
// initialize config mode UI
|
||||
//
|
||||
LRemote.setName("Hexapod (S)");
|
||||
LRemote.setOrientation(RC_PORTRAIT);
|
||||
LRemote.setGrid(WIDTH, HEIGHT);
|
||||
|
||||
LRemote_addControls({
|
||||
&label1, &text1, &button1l, &button1r, // row1
|
||||
&label2, &text2, &button2l, &button2r, // row2
|
||||
&label3, &text3, &button3l, &button3r, // row3
|
||||
&label4, &text4, &button4l, &button4r, // row4
|
||||
&label5, &button5l, &button5c, &button5r, // row5
|
||||
&buttonSave // row6
|
||||
});
|
||||
|
||||
//
|
||||
// Button group
|
||||
//
|
||||
btnGroup.addControls({
|
||||
&button1l, &button1r,
|
||||
&button2l, &button2r,
|
||||
&button3l, &button3r,
|
||||
&button4l, &button4r,
|
||||
&buttonSave
|
||||
});
|
||||
|
||||
btnGroup.config([](int btn_id, ButtonGroup::mode mode) {
|
||||
if (mode != ButtonGroup::mode::DOWN && mode != ButtonGroup::mode::REPEAT)
|
||||
return;
|
||||
|
||||
Serial.print("button id: ");
|
||||
Serial.print(String(btn_id));
|
||||
Serial.println(" is triggered");
|
||||
|
||||
int g = btn_id / 10;
|
||||
int lr = btn_id % 10;
|
||||
|
||||
switch(g) {
|
||||
case 1:
|
||||
_cur_leg = (_cur_leg + 6 + (lr ? 1 : -1)) % 6;
|
||||
hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
||||
hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
|
||||
break;
|
||||
case 2:
|
||||
_cur_joint = (_cur_joint + 3 + (lr ? 1 : -1)) % 3;
|
||||
hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
||||
hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
|
||||
break;
|
||||
case 3:
|
||||
_cur_offset += (lr ? 1 : -1);
|
||||
hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
||||
break;
|
||||
case 4:
|
||||
_cur_scale += (lr ? 1 : -1);
|
||||
hexapod::Hexapod.calibrationSet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
||||
break;
|
||||
case 6:
|
||||
hexapod::Hexapod.calibrationSave();
|
||||
}
|
||||
update_text();
|
||||
});
|
||||
|
||||
//
|
||||
// Button group: Testing
|
||||
//
|
||||
btnGroupTest.addControls({
|
||||
&button5l, &button5c, &button5r
|
||||
});
|
||||
|
||||
btnGroupTest.config([](int btn_id, ButtonGroup::mode mode) {
|
||||
if (mode != ButtonGroup::mode::DOWN)
|
||||
return;
|
||||
_test_angle[_cur_leg*3+_cur_joint] = btn_id;
|
||||
hexapod::Hexapod.calibrationTest(_cur_leg, _cur_joint, _test_angle[_cur_leg*3+_cur_joint]);
|
||||
delay(20);
|
||||
});
|
||||
|
||||
//
|
||||
// Ready to go
|
||||
//
|
||||
hexapod::Hexapod.calibrationGet(_cur_leg, _cur_joint, _cur_offset, _cur_scale);
|
||||
update_text();
|
||||
|
||||
for(auto i=0;i<6;i++) {
|
||||
for(auto j=0;j<3;j++) {
|
||||
hexapod::Hexapod.calibrationTest(i, j, _test_angle[i*3+j]);
|
||||
}
|
||||
}
|
||||
|
||||
LRemote.begin();
|
||||
}
|
||||
|
||||
void setting_loop(void) {
|
||||
|
||||
// check if we are connect by some
|
||||
// BLE central device, e.g. an mobile app
|
||||
if(!LRemote.connected()) {
|
||||
delay(500);
|
||||
led_blink_state = !led_blink_state;
|
||||
} else {
|
||||
// The interval between button down/up
|
||||
// can be very short - e.g. a quick tap
|
||||
// on the screen.
|
||||
// We could lose some event if we
|
||||
// delay something like 100ms.
|
||||
delay(15);
|
||||
led_blink_state = 1;
|
||||
}
|
||||
digitalWrite(LED_BUILTIN, led_blink_state ? HIGH : LOW);
|
||||
|
||||
// Process the incoming BLE write request
|
||||
// and translate them to control events
|
||||
LRemote.process();
|
||||
|
||||
btnGroup.process();
|
||||
btnGroupTest.process();
|
||||
}
|
||||
@ -0,0 +1,5 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
void setting_setup(void);
|
||||
void setting_loop(void);
|
||||
@ -0,0 +1,95 @@
|
||||
import argparse
|
||||
import logging
|
||||
import os
|
||||
import sys
|
||||
|
||||
def collectPath(sub_folder):
|
||||
scripts = {}
|
||||
for script_name in [f[:-3] for f in sorted(os.listdir(sub_folder)) if f.endswith('.py') and os.path.isfile(os.path.join(sub_folder, f))]:
|
||||
module = __import__(script_name)
|
||||
if not hasattr(module, 'path_generator'):
|
||||
continue
|
||||
scripts[script_name] = module.path_generator
|
||||
|
||||
return scripts
|
||||
|
||||
def show_detail(path, result):
|
||||
print("path:{}:".format(path))
|
||||
for i, p in enumerate(result):
|
||||
print("{:2d} {:5.2f}, {:5.2f}, {:5.2f}".format(i, p[0], p[1], p[2]))
|
||||
|
||||
def generate_c_body(path, params):
|
||||
data, mode, dur, entries = params
|
||||
result = "\nconst Locations {}_paths[] {{\n".format(path)
|
||||
|
||||
if mode == "shift":
|
||||
# data: float[6][N][3]
|
||||
|
||||
assert(len(data) == 6)
|
||||
|
||||
count = len(data[0])
|
||||
for i in range(count):
|
||||
result += " {" + ", ".join(
|
||||
"{{P{idx}X+({x:.2f}), P{idx}Y+({y:.2f}), P{idx}Z+({z:.2f})}}".format(x=data[j][i][0], y=data[j][i][1], z=data[j][i][2], idx=j+1)
|
||||
for j in range(6)
|
||||
) + "},\n"
|
||||
|
||||
elif mode == "matrix":
|
||||
# data: np.matrix[N]
|
||||
|
||||
count = len(data)
|
||||
for i in range(count):
|
||||
result += " {" + ", \n ".join(
|
||||
"{{P{idx}X*{e00:.2f} + P{idx}Y*{e01:.2f} + P{idx}Z*{e02:.2f} + {e03:.2f}, P{idx}X*{e10:.2f} + P{idx}Y*{e11:.2f} + P{idx}Z*{e12:.2f} + {e13:.2f}, P{idx}X*{e20:.2f} + P{idx}Y*{e21:.2f} + P{idx}Z*{e22:.2f} + {e23:.2f}}}".format(
|
||||
e00=data[i].item((0,0)), e01=data[i].item((0,1)), e02=data[i].item((0,2)), e03=data[i].item((0,3)),
|
||||
e10=data[i].item((1,0)), e11=data[i].item((1,1)), e12=data[i].item((1,2)), e13=data[i].item((1,3)),
|
||||
e20=data[i].item((2,0)), e21=data[i].item((2,1)), e22=data[i].item((2,2)), e23=data[i].item((2,3)),
|
||||
idx=j+1)
|
||||
for j in range(6)
|
||||
) + "},\n"
|
||||
|
||||
else:
|
||||
raise RuntimeError("Generation mode: {} not supported".format(mode))
|
||||
|
||||
result += "};\n"
|
||||
result += "const int {}_entries[] {{ {} }};\n".format(path, ",".join(str(e) for e in entries))
|
||||
result += "const MovementTable {name}_table {{{name}_paths, {count}, {dur}, {name}_entries, {ecount} }};".format(name=path, count=count, dur=dur, ecount=len(entries))
|
||||
return result
|
||||
|
||||
def generate_c_def(path):
|
||||
return """const MovementTable& {name}Table() {{
|
||||
return {name}_table;
|
||||
}}""".format(name=path)
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='pathTool: generate Hexapod path')
|
||||
parser.add_argument('--pathDir', metavar='DIR', dest='path_dir', default='path',
|
||||
help='path script directory (default: {})'.format('path'))
|
||||
parser.add_argument('--outPath', metavar='PATH', dest='out_path', default='output/movement_table.h',
|
||||
help='path script directory (default: {})'.format('output/movement_table.h'))
|
||||
args = parser.parse_args()
|
||||
|
||||
sys.path.insert(0, args.path_dir)
|
||||
|
||||
# find available path generator
|
||||
paths = collectPath(args.path_dir)
|
||||
|
||||
# generate all paths
|
||||
results = {path: generator() for path, generator in paths.items()}
|
||||
|
||||
# output results
|
||||
with open(args.out_path, "w") as f:
|
||||
print("//", file=f)
|
||||
print("// This file is generated, dont directly modify content...", file=f)
|
||||
print("//", file=f)
|
||||
print("namespace {", file=f)
|
||||
for path, data in results.items():
|
||||
print(generate_c_body(path, data), file=f)
|
||||
print("}\n", file=f)
|
||||
for path in results:
|
||||
print(generate_c_def(path), file=f)
|
||||
|
||||
print("Result written to {}".format(args.out_path))
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1 @@
|
||||
../../../hexapod7697/src/hexapod/movement_table.h
|
||||
@ -0,0 +1,15 @@
|
||||
from collections import deque
|
||||
|
||||
from lib import semicircle_generator
|
||||
from forward import g_steps, g_radius
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps, reverse=True)
|
||||
|
||||
mir_path = deque(path)
|
||||
mir_path.rotate(halfsteps)
|
||||
|
||||
return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
|
||||
@ -0,0 +1,19 @@
|
||||
|
||||
from collections import deque
|
||||
|
||||
from lib import semicircle_generator
|
||||
|
||||
g_steps = 20
|
||||
g_radius = 20
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
|
||||
mir_path = deque(path)
|
||||
mir_path.rotate(halfsteps)
|
||||
|
||||
return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
|
||||
|
||||
@ -0,0 +1,85 @@
|
||||
from collections import deque
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
pi = math.acos(-1)
|
||||
|
||||
def semicircle_generator(radius, steps, reverse=False):
|
||||
assert (steps % 4) == 0
|
||||
halfsteps = int(steps/2)
|
||||
|
||||
step_angle = pi / halfsteps
|
||||
|
||||
result = []
|
||||
|
||||
# first half, move backward (only y change)
|
||||
for i in range(halfsteps):
|
||||
result.append((0, radius - i*radius*2/(halfsteps), 0))
|
||||
|
||||
# second half, move forward in semicircle shape (y, z change)
|
||||
for i in range(halfsteps):
|
||||
angle = pi - step_angle*i
|
||||
y = radius * math.cos(angle)
|
||||
z = radius * math.sin(angle)
|
||||
result.append((0, y, z))
|
||||
|
||||
result = deque(result)
|
||||
result.rotate(int(steps/4))
|
||||
|
||||
if reverse:
|
||||
result = deque(reversed(result))
|
||||
result.rotate(1)
|
||||
|
||||
return result
|
||||
|
||||
def get_rotate_x_matrix(angle):
|
||||
angle = angle * pi / 180
|
||||
return np.matrix([
|
||||
[1, 0, 0, 0],
|
||||
[0, math.cos(angle), -math.sin(angle), 0],
|
||||
[0, math.sin(angle), math.cos(angle), 0],
|
||||
[0, 0, 0, 1],
|
||||
])
|
||||
|
||||
def get_rotate_y_matrix(angle):
|
||||
angle = angle * pi / 180
|
||||
return np.matrix([
|
||||
[math.cos(angle), 0, math.sin(angle), 0],
|
||||
[0, 1, 0, 0],
|
||||
[-math.sin(angle), 0, math.cos(angle), 0],
|
||||
[0, 0, 0, 1],
|
||||
])
|
||||
|
||||
def get_rotate_z_matrix(angle):
|
||||
angle = angle * pi / 180
|
||||
return np.matrix([
|
||||
[math.cos(angle), -math.sin(angle), 0, 0],
|
||||
[math.sin(angle), math.cos(angle), 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1],
|
||||
])
|
||||
|
||||
def point_rotate_x(pt, angle):
|
||||
ptx = list(pt) + [1]
|
||||
return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
|
||||
|
||||
def point_rotate_y(pt, angle):
|
||||
ptx = list(pt) + [1]
|
||||
return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
|
||||
|
||||
def point_rotate_z(pt, angle):
|
||||
ptx = list(pt) + [1]
|
||||
return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
|
||||
|
||||
def path_rotate_x(path, angle):
|
||||
return [point_rotate_x(p, angle) for p in path]
|
||||
|
||||
def path_rotate_y(path, angle):
|
||||
return [point_rotate_y(p, angle) for p in path]
|
||||
|
||||
def path_rotate_z(path, angle):
|
||||
return [point_rotate_z(p, angle) for p in path]
|
||||
|
||||
if __name__ == '__main__':
|
||||
pt = [0, 1, 0]
|
||||
print(point_rotate_z(pt, 270))
|
||||
@ -0,0 +1,42 @@
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
|
||||
from lib import get_rotate_x_matrix as get_matrix
|
||||
|
||||
g_steps = 20
|
||||
|
||||
swing_angle = 15
|
||||
y_radius = 15
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
quarter = int(g_steps/4)
|
||||
|
||||
pi = math.acos(-1)
|
||||
|
||||
result = []
|
||||
step_angle = swing_angle / quarter
|
||||
step_offset = y_radius / quarter
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(swing_angle - i*step_angle)
|
||||
m[1,3] = -i * step_offset
|
||||
result.append(m)
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(-i*step_angle)
|
||||
m[1,3] = -y_radius + i * step_offset
|
||||
result.append(m)
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(i*step_angle-swing_angle)
|
||||
m[1,3] = i * step_offset
|
||||
result.append(m)
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(i*step_angle)
|
||||
m[1,3] = y_radius-i * step_offset
|
||||
result.append(m)
|
||||
|
||||
return result, "matrix", 50, (0, quarter*2)
|
||||
@ -0,0 +1,42 @@
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
|
||||
from lib import get_rotate_y_matrix as get_matrix
|
||||
|
||||
g_steps = 20
|
||||
|
||||
swing_angle = 15
|
||||
x_radius = 15
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
quarter = int(g_steps/4)
|
||||
|
||||
pi = math.acos(-1)
|
||||
|
||||
result = []
|
||||
step_angle = swing_angle / quarter
|
||||
step_offset = x_radius / quarter
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(swing_angle - i*step_angle)
|
||||
m[0,3] = -i * step_offset
|
||||
result.append(m)
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(-i*step_angle)
|
||||
m[0,3] = -x_radius + i * step_offset
|
||||
result.append(m)
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(i*step_angle-swing_angle)
|
||||
m[0,3] = i * step_offset
|
||||
result.append(m)
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_matrix(i*step_angle)
|
||||
m[0,3] = x_radius-i * step_offset
|
||||
result.append(m)
|
||||
|
||||
return result, "matrix", 50, (0, quarter*2)
|
||||
@ -0,0 +1,25 @@
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
|
||||
from lib import get_rotate_x_matrix, get_rotate_y_matrix
|
||||
|
||||
g_steps = 20
|
||||
|
||||
z_lift = 4.5
|
||||
xy_radius = 1
|
||||
|
||||
def path_generator():
|
||||
|
||||
pi = math.acos(-1)
|
||||
|
||||
result = []
|
||||
step_angle = 2*pi / g_steps
|
||||
for i in range(g_steps):
|
||||
x = xy_radius * math.cos(i*step_angle)
|
||||
y = xy_radius * math.sin(i*step_angle)
|
||||
|
||||
m = get_rotate_y_matrix(math.atan2(x, z_lift)*180/pi) * get_rotate_x_matrix(math.atan2(y, z_lift)*180/pi)
|
||||
result.append(m)
|
||||
|
||||
return result, "matrix", 50, range(g_steps)
|
||||
@ -0,0 +1,20 @@
|
||||
|
||||
from collections import deque
|
||||
|
||||
from lib import semicircle_generator
|
||||
from lib import path_rotate_z
|
||||
|
||||
g_steps = 20
|
||||
g_radius = 20
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
path = path_rotate_z(path, 90) # shift 90 degree to make the path "left" shift
|
||||
|
||||
mir_path = deque(path)
|
||||
mir_path.rotate(halfsteps)
|
||||
|
||||
return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
|
||||
@ -0,0 +1,20 @@
|
||||
|
||||
from collections import deque
|
||||
|
||||
from lib import semicircle_generator
|
||||
from lib import path_rotate_z
|
||||
|
||||
g_steps = 20
|
||||
g_radius = 20
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
path = path_rotate_z(path, 270) # shift 270 degree to make the path "right" shift
|
||||
|
||||
mir_path = deque(path)
|
||||
mir_path.rotate(halfsteps)
|
||||
|
||||
return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
|
||||
@ -0,0 +1,27 @@
|
||||
|
||||
from collections import deque
|
||||
|
||||
from lib import semicircle_generator
|
||||
from lib import path_rotate_z
|
||||
|
||||
g_steps = 20
|
||||
g_radius = 20
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
|
||||
mir_path = deque(path)
|
||||
mir_path.rotate(halfsteps)
|
||||
|
||||
return [
|
||||
path_rotate_z(path, 45),
|
||||
path_rotate_z(mir_path, 0),
|
||||
path_rotate_z(path, 315),
|
||||
path_rotate_z(mir_path, 225),
|
||||
path_rotate_z(path, 180),
|
||||
path_rotate_z(mir_path, 135),
|
||||
], "shift", 20, (0, halfsteps)
|
||||
|
||||
@ -0,0 +1,27 @@
|
||||
|
||||
from collections import deque
|
||||
|
||||
from lib import semicircle_generator
|
||||
from lib import path_rotate_z
|
||||
|
||||
g_steps = 20
|
||||
g_radius = 20
|
||||
|
||||
def path_generator():
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
|
||||
mir_path = deque(path)
|
||||
mir_path.rotate(halfsteps)
|
||||
|
||||
return [
|
||||
path_rotate_z(path, 45+180),
|
||||
path_rotate_z(mir_path, 0+180),
|
||||
path_rotate_z(path, 315+180),
|
||||
path_rotate_z(mir_path, 225+180),
|
||||
path_rotate_z(path, 180+180),
|
||||
path_rotate_z(mir_path, 135+180),
|
||||
], "shift", 20, (0, halfsteps)
|
||||
|
||||
Loading…
Reference in New Issue