shark
/
hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add run/climb movement

Browse Source
master
Smallp Tsai 6 years ago
parent 1989cb6ce3
commit 4efa90fd13
18 changed files with 607 additions and 147 deletions
Show Stats Download Patch File Download Diff File

12
software/hexapod7697/src/hexapod/movement.cpp
Unescape Escape View File

@ -8,26 +8,32 @@ namespace hexapod {
extern const MovementTable& standbyTable();
extern const MovementTable& forwardTable();
extern const MovementTable& forwardfastTable();
extern const MovementTable& backwardTable();
extern const MovementTable& turnleftTable();
extern const MovementTable& turnrightTable();
extern const MovementTable& shiftleftTable();
extern const MovementTable& shiftrightTable();
extern const MovementTable& climbTable();
extern const MovementTable& rotatexTable();
extern const MovementTable& rotateyTable();
extern const MovementTable& rotatezTable();
extern const MovementTable& twistTable();
const MovementTable kTable[MOVEMENT_TOTAL] {
standbyTable(),
forwardTable(),
forwardfastTable(),
backwardTable(),
turnleftTable(),
turnrightTable(),
shiftleftTable(),
shiftrightTable(),
climbTable(),
rotatexTable(),
rotateyTable(),
rotatezTable(),
twistTable(),
};
Movement::Movement(MovementMode mode):
@ -36,6 +42,12 @@ namespace hexapod {
}
void Movement::setMode(MovementMode newMode) {
if (!kTable[newMode].entries) {
LOG_INFO("Error: null movement of mode(%d)!", newMode);
return;
}
mode_ = newMode;
const MovementTable& table = kTable[mode_];

3
software/hexapod7697/src/hexapod/movement.h
Unescape Escape View File

@ -8,14 +8,17 @@ namespace hexapod {
MOVEMENT_STANDBY = 0,
MOVEMENT_FORWARD,
MOVEMENT_FORWARDFAST,
MOVEMENT_BACKWARD,
MOVEMENT_TURNLEFT,
MOVEMENT_TURNRIGHT,
MOVEMENT_SHIFTLEFT,
MOVEMENT_SHIFTRIGHT,
MOVEMENT_CLIMB,
MOVEMENT_ROTATEX,
MOVEMENT_ROTATEY,
MOVEMENT_ROTATEZ,
MOVEMENT_TWIST,
MOVEMENT_TOTAL,
};

424
software/hexapod7697/src/hexapod/movement_table.h
Unescape Escape View File

@ -4,55 +4,105 @@
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)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(5.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(15.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(25.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(23.78), P6Z+(7.73)}},
{{P1X+(0.00), P1Y+(-15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(-15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.23), P6Z+(14.69)}},
{{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(14.69), P6Z+(20.23)}},
{{P1X+(0.00), P1Y+(-5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(-5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(7.73), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}},
{{P1X+(0.00), P1Y+(5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-7.73), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-14.69), P6Z+(20.23)}},
{{P1X+(0.00), P1Y+(15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.23), P6Z+(14.69)}},
{{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-23.78), P6Z+(7.73)}},
{{P1X+(0.00), P1Y+(25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-25.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(-15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(-15.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(-5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(-5.00), P6Z+(0.00)}},
};
const int backward_entries[] { 0,10 };
const MovementTable backward_table {backward_paths, 20, 20, backward_entries, 2 };
const Locations climb_paths[] {
{{P1X+(30.00), P1Y+(0.00), P1Z+(50.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(-30.00)}, {P3X+(30.00), P3Y+(0.00), P3Z+(50.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(-30.00)}, {P5X+(-30.00), P5Y+(0.00), P5Z+(50.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(-30.00)}},
{{P1X+(28.53), P1Y+(6.18), P1Z+(46.08)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(-30.00)}, {P3X+(28.53), P3Y+(6.18), P3Z+(46.08)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(-30.00)}, {P5X+(-28.53), P5Y+(6.18), P5Z+(46.08)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(-30.00)}},
{{P1X+(24.27), P1Y+(11.76), P1Z+(34.72)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(-30.00)}, {P3X+(24.27), P3Y+(11.76), P3Z+(34.72)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(-30.00)}, {P5X+(-24.27), P5Y+(11.76), P5Z+(34.72)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(-30.00)}},
{{P1X+(17.63), P1Y+(16.18), P1Z+(17.02)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(-30.00)}, {P3X+(17.63), P3Y+(16.18), P3Z+(17.02)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(-30.00)}, {P5X+(-17.63), P5Y+(16.18), P5Z+(17.02)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(-30.00)}},
{{P1X+(9.27), P1Y+(19.02), P1Z+(-5.28)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(-30.00)}, {P3X+(9.27), P3Y+(19.02), P3Z+(-5.28)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(-30.00)}, {P5X+(-9.27), P5Y+(19.02), P5Z+(-5.28)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(-30.00)}},
{{P1X+(0.00), P1Y+(20.00), P1Z+(-30.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(-30.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(-30.00)}, {P4X+(-0.00), P4Y+(-20.00), P4Z+(-30.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(-30.00)}, {P6X+(-0.00), P6Y+(-20.00), P6Z+(-30.00)}},
{{P1X+(0.00), P1Y+(16.00), P1Z+(-30.00)}, {P2X+(9.27), P2Y+(-19.02), P2Z+(-5.28)}, {P3X+(0.00), P3Y+(16.00), P3Z+(-30.00)}, {P4X+(-9.27), P4Y+(-19.02), P4Z+(-5.28)}, {P5X+(0.00), P5Y+(16.00), P5Z+(-30.00)}, {P6X+(-9.27), P6Y+(-19.02), P6Z+(-5.28)}},
{{P1X+(0.00), P1Y+(12.00), P1Z+(-30.00)}, {P2X+(17.63), P2Y+(-16.18), P2Z+(17.02)}, {P3X+(0.00), P3Y+(12.00), P3Z+(-30.00)}, {P4X+(-17.63), P4Y+(-16.18), P4Z+(17.02)}, {P5X+(0.00), P5Y+(12.00), P5Z+(-30.00)}, {P6X+(-17.63), P6Y+(-16.18), P6Z+(17.02)}},
{{P1X+(0.00), P1Y+(8.00), P1Z+(-30.00)}, {P2X+(24.27), P2Y+(-11.76), P2Z+(34.72)}, {P3X+(0.00), P3Y+(8.00), P3Z+(-30.00)}, {P4X+(-24.27), P4Y+(-11.76), P4Z+(34.72)}, {P5X+(0.00), P5Y+(8.00), P5Z+(-30.00)}, {P6X+(-24.27), P6Y+(-11.76), P6Z+(34.72)}},
{{P1X+(0.00), P1Y+(4.00), P1Z+(-30.00)}, {P2X+(28.53), P2Y+(-6.18), P2Z+(46.08)}, {P3X+(0.00), P3Y+(4.00), P3Z+(-30.00)}, {P4X+(-28.53), P4Y+(-6.18), P4Z+(46.08)}, {P5X+(0.00), P5Y+(4.00), P5Z+(-30.00)}, {P6X+(-28.53), P6Y+(-6.18), P6Z+(46.08)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(-30.00)}, {P2X+(30.00), P2Y+(0.00), P2Z+(50.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(-30.00)}, {P4X+(-30.00), P4Y+(0.00), P4Z+(50.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(-30.00)}, {P6X+(-30.00), P6Y+(0.00), P6Z+(50.00)}},
{{P1X+(0.00), P1Y+(-4.00), P1Z+(-30.00)}, {P2X+(28.53), P2Y+(6.18), P2Z+(46.08)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(-30.00)}, {P4X+(-28.53), P4Y+(6.18), P4Z+(46.08)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(-30.00)}, {P6X+(-28.53), P6Y+(6.18), P6Z+(46.08)}},
{{P1X+(0.00), P1Y+(-8.00), P1Z+(-30.00)}, {P2X+(24.27), P2Y+(11.76), P2Z+(34.72)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(-30.00)}, {P4X+(-24.27), P4Y+(11.76), P4Z+(34.72)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(-30.00)}, {P6X+(-24.27), P6Y+(11.76), P6Z+(34.72)}},
{{P1X+(0.00), P1Y+(-12.00), P1Z+(-30.00)}, {P2X+(17.63), P2Y+(16.18), P2Z+(17.02)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(-30.00)}, {P4X+(-17.63), P4Y+(16.18), P4Z+(17.02)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(-30.00)}, {P6X+(-17.63), P6Y+(16.18), P6Z+(17.02)}},
{{P1X+(0.00), P1Y+(-16.00), P1Z+(-30.00)}, {P2X+(9.27), P2Y+(19.02), P2Z+(-5.28)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(-30.00)}, {P4X+(-9.27), P4Y+(19.02), P4Z+(-5.28)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(-30.00)}, {P6X+(-9.27), P6Y+(19.02), P6Z+(-5.28)}},
{{P1X+(0.00), P1Y+(-20.00), P1Z+(-30.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(-30.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(-30.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(-30.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(-30.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(-30.00)}},
{{P1X+(9.27), P1Y+(-19.02), P1Z+(-5.28)}, {P2X+(0.00), P2Y+(16.00), P2Z+(-30.00)}, {P3X+(9.27), P3Y+(-19.02), P3Z+(-5.28)}, {P4X+(0.00), P4Y+(16.00), P4Z+(-30.00)}, {P5X+(-9.27), P5Y+(-19.02), P5Z+(-5.28)}, {P6X+(0.00), P6Y+(16.00), P6Z+(-30.00)}},
{{P1X+(17.63), P1Y+(-16.18), P1Z+(17.02)}, {P2X+(0.00), P2Y+(12.00), P2Z+(-30.00)}, {P3X+(17.63), P3Y+(-16.18), P3Z+(17.02)}, {P4X+(0.00), P4Y+(12.00), P4Z+(-30.00)}, {P5X+(-17.63), P5Y+(-16.18), P5Z+(17.02)}, {P6X+(0.00), P6Y+(12.00), P6Z+(-30.00)}},
{{P1X+(24.27), P1Y+(-11.76), P1Z+(34.72)}, {P2X+(0.00), P2Y+(8.00), P2Z+(-30.00)}, {P3X+(24.27), P3Y+(-11.76), P3Z+(34.72)}, {P4X+(0.00), P4Y+(8.00), P4Z+(-30.00)}, {P5X+(-24.27), P5Y+(-11.76), P5Z+(34.72)}, {P6X+(0.00), P6Y+(8.00), P6Z+(-30.00)}},
{{P1X+(28.53), P1Y+(-6.18), P1Z+(46.08)}, {P2X+(0.00), P2Y+(4.00), P2Z+(-30.00)}, {P3X+(28.53), P3Y+(-6.18), P3Z+(46.08)}, {P4X+(0.00), P4Y+(4.00), P4Z+(-30.00)}, {P5X+(-28.53), P5Y+(-6.18), P5Z+(46.08)}, {P6X+(0.00), P6Y+(4.00), P6Z+(-30.00)}},
};
const int climb_entries[] { 0,10 };
const MovementTable climb_table {climb_paths, 20, 30, climb_entries, 2 };
const Locations forward_paths[] {
{{P1X+(0.00), P1Y+(0.00), P1Z+(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)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(-5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(-5.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(-15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(-15.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-25.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-23.78), P6Z+(7.73)}},
{{P1X+(0.00), P1Y+(15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.23), P6Z+(14.69)}},
{{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-14.69), P6Z+(20.23)}},
{{P1X+(0.00), P1Y+(5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-7.73), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}},
{{P1X+(0.00), P1Y+(-5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(-5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(7.73), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(14.69), P6Z+(20.23)}},
{{P1X+(0.00), P1Y+(-15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(-15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.23), P6Z+(14.69)}},
{{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(23.78), P6Z+(7.73)}},
{{P1X+(0.00), P1Y+(-25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(25.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(15.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(-7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(5.00), P6Z+(0.00)}},
};
const int forward_entries[] { 0,10 };
const MovementTable forward_table {forward_paths, 20, 20, forward_entries, 2 };
const Locations forwardfast_paths[] {
{{P1X+(10.00), P1Y+(0.00), P1Z+(30.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(10.00), P3Y+(0.00), P3Z+(30.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(30.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(9.51), P1Y+(15.45), P1Z+(28.53)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(9.51), P3Y+(15.45), P3Z+(28.53)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(-9.51), P5Y+(15.45), P5Z+(28.53)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}},
{{P1X+(8.09), P1Y+(29.39), P1Z+(24.27)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(8.09), P3Y+(29.39), P3Z+(24.27)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(-8.09), P5Y+(29.39), P5Z+(24.27)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}},
{{P1X+(5.88), P1Y+(40.45), P1Z+(17.63)}, {P2X+(0.00), P2Y+(-30.00), P2Z+(0.00)}, {P3X+(5.88), P3Y+(40.45), P3Z+(17.63)}, {P4X+(0.00), P4Y+(-30.00), P4Z+(0.00)}, {P5X+(-5.88), P5Y+(40.45), P5Z+(17.63)}, {P6X+(0.00), P6Y+(-30.00), P6Z+(0.00)}},
{{P1X+(3.09), P1Y+(47.55), P1Z+(9.27)}, {P2X+(0.00), P2Y+(-40.00), P2Z+(0.00)}, {P3X+(3.09), P3Y+(47.55), P3Z+(9.27)}, {P4X+(0.00), P4Y+(-40.00), P4Z+(0.00)}, {P5X+(-3.09), P5Y+(47.55), P5Z+(9.27)}, {P6X+(0.00), P6Y+(-40.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(50.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-50.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(50.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(-50.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(50.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-50.00), P6Z+(0.00)}},
{{P1X+(0.00), P1Y+(40.00), P1Z+(0.00)}, {P2X+(3.09), P2Y+(-47.55), P2Z+(9.27)}, {P3X+(0.00), P3Y+(40.00), P3Z+(0.00)}, {P4X+(-3.09), P4Y+(-47.55), P4Z+(9.27)}, {P5X+(0.00), P5Y+(40.00), P5Z+(0.00)}, {P6X+(-3.09), P6Y+(-47.55), P6Z+(9.27)}},
{{P1X+(0.00), P1Y+(30.00), P1Z+(0.00)}, {P2X+(5.88), P2Y+(-40.45), P2Z+(17.63)}, {P3X+(0.00), P3Y+(30.00), P3Z+(0.00)}, {P4X+(-5.88), P4Y+(-40.45), P4Z+(17.63)}, {P5X+(0.00), P5Y+(30.00), P5Z+(0.00)}, {P6X+(-5.88), P6Y+(-40.45), P6Z+(17.63)}},
{{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(8.09), P2Y+(-29.39), P2Z+(24.27)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(-8.09), P4Y+(-29.39), P4Z+(24.27)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-8.09), P6Y+(-29.39), P6Z+(24.27)}},
{{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(9.51), P2Y+(-15.45), P2Z+(28.53)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(-9.51), P4Y+(-15.45), P4Z+(28.53)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-9.51), P6Y+(-15.45), P6Z+(28.53)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(10.00), P2Y+(0.00), P2Z+(30.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(30.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(30.00)}},
{{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(9.51), P2Y+(15.45), P2Z+(28.53)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(-9.51), P4Y+(15.45), P4Z+(28.53)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(-9.51), P6Y+(15.45), P6Z+(28.53)}},
{{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(8.09), P2Y+(29.39), P2Z+(24.27)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(-8.09), P4Y+(29.39), P4Z+(24.27)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(-8.09), P6Y+(29.39), P6Z+(24.27)}},
{{P1X+(0.00), P1Y+(-30.00), P1Z+(0.00)}, {P2X+(5.88), P2Y+(40.45), P2Z+(17.63)}, {P3X+(0.00), P3Y+(-30.00), P3Z+(0.00)}, {P4X+(-5.88), P4Y+(40.45), P4Z+(17.63)}, {P5X+(0.00), P5Y+(-30.00), P5Z+(0.00)}, {P6X+(-5.88), P6Y+(40.45), P6Z+(17.63)}},
{{P1X+(0.00), P1Y+(-40.00), P1Z+(0.00)}, {P2X+(3.09), P2Y+(47.55), P2Z+(9.27)}, {P3X+(0.00), P3Y+(-40.00), P3Z+(0.00)}, {P4X+(-3.09), P4Y+(47.55), P4Z+(9.27)}, {P5X+(0.00), P5Y+(-40.00), P5Z+(0.00)}, {P6X+(-3.09), P6Y+(47.55), P6Z+(9.27)}},
{{P1X+(0.00), P1Y+(-50.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(50.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-50.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(50.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-50.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(50.00), P6Z+(0.00)}},
{{P1X+(3.09), P1Y+(-47.55), P1Z+(9.27)}, {P2X+(0.00), P2Y+(40.00), P2Z+(0.00)}, {P3X+(3.09), P3Y+(-47.55), P3Z+(9.27)}, {P4X+(0.00), P4Y+(40.00), P4Z+(0.00)}, {P5X+(-3.09), P5Y+(-47.55), P5Z+(9.27)}, {P6X+(0.00), P6Y+(40.00), P6Z+(0.00)}},
{{P1X+(5.88), P1Y+(-40.45), P1Z+(17.63)}, {P2X+(0.00), P2Y+(30.00), P2Z+(0.00)}, {P3X+(5.88), P3Y+(-40.45), P3Z+(17.63)}, {P4X+(0.00), P4Y+(30.00), P4Z+(0.00)}, {P5X+(-5.88), P5Y+(-40.45), P5Z+(17.63)}, {P6X+(0.00), P6Y+(30.00), P6Z+(0.00)}},
{{P1X+(8.09), P1Y+(-29.39), P1Z+(24.27)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(8.09), P3Y+(-29.39), P3Z+(24.27)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(-8.09), P5Y+(-29.39), P5Z+(24.27)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}},
{{P1X+(9.51), P1Y+(-15.45), P1Z+(28.53)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(9.51), P3Y+(-15.45), P3Z+(28.53)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(-9.51), P5Y+(-15.45), P5Z+(28.53)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}},
};
const int forwardfast_entries[] { 0,10 };
const MovementTable forwardfast_table {forwardfast_paths, 20, 20, forwardfast_entries, 2 };
const Locations rotatex_paths[] {
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.97 + P1Z*-0.26 + 0.00, P1X*0.00 + P1Y*0.26 + P1Z*0.97 + 0.00},
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.97 + P2Z*-0.26 + 0.00, P2X*0.00 + P2Y*0.26 + P2Z*0.97 + 0.00},
@ -429,112 +479,243 @@ 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)}},
{{P1X+(-0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(-7.73), P1Y+(0.00), P1Z+(23.78)}, {P2X+(5.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-7.73), P3Y+(0.00), P3Z+(23.78)}, {P4X+(5.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-7.73), P5Y+(0.00), P5Z+(23.78)}, {P6X+(5.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-14.69), P1Y+(0.00), P1Z+(20.23)}, {P2X+(10.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-14.69), P3Y+(0.00), P3Z+(20.23)}, {P4X+(10.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-14.69), P5Y+(0.00), P5Z+(20.23)}, {P6X+(10.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-20.23), P1Y+(0.00), P1Z+(14.69)}, {P2X+(15.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.23), P3Y+(0.00), P3Z+(14.69)}, {P4X+(15.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.23), P5Y+(0.00), P5Z+(14.69)}, {P6X+(15.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-23.78), P1Y+(0.00), P1Z+(7.73)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-23.78), P3Y+(0.00), P3Z+(7.73)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-23.78), P5Y+(0.00), P5Z+(7.73)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-25.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(25.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-25.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(25.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-25.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(25.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(23.78), P2Y+(-0.00), P2Z+(7.73)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(23.78), P4Y+(-0.00), P4Z+(7.73)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(23.78), P6Y+(-0.00), P6Z+(7.73)}},
{{P1X+(-15.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.23), P2Y+(-0.00), P2Z+(14.69)}, {P3X+(-15.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.23), P4Y+(-0.00), P4Z+(14.69)}, {P5X+(-15.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.23), P6Y+(-0.00), P6Z+(14.69)}},
{{P1X+(-10.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(14.69), P2Y+(-0.00), P2Z+(20.23)}, {P3X+(-10.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(14.69), P4Y+(-0.00), P4Z+(20.23)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(14.69), P6Y+(-0.00), P6Z+(20.23)}},
{{P1X+(-5.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(7.73), P2Y+(-0.00), P2Z+(23.78)}, {P3X+(-5.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(7.73), P4Y+(-0.00), P4Z+(23.78)}, {P5X+(-5.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(7.73), P6Y+(-0.00), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(0.00), P6Z+(25.00)}},
{{P1X+(5.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-7.73), P2Y+(0.00), P2Z+(23.78)}, {P3X+(5.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-7.73), P4Y+(0.00), P4Z+(23.78)}, {P5X+(5.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-7.73), P6Y+(0.00), P6Z+(23.78)}},
{{P1X+(10.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-14.69), P2Y+(0.00), P2Z+(20.23)}, {P3X+(10.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-14.69), P4Y+(0.00), P4Z+(20.23)}, {P5X+(10.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-14.69), P6Y+(0.00), P6Z+(20.23)}},
{{P1X+(15.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.23), P2Y+(0.00), P2Z+(14.69)}, {P3X+(15.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.23), P4Y+(0.00), P4Z+(14.69)}, {P5X+(15.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.23), P6Y+(0.00), P6Z+(14.69)}},
{{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-23.78), P2Y+(0.00), P2Z+(7.73)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-23.78), P4Y+(0.00), P4Z+(7.73)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-23.78), P6Y+(0.00), P6Z+(7.73)}},
{{P1X+(25.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-25.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(25.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-25.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(25.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-25.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(23.78), P1Y+(-0.00), P1Z+(7.73)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(23.78), P3Y+(-0.00), P3Z+(7.73)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(23.78), P5Y+(-0.00), P5Z+(7.73)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(20.23), P1Y+(-0.00), P1Z+(14.69)}, {P2X+(-15.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.23), P3Y+(-0.00), P3Z+(14.69)}, {P4X+(-15.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.23), P5Y+(-0.00), P5Z+(14.69)}, {P6X+(-15.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(14.69), P1Y+(-0.00), P1Z+(20.23)}, {P2X+(-10.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(14.69), P3Y+(-0.00), P3Z+(20.23)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(14.69), P5Y+(-0.00), P5Z+(20.23)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(7.73), P1Y+(-0.00), P1Z+(23.78)}, {P2X+(-5.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(7.73), P3Y+(-0.00), P3Z+(23.78)}, {P4X+(-5.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(7.73), P5Y+(-0.00), P5Z+(23.78)}, {P6X+(-5.00), P6Y+(0.00), P6Z+(0.00)}},
};
const int shiftleft_entries[] { 0,10 };
const MovementTable shiftleft_table {shiftleft_paths, 20, 20, shiftleft_entries, 2 };
const Locations shiftright_paths[] {
{{P1X+(0.00), P1Y+(-0.00), P1Z+(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)}},
{{P1X+(0.00), P1Y+(-0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(7.73), P1Y+(-0.00), P1Z+(23.78)}, {P2X+(-5.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(7.73), P3Y+(-0.00), P3Z+(23.78)}, {P4X+(-5.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(7.73), P5Y+(-0.00), P5Z+(23.78)}, {P6X+(-5.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(14.69), P1Y+(-0.00), P1Z+(20.23)}, {P2X+(-10.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(14.69), P3Y+(-0.00), P3Z+(20.23)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(14.69), P5Y+(-0.00), P5Z+(20.23)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(20.23), P1Y+(-0.00), P1Z+(14.69)}, {P2X+(-15.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.23), P3Y+(-0.00), P3Z+(14.69)}, {P4X+(-15.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.23), P5Y+(-0.00), P5Z+(14.69)}, {P6X+(-15.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(23.78), P1Y+(-0.00), P1Z+(7.73)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(23.78), P3Y+(-0.00), P3Z+(7.73)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(23.78), P5Y+(-0.00), P5Z+(7.73)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(25.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-25.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(25.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-25.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(25.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-25.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-23.78), P2Y+(0.00), P2Z+(7.73)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-23.78), P4Y+(0.00), P4Z+(7.73)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-23.78), P6Y+(0.00), P6Z+(7.73)}},
{{P1X+(15.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.23), P2Y+(0.00), P2Z+(14.69)}, {P3X+(15.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.23), P4Y+(0.00), P4Z+(14.69)}, {P5X+(15.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.23), P6Y+(0.00), P6Z+(14.69)}},
{{P1X+(10.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-14.69), P2Y+(0.00), P2Z+(20.23)}, {P3X+(10.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-14.69), P4Y+(0.00), P4Z+(20.23)}, {P5X+(10.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-14.69), P6Y+(0.00), P6Z+(20.23)}},
{{P1X+(5.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-7.73), P2Y+(0.00), P2Z+(23.78)}, {P3X+(5.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-7.73), P4Y+(0.00), P4Z+(23.78)}, {P5X+(5.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-7.73), P6Y+(0.00), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-0.00), P6Z+(25.00)}},
{{P1X+(-5.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(7.73), P2Y+(-0.00), P2Z+(23.78)}, {P3X+(-5.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(7.73), P4Y+(-0.00), P4Z+(23.78)}, {P5X+(-5.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(7.73), P6Y+(-0.00), P6Z+(23.78)}},
{{P1X+(-10.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(14.69), P2Y+(-0.00), P2Z+(20.23)}, {P3X+(-10.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(14.69), P4Y+(-0.00), P4Z+(20.23)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(14.69), P6Y+(-0.00), P6Z+(20.23)}},
{{P1X+(-15.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.23), P2Y+(-0.00), P2Z+(14.69)}, {P3X+(-15.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.23), P4Y+(-0.00), P4Z+(14.69)}, {P5X+(-15.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.23), P6Y+(-0.00), P6Z+(14.69)}},
{{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(23.78), P2Y+(-0.00), P2Z+(7.73)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(23.78), P4Y+(-0.00), P4Z+(7.73)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(23.78), P6Y+(-0.00), P6Z+(7.73)}},
{{P1X+(-25.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(25.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-25.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(25.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-25.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(25.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-23.78), P1Y+(0.00), P1Z+(7.73)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-23.78), P3Y+(0.00), P3Z+(7.73)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-23.78), P5Y+(0.00), P5Z+(7.73)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-20.23), P1Y+(0.00), P1Z+(14.69)}, {P2X+(15.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.23), P3Y+(0.00), P3Z+(14.69)}, {P4X+(15.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.23), P5Y+(0.00), P5Z+(14.69)}, {P6X+(15.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-14.69), P1Y+(0.00), P1Z+(20.23)}, {P2X+(10.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-14.69), P3Y+(0.00), P3Z+(20.23)}, {P4X+(10.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-14.69), P5Y+(0.00), P5Z+(20.23)}, {P6X+(10.00), P6Y+(-0.00), P6Z+(0.00)}},
{{P1X+(-7.73), P1Y+(0.00), P1Z+(23.78)}, {P2X+(5.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-7.73), P3Y+(0.00), P3Z+(23.78)}, {P4X+(5.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-7.73), P5Y+(0.00), P5Z+(23.78)}, {P6X+(5.00), P6Y+(-0.00), P6Z+(0.00)}},
};
const int shiftright_entries[] { 0,10 };
const MovementTable shiftright_table {shiftright_paths, 20, 20, shiftright_entries, 2 };
const Locations turnleft_paths[] {
{{P1X+(-0.00), P1Y+(0.00), P1Z+(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)}},
{{P1X+(-0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(-5.46), P1Y+(5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(5.46), P3Y+(5.46), P3Z+(23.78)}, {P4X+(-3.54), P4Y+(3.54), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(3.54), P6Y+(3.54), P6Z+(0.00)}},
{{P1X+(-10.39), P1Y+(10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(10.39), P3Y+(10.39), P3Z+(20.23)}, {P4X+(-7.07), P4Y+(7.07), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(7.07), P6Y+(7.07), P6Z+(0.00)}},
{{P1X+(-14.30), P1Y+(14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(14.30), P3Y+(14.30), P3Z+(14.69)}, {P4X+(-10.61), P4Y+(10.61), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(10.61), P6Y+(10.61), P6Z+(0.00)}},
{{P1X+(-16.81), P1Y+(16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(16.81), P3Y+(16.81), P3Z+(7.73)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
{{P1X+(-17.68), P1Y+(17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(17.68), P3Y+(17.68), P3Z+(0.00)}, {P4X+(-17.68), P4Y+(17.68), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(17.68), P6Y+(17.68), P6Z+(0.00)}},
{{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-16.81), P4Y+(16.81), P4Z+(7.73)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(16.81), P6Y+(16.81), P6Z+(7.73)}},
{{P1X+(-10.61), P1Y+(10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(10.61), P3Y+(10.61), P3Z+(0.00)}, {P4X+(-14.30), P4Y+(14.30), P4Z+(14.69)}, {P5X+(-0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(14.30), P6Y+(14.30), P6Z+(14.69)}},
{{P1X+(-7.07), P1Y+(7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(7.07), P3Y+(7.07), P3Z+(0.00)}, {P4X+(-10.39), P4Y+(10.39), P4Z+(20.23)}, {P5X+(-0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(10.39), P6Y+(10.39), P6Z+(20.23)}},
{{P1X+(-3.54), P1Y+(3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(3.54), P3Y+(3.54), P3Z+(0.00)}, {P4X+(-5.46), P4Y+(5.46), P4Z+(23.78)}, {P5X+(-0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(5.46), P6Y+(5.46), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-0.00), P6Z+(25.00)}},
{{P1X+(3.54), P1Y+(-3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(-3.54), P3Y+(-3.54), P3Z+(0.00)}, {P4X+(5.46), P4Y+(-5.46), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(-5.46), P6Y+(-5.46), P6Z+(23.78)}},
{{P1X+(7.07), P1Y+(-7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(-7.07), P3Y+(-7.07), P3Z+(0.00)}, {P4X+(10.39), P4Y+(-10.39), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-10.39), P6Y+(-10.39), P6Z+(20.23)}},
{{P1X+(10.61), P1Y+(-10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(-10.61), P3Y+(-10.61), P3Z+(0.00)}, {P4X+(14.30), P4Y+(-14.30), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(-14.30), P6Y+(-14.30), P6Z+(14.69)}},
{{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(16.81), P4Y+(-16.81), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-16.81), P6Y+(-16.81), P6Z+(7.73)}},
{{P1X+(17.68), P1Y+(-17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(-17.68), P3Y+(-17.68), P3Z+(0.00)}, {P4X+(17.68), P4Y+(-17.68), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(-17.68), P6Y+(-17.68), P6Z+(0.00)}},
{{P1X+(16.81), P1Y+(-16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-16.81), P3Y+(-16.81), P3Z+(7.73)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
{{P1X+(14.30), P1Y+(-14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(-14.30), P3Y+(-14.30), P3Z+(14.69)}, {P4X+(10.61), P4Y+(-10.61), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(-10.61), P6Y+(-10.61), P6Z+(0.00)}},
{{P1X+(10.39), P1Y+(-10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(-10.39), P3Y+(-10.39), P3Z+(20.23)}, {P4X+(7.07), P4Y+(-7.07), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(-7.07), P6Y+(-7.07), P6Z+(0.00)}},
{{P1X+(5.46), P1Y+(-5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(-5.46), P3Y+(-5.46), P3Z+(23.78)}, {P4X+(3.54), P4Y+(-3.54), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(-3.54), P6Y+(-3.54), P6Z+(0.00)}},
};
const int turnleft_entries[] { 0,10 };
const MovementTable turnleft_table {turnleft_paths, 20, 20, turnleft_entries, 2 };
const Locations turnright_paths[] {
{{P1X+(0.00), P1Y+(-0.00), P1Z+(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)}},
{{P1X+(0.00), P1Y+(-0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(-0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}},
{{P1X+(5.46), P1Y+(-5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(-5.46), P3Y+(-5.46), P3Z+(23.78)}, {P4X+(3.54), P4Y+(-3.54), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(-3.54), P6Y+(-3.54), P6Z+(0.00)}},
{{P1X+(10.39), P1Y+(-10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(-10.39), P3Y+(-10.39), P3Z+(20.23)}, {P4X+(7.07), P4Y+(-7.07), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(-7.07), P6Y+(-7.07), P6Z+(0.00)}},
{{P1X+(14.30), P1Y+(-14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(-14.30), P3Y+(-14.30), P3Z+(14.69)}, {P4X+(10.61), P4Y+(-10.61), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(-10.61), P6Y+(-10.61), P6Z+(0.00)}},
{{P1X+(16.81), P1Y+(-16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-16.81), P3Y+(-16.81), P3Z+(7.73)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}},
{{P1X+(17.68), P1Y+(-17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(-17.68), P3Y+(-17.68), P3Z+(0.00)}, {P4X+(17.68), P4Y+(-17.68), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(-17.68), P6Y+(-17.68), P6Z+(0.00)}},
{{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(16.81), P4Y+(-16.81), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-16.81), P6Y+(-16.81), P6Z+(7.73)}},
{{P1X+(10.61), P1Y+(-10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(-10.61), P3Y+(-10.61), P3Z+(0.00)}, {P4X+(14.30), P4Y+(-14.30), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(-14.30), P6Y+(-14.30), P6Z+(14.69)}},
{{P1X+(7.07), P1Y+(-7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(-7.07), P3Y+(-7.07), P3Z+(0.00)}, {P4X+(10.39), P4Y+(-10.39), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-10.39), P6Y+(-10.39), P6Z+(20.23)}},
{{P1X+(3.54), P1Y+(-3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(-3.54), P3Y+(-3.54), P3Z+(0.00)}, {P4X+(5.46), P4Y+(-5.46), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(-5.46), P6Y+(-5.46), P6Z+(23.78)}},
{{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}},
{{P1X+(-3.54), P1Y+(3.54), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(3.54), P3Y+(3.54), P3Z+(0.00)}, {P4X+(-5.46), P4Y+(5.46), P4Z+(23.78)}, {P5X+(-0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(5.46), P6Y+(5.46), P6Z+(23.78)}},
{{P1X+(-7.07), P1Y+(7.07), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(7.07), P3Y+(7.07), P3Z+(0.00)}, {P4X+(-10.39), P4Y+(10.39), P4Z+(20.23)}, {P5X+(-0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(10.39), P6Y+(10.39), P6Z+(20.23)}},
{{P1X+(-10.61), P1Y+(10.61), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(10.61), P3Y+(10.61), P3Z+(0.00)}, {P4X+(-14.30), P4Y+(14.30), P4Z+(14.69)}, {P5X+(-0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(14.30), P6Y+(14.30), P6Z+(14.69)}},
{{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-16.81), P4Y+(16.81), P4Z+(7.73)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(16.81), P6Y+(16.81), P6Z+(7.73)}},
{{P1X+(-17.68), P1Y+(17.68), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(17.68), P3Y+(17.68), P3Z+(0.00)}, {P4X+(-17.68), P4Y+(17.68), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(17.68), P6Y+(17.68), P6Z+(0.00)}},
{{P1X+(-16.81), P1Y+(16.81), P1Z+(7.73)}, {P2X+(-0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(16.81), P3Y+(16.81), P3Z+(7.73)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}},
{{P1X+(-14.30), P1Y+(14.30), P1Z+(14.69)}, {P2X+(-0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(14.30), P3Y+(14.30), P3Z+(14.69)}, {P4X+(-10.61), P4Y+(10.61), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(10.61), P6Y+(10.61), P6Z+(0.00)}},
{{P1X+(-10.39), P1Y+(10.39), P1Z+(20.23)}, {P2X+(-0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(10.39), P3Y+(10.39), P3Z+(20.23)}, {P4X+(-7.07), P4Y+(7.07), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(7.07), P6Y+(7.07), P6Z+(0.00)}},
{{P1X+(-5.46), P1Y+(5.46), P1Z+(23.78)}, {P2X+(-0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(5.46), P3Y+(5.46), P3Z+(23.78)}, {P4X+(-3.54), P4Y+(3.54), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(3.54), P6Y+(3.54), P6Z+(0.00)}},
};
const int turnright_entries[] { 0,10 };
const MovementTable turnright_table {turnright_paths, 20, 20, turnright_entries, 2 };
const Locations twist_paths[] {
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 0.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00},
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + 0.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00},
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + 0.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00},
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + 0.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00},
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + 0.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00},
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + 0.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
{{P1X*1.00 + P1Y*-0.07 + P1Z*0.00 + 0.00, P1X*0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*0.00 + P1Y*0.09 + P1Z*1.00 + 0.00},
{P2X*1.00 + P2Y*-0.07 + P2Z*0.00 + 0.00, P2X*0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*0.00 + P2Y*0.09 + P2Z*1.00 + 0.00},
{P3X*1.00 + P3Y*-0.07 + P3Z*0.00 + 0.00, P3X*0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*0.00 + P3Y*0.09 + P3Z*1.00 + 0.00},
{P4X*1.00 + P4Y*-0.07 + P4Z*0.00 + 0.00, P4X*0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*0.00 + P4Y*0.09 + P4Z*1.00 + 0.00},
{P5X*1.00 + P5Y*-0.07 + P5Z*0.00 + 0.00, P5X*0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*0.00 + P5Y*0.09 + P5Z*1.00 + 0.00},
{P6X*1.00 + P6Y*-0.07 + P6Z*0.00 + 0.00, P6X*0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
{{P1X*0.99 + P1Y*-0.14 + P1Z*0.01 + 0.00, P1X*0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*0.01 + P1Y*0.14 + P1Z*0.99 + 0.00},
{P2X*0.99 + P2Y*-0.14 + P2Z*0.01 + 0.00, P2X*0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*0.01 + P2Y*0.14 + P2Z*0.99 + 0.00},
{P3X*0.99 + P3Y*-0.14 + P3Z*0.01 + 0.00, P3X*0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*0.01 + P3Y*0.14 + P3Z*0.99 + 0.00},
{P4X*0.99 + P4Y*-0.14 + P4Z*0.01 + 0.00, P4X*0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*0.01 + P4Y*0.14 + P4Z*0.99 + 0.00},
{P5X*0.99 + P5Y*-0.14 + P5Z*0.01 + 0.00, P5X*0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*0.01 + P5Y*0.14 + P5Z*0.99 + 0.00},
{P6X*0.99 + P6Y*-0.14 + P6Z*0.01 + 0.00, P6X*0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
{{P1X*0.98 + P1Y*-0.21 + P1Z*0.03 + 0.00, P1X*0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*0.01 + P1Y*0.18 + P1Z*0.98 + 0.00},
{P2X*0.98 + P2Y*-0.21 + P2Z*0.03 + 0.00, P2X*0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*0.01 + P2Y*0.18 + P2Z*0.98 + 0.00},
{P3X*0.98 + P3Y*-0.21 + P3Z*0.03 + 0.00, P3X*0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*0.01 + P3Y*0.18 + P3Z*0.98 + 0.00},
{P4X*0.98 + P4Y*-0.21 + P4Z*0.03 + 0.00, P4X*0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*0.01 + P4Y*0.18 + P4Z*0.98 + 0.00},
{P5X*0.98 + P5Y*-0.21 + P5Z*0.03 + 0.00, P5X*0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*0.01 + P5Y*0.18 + P5Z*0.98 + 0.00},
{P6X*0.98 + P6Y*-0.21 + P6Z*0.03 + 0.00, P6X*0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
{{P1X*0.96 + P1Y*-0.27 + P1Z*0.05 + 0.00, P1X*0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*0.01 + P1Y*0.22 + P1Z*0.98 + 0.00},
{P2X*0.96 + P2Y*-0.27 + P2Z*0.05 + 0.00, P2X*0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*0.01 + P2Y*0.22 + P2Z*0.98 + 0.00},
{P3X*0.96 + P3Y*-0.27 + P3Z*0.05 + 0.00, P3X*0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*0.01 + P3Y*0.22 + P3Z*0.98 + 0.00},
{P4X*0.96 + P4Y*-0.27 + P4Z*0.05 + 0.00, P4X*0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*0.01 + P4Y*0.22 + P4Z*0.98 + 0.00},
{P5X*0.96 + P5Y*-0.27 + P5Z*0.05 + 0.00, P5X*0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*0.01 + P5Y*0.22 + P5Z*0.98 + 0.00},
{P6X*0.96 + P6Y*-0.27 + P6Z*0.05 + 0.00, P6X*0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
{{P1X*0.94 + P1Y*-0.33 + P1Z*0.07 + 0.00, P1X*0.34 + P1Y*0.91 + P1Z*-0.25 + 0.00, P1X*0.02 + P1Y*0.26 + P1Z*0.97 + 0.00},
{P2X*0.94 + P2Y*-0.33 + P2Z*0.07 + 0.00, P2X*0.34 + P2Y*0.91 + P2Z*-0.25 + 0.00, P2X*0.02 + P2Y*0.26 + P2Z*0.97 + 0.00},
{P3X*0.94 + P3Y*-0.33 + P3Z*0.07 + 0.00, P3X*0.34 + P3Y*0.91 + P3Z*-0.25 + 0.00, P3X*0.02 + P3Y*0.26 + P3Z*0.97 + 0.00},
{P4X*0.94 + P4Y*-0.33 + P4Z*0.07 + 0.00, P4X*0.34 + P4Y*0.91 + P4Z*-0.25 + 0.00, P4X*0.02 + P4Y*0.26 + P4Z*0.97 + 0.00},
{P5X*0.94 + P5Y*-0.33 + P5Z*0.07 + 0.00, P5X*0.34 + P5Y*0.91 + P5Z*-0.25 + 0.00, P5X*0.02 + P5Y*0.26 + P5Z*0.97 + 0.00},
{P6X*0.94 + P6Y*-0.33 + P6Z*0.07 + 0.00, P6X*0.34 + P6Y*0.91 + P6Z*-0.25 + 0.00, P6X*0.02 + P6Y*0.26 + P6Z*0.97 + 0.00}},
{{P1X*0.96 + P1Y*-0.27 + P1Z*0.05 + 0.00, P1X*0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*0.01 + P1Y*0.22 + P1Z*0.98 + 0.00},
{P2X*0.96 + P2Y*-0.27 + P2Z*0.05 + 0.00, P2X*0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*0.01 + P2Y*0.22 + P2Z*0.98 + 0.00},
{P3X*0.96 + P3Y*-0.27 + P3Z*0.05 + 0.00, P3X*0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*0.01 + P3Y*0.22 + P3Z*0.98 + 0.00},
{P4X*0.96 + P4Y*-0.27 + P4Z*0.05 + 0.00, P4X*0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*0.01 + P4Y*0.22 + P4Z*0.98 + 0.00},
{P5X*0.96 + P5Y*-0.27 + P5Z*0.05 + 0.00, P5X*0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*0.01 + P5Y*0.22 + P5Z*0.98 + 0.00},
{P6X*0.96 + P6Y*-0.27 + P6Z*0.05 + 0.00, P6X*0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
{{P1X*0.98 + P1Y*-0.21 + P1Z*0.03 + 0.00, P1X*0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*0.01 + P1Y*0.18 + P1Z*0.98 + 0.00},
{P2X*0.98 + P2Y*-0.21 + P2Z*0.03 + 0.00, P2X*0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*0.01 + P2Y*0.18 + P2Z*0.98 + 0.00},
{P3X*0.98 + P3Y*-0.21 + P3Z*0.03 + 0.00, P3X*0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*0.01 + P3Y*0.18 + P3Z*0.98 + 0.00},
{P4X*0.98 + P4Y*-0.21 + P4Z*0.03 + 0.00, P4X*0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*0.01 + P4Y*0.18 + P4Z*0.98 + 0.00},
{P5X*0.98 + P5Y*-0.21 + P5Z*0.03 + 0.00, P5X*0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*0.01 + P5Y*0.18 + P5Z*0.98 + 0.00},
{P6X*0.98 + P6Y*-0.21 + P6Z*0.03 + 0.00, P6X*0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
{{P1X*0.99 + P1Y*-0.14 + P1Z*0.01 + 0.00, P1X*0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*0.01 + P1Y*0.14 + P1Z*0.99 + 0.00},
{P2X*0.99 + P2Y*-0.14 + P2Z*0.01 + 0.00, P2X*0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*0.01 + P2Y*0.14 + P2Z*0.99 + 0.00},
{P3X*0.99 + P3Y*-0.14 + P3Z*0.01 + 0.00, P3X*0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*0.01 + P3Y*0.14 + P3Z*0.99 + 0.00},
{P4X*0.99 + P4Y*-0.14 + P4Z*0.01 + 0.00, P4X*0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*0.01 + P4Y*0.14 + P4Z*0.99 + 0.00},
{P5X*0.99 + P5Y*-0.14 + P5Z*0.01 + 0.00, P5X*0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*0.01 + P5Y*0.14 + P5Z*0.99 + 0.00},
{P6X*0.99 + P6Y*-0.14 + P6Z*0.01 + 0.00, P6X*0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
{{P1X*1.00 + P1Y*-0.07 + P1Z*0.00 + 0.00, P1X*0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*0.00 + P1Y*0.09 + P1Z*1.00 + 0.00},
{P2X*1.00 + P2Y*-0.07 + P2Z*0.00 + 0.00, P2X*0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*0.00 + P2Y*0.09 + P2Z*1.00 + 0.00},
{P3X*1.00 + P3Y*-0.07 + P3Z*0.00 + 0.00, P3X*0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*0.00 + P3Y*0.09 + P3Z*1.00 + 0.00},
{P4X*1.00 + P4Y*-0.07 + P4Z*0.00 + 0.00, P4X*0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*0.00 + P4Y*0.09 + P4Z*1.00 + 0.00},
{P5X*1.00 + P5Y*-0.07 + P5Z*0.00 + 0.00, P5X*0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*0.00 + P5Y*0.09 + P5Z*1.00 + 0.00},
{P6X*1.00 + P6Y*-0.07 + P6Z*0.00 + 0.00, P6X*0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
{{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 0.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00},
{P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + 0.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00},
{P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + 0.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00},
{P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + 0.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00},
{P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + 0.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00},
{P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + 0.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}},
{{P1X*1.00 + P1Y*0.07 + P1Z*-0.00 + 0.00, P1X*-0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*-0.00 + P1Y*0.09 + P1Z*1.00 + 0.00},
{P2X*1.00 + P2Y*0.07 + P2Z*-0.00 + 0.00, P2X*-0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*-0.00 + P2Y*0.09 + P2Z*1.00 + 0.00},
{P3X*1.00 + P3Y*0.07 + P3Z*-0.00 + 0.00, P3X*-0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*-0.00 + P3Y*0.09 + P3Z*1.00 + 0.00},
{P4X*1.00 + P4Y*0.07 + P4Z*-0.00 + 0.00, P4X*-0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*-0.00 + P4Y*0.09 + P4Z*1.00 + 0.00},
{P5X*1.00 + P5Y*0.07 + P5Z*-0.00 + 0.00, P5X*-0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*-0.00 + P5Y*0.09 + P5Z*1.00 + 0.00},
{P6X*1.00 + P6Y*0.07 + P6Z*-0.00 + 0.00, P6X*-0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*-0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
{{P1X*0.99 + P1Y*0.14 + P1Z*-0.01 + 0.00, P1X*-0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*-0.01 + P1Y*0.14 + P1Z*0.99 + 0.00},
{P2X*0.99 + P2Y*0.14 + P2Z*-0.01 + 0.00, P2X*-0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*-0.01 + P2Y*0.14 + P2Z*0.99 + 0.00},
{P3X*0.99 + P3Y*0.14 + P3Z*-0.01 + 0.00, P3X*-0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*-0.01 + P3Y*0.14 + P3Z*0.99 + 0.00},
{P4X*0.99 + P4Y*0.14 + P4Z*-0.01 + 0.00, P4X*-0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*-0.01 + P4Y*0.14 + P4Z*0.99 + 0.00},
{P5X*0.99 + P5Y*0.14 + P5Z*-0.01 + 0.00, P5X*-0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*-0.01 + P5Y*0.14 + P5Z*0.99 + 0.00},
{P6X*0.99 + P6Y*0.14 + P6Z*-0.01 + 0.00, P6X*-0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*-0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
{{P1X*0.98 + P1Y*0.21 + P1Z*-0.03 + 0.00, P1X*-0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*-0.01 + P1Y*0.18 + P1Z*0.98 + 0.00},
{P2X*0.98 + P2Y*0.21 + P2Z*-0.03 + 0.00, P2X*-0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*-0.01 + P2Y*0.18 + P2Z*0.98 + 0.00},
{P3X*0.98 + P3Y*0.21 + P3Z*-0.03 + 0.00, P3X*-0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*-0.01 + P3Y*0.18 + P3Z*0.98 + 0.00},
{P4X*0.98 + P4Y*0.21 + P4Z*-0.03 + 0.00, P4X*-0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*-0.01 + P4Y*0.18 + P4Z*0.98 + 0.00},
{P5X*0.98 + P5Y*0.21 + P5Z*-0.03 + 0.00, P5X*-0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*-0.01 + P5Y*0.18 + P5Z*0.98 + 0.00},
{P6X*0.98 + P6Y*0.21 + P6Z*-0.03 + 0.00, P6X*-0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*-0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
{{P1X*0.96 + P1Y*0.27 + P1Z*-0.05 + 0.00, P1X*-0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*-0.01 + P1Y*0.22 + P1Z*0.98 + 0.00},
{P2X*0.96 + P2Y*0.27 + P2Z*-0.05 + 0.00, P2X*-0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*-0.01 + P2Y*0.22 + P2Z*0.98 + 0.00},
{P3X*0.96 + P3Y*0.27 + P3Z*-0.05 + 0.00, P3X*-0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*-0.01 + P3Y*0.22 + P3Z*0.98 + 0.00},
{P4X*0.96 + P4Y*0.27 + P4Z*-0.05 + 0.00, P4X*-0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*-0.01 + P4Y*0.22 + P4Z*0.98 + 0.00},
{P5X*0.96 + P5Y*0.27 + P5Z*-0.05 + 0.00, P5X*-0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*-0.01 + P5Y*0.22 + P5Z*0.98 + 0.00},
{P6X*0.96 + P6Y*0.27 + P6Z*-0.05 + 0.00, P6X*-0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*-0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
{{P1X*0.94 + P1Y*0.33 + P1Z*-0.07 + 0.00, P1X*-0.34 + P1Y*0.91 + P1Z*-0.25 + 0.00, P1X*-0.02 + P1Y*0.26 + P1Z*0.97 + 0.00},
{P2X*0.94 + P2Y*0.33 + P2Z*-0.07 + 0.00, P2X*-0.34 + P2Y*0.91 + P2Z*-0.25 + 0.00, P2X*-0.02 + P2Y*0.26 + P2Z*0.97 + 0.00},
{P3X*0.94 + P3Y*0.33 + P3Z*-0.07 + 0.00, P3X*-0.34 + P3Y*0.91 + P3Z*-0.25 + 0.00, P3X*-0.02 + P3Y*0.26 + P3Z*0.97 + 0.00},
{P4X*0.94 + P4Y*0.33 + P4Z*-0.07 + 0.00, P4X*-0.34 + P4Y*0.91 + P4Z*-0.25 + 0.00, P4X*-0.02 + P4Y*0.26 + P4Z*0.97 + 0.00},
{P5X*0.94 + P5Y*0.33 + P5Z*-0.07 + 0.00, P5X*-0.34 + P5Y*0.91 + P5Z*-0.25 + 0.00, P5X*-0.02 + P5Y*0.26 + P5Z*0.97 + 0.00},
{P6X*0.94 + P6Y*0.33 + P6Z*-0.07 + 0.00, P6X*-0.34 + P6Y*0.91 + P6Z*-0.25 + 0.00, P6X*-0.02 + P6Y*0.26 + P6Z*0.97 + 0.00}},
{{P1X*0.96 + P1Y*0.27 + P1Z*-0.05 + 0.00, P1X*-0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*-0.01 + P1Y*0.22 + P1Z*0.98 + 0.00},
{P2X*0.96 + P2Y*0.27 + P2Z*-0.05 + 0.00, P2X*-0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*-0.01 + P2Y*0.22 + P2Z*0.98 + 0.00},
{P3X*0.96 + P3Y*0.27 + P3Z*-0.05 + 0.00, P3X*-0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*-0.01 + P3Y*0.22 + P3Z*0.98 + 0.00},
{P4X*0.96 + P4Y*0.27 + P4Z*-0.05 + 0.00, P4X*-0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*-0.01 + P4Y*0.22 + P4Z*0.98 + 0.00},
{P5X*0.96 + P5Y*0.27 + P5Z*-0.05 + 0.00, P5X*-0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*-0.01 + P5Y*0.22 + P5Z*0.98 + 0.00},
{P6X*0.96 + P6Y*0.27 + P6Z*-0.05 + 0.00, P6X*-0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*-0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}},
{{P1X*0.98 + P1Y*0.21 + P1Z*-0.03 + 0.00, P1X*-0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*-0.01 + P1Y*0.18 + P1Z*0.98 + 0.00},
{P2X*0.98 + P2Y*0.21 + P2Z*-0.03 + 0.00, P2X*-0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*-0.01 + P2Y*0.18 + P2Z*0.98 + 0.00},
{P3X*0.98 + P3Y*0.21 + P3Z*-0.03 + 0.00, P3X*-0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*-0.01 + P3Y*0.18 + P3Z*0.98 + 0.00},
{P4X*0.98 + P4Y*0.21 + P4Z*-0.03 + 0.00, P4X*-0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*-0.01 + P4Y*0.18 + P4Z*0.98 + 0.00},
{P5X*0.98 + P5Y*0.21 + P5Z*-0.03 + 0.00, P5X*-0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*-0.01 + P5Y*0.18 + P5Z*0.98 + 0.00},
{P6X*0.98 + P6Y*0.21 + P6Z*-0.03 + 0.00, P6X*-0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*-0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}},
{{P1X*0.99 + P1Y*0.14 + P1Z*-0.01 + 0.00, P1X*-0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*-0.01 + P1Y*0.14 + P1Z*0.99 + 0.00},
{P2X*0.99 + P2Y*0.14 + P2Z*-0.01 + 0.00, P2X*-0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*-0.01 + P2Y*0.14 + P2Z*0.99 + 0.00},
{P3X*0.99 + P3Y*0.14 + P3Z*-0.01 + 0.00, P3X*-0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*-0.01 + P3Y*0.14 + P3Z*0.99 + 0.00},
{P4X*0.99 + P4Y*0.14 + P4Z*-0.01 + 0.00, P4X*-0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*-0.01 + P4Y*0.14 + P4Z*0.99 + 0.00},
{P5X*0.99 + P5Y*0.14 + P5Z*-0.01 + 0.00, P5X*-0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*-0.01 + P5Y*0.14 + P5Z*0.99 + 0.00},
{P6X*0.99 + P6Y*0.14 + P6Z*-0.01 + 0.00, P6X*-0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*-0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}},
{{P1X*1.00 + P1Y*0.07 + P1Z*-0.00 + 0.00, P1X*-0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*-0.00 + P1Y*0.09 + P1Z*1.00 + 0.00},
{P2X*1.00 + P2Y*0.07 + P2Z*-0.00 + 0.00, P2X*-0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*-0.00 + P2Y*0.09 + P2Z*1.00 + 0.00},
{P3X*1.00 + P3Y*0.07 + P3Z*-0.00 + 0.00, P3X*-0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*-0.00 + P3Y*0.09 + P3Z*1.00 + 0.00},
{P4X*1.00 + P4Y*0.07 + P4Z*-0.00 + 0.00, P4X*-0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*-0.00 + P4Y*0.09 + P4Z*1.00 + 0.00},
{P5X*1.00 + P5Y*0.07 + P5Z*-0.00 + 0.00, P5X*-0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*-0.00 + P5Y*0.09 + P5Z*1.00 + 0.00},
{P6X*1.00 + P6Y*0.07 + P6Z*-0.00 + 0.00, P6X*-0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*-0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}},
};
const int twist_entries[] { 0,10 };
const MovementTable twist_table {twist_paths, 20, 50, twist_entries, 2 };
}
const MovementTable& backwardTable() {
return backward_table;
}
const MovementTable& climbTable() {
return climb_table;
}
const MovementTable& forwardTable() {
return forward_table;
}
const MovementTable& forwardfastTable() {
return forwardfast_table;
}
const MovementTable& rotatexTable() {
return rotatex_table;
}
@ -556,3 +737,6 @@ const MovementTable& turnleftTable() {
const MovementTable& turnrightTable() {
return turnright_table;
}
const MovementTable& twistTable() {
return twist_table;
}

20
software/hexapod7697/src/normal_mode.cpp
Unescape Escape View File

@ -12,15 +12,19 @@
#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 buttonForward(hexapod::MOVEMENT_FORWARD, "Forward", 1, 3, 1, 1, RC_BLUE);
static Button buttonRun(hexapod::MOVEMENT_FORWARDFAST, "Run", 1, 2, 1, 1, RC_ORANGE);
static Button buttonBackward(hexapod::MOVEMENT_BACKWARD, "Backward", 1, 4, 1, 1, RC_BLUE);
static Button buttonTL(hexapod::MOVEMENT_TURNLEFT, "TurnLeft", 0, 3, 1, 1, RC_GREEN);
static Button buttonTR(hexapod::MOVEMENT_TURNRIGHT, "TurnRight", 2, 3, 1, 1, RC_GREEN);
static Button buttonSL(hexapod::MOVEMENT_SHIFTLEFT, "ShiftLeft", 0, 4, 1, 1, RC_BLUE);
static Button buttonSR(hexapod::MOVEMENT_SHIFTRIGHT, "ShiftRight", 2, 4, 1, 1, RC_BLUE);
static Button buttonClimb(hexapod::MOVEMENT_CLIMB, "Climb", 2, 2, 1, 1, RC_ORANGE);
static Button buttonRotateX(hexapod::MOVEMENT_ROTATEX, "RotateX", 0, 0, 1, 1, RC_YELLOW);
static Button buttonRotateY(hexapod::MOVEMENT_ROTATEY, "RotateY", 1, 0, 1, 1, RC_YELLOW);
static Button buttonRotateZ(hexapod::MOVEMENT_ROTATEZ, "RotateZ", 2, 0, 1, 1, RC_YELLOW);
static Button buttonTwist(hexapod::MOVEMENT_TWIST, "Twist", 0, 1, 1, 1, RC_YELLOW);
static ButtonGroup btnGroup;
@ -37,13 +41,17 @@ void normal_setup(void) {
&buttonForward, &buttonBackward,
&buttonTL, &buttonTR,
&buttonSL, &buttonSR,
&buttonRun, &buttonClimb,
&buttonRotateX, &buttonRotateY, &buttonRotateZ,
&buttonTwist,
});
btnGroup.addControls({
&buttonForward, &buttonBackward,
&buttonTL, &buttonTR,
&buttonSL, &buttonSR,
&buttonRun, &buttonClimb,
&buttonRotateX, &buttonRotateY, &buttonRotateZ,
&buttonTwist,
});
LRemote.begin();

48
software/pathTool/src/config.py
Unescape Escape View File

@ -0,0 +1,48 @@
kLegMountLeftRightX = 29.87
kLegMountOtherX = 22.41
kLegMountOtherY = 55.41
kLegRootToJoint1 = 20.75
kLegJoint1ToJoint2 = 28.0
kLegJoint2ToJoint3 = 42.6
kLegJoint3ToTip = 89.07
SIN30 = 0.5
COS30 = 0.866
SIN45 = 0.7071
COS45 = 0.7071
SIN15 = 0.2588
COS15 = 0.9659
STANDBY_Z = (kLegJoint3ToTip*COS15-kLegJoint2ToJoint3*SIN30)
LEFTRIGHT_X = (kLegMountLeftRightX+kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)
OTHER_X = (kLegMountOtherX + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*COS45)
OTHER_Y = (kLegMountOtherY + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*SIN45)
defaultPosition = (
(OTHER_X, OTHER_Y, -STANDBY_Z),
(LEFTRIGHT_X, 0, -STANDBY_Z),
(OTHER_X, -OTHER_Y, -STANDBY_Z),
(-OTHER_X, -OTHER_Y, -STANDBY_Z),
(-LEFTRIGHT_X, 0, -STANDBY_Z),
(-OTHER_X, OTHER_Y, -STANDBY_Z),
)
mountPosition = (
(kLegMountOtherX, kLegMountOtherY, 0),
(kLegMountLeftRightX, 0, 0),
(kLegMountOtherX, -kLegMountOtherY, 0),
(-kLegMountOtherX, -kLegMountOtherY, 0),
(-kLegMountLeftRightX, 0, 0),
(-kLegMountOtherX, kLegMountOtherY, 0),
)
defaultAngle = (
-45, 0, 45, 135, 180, 225
)
angleLimitation = (
(-45, 45),
(-45, 75),
(-60, 60),
)

25
software/pathTool/src/kinematics.py
Unescape Escape View File

@ -0,0 +1,25 @@
import math
import config
pi = math.acos(-1)
def ik(to):
angles = []
x = to[0] - config.kLegRootToJoint1
y = to[1]
angles.append(math.atan2(y, x) * 180 / pi)
x = math.sqrt(x*x + y*y) - config.kLegJoint1ToJoint2
y = to[2]
ar = math.atan2(y, x)
lr2 = x*x + y*y
lr = math.sqrt(lr2)
a1 = math.acos((lr2 + config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 - config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint2ToJoint3*lr))
a2 = math.acos((lr2 - config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 + config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint3ToTip*lr))
angles.append((ar + a1) * 180 / pi)
angles.append(90 - ((a1 + a2) * 180 / pi))
return angles

60
software/pathTool/src/main.py
Unescape Escape View File

@ -3,6 +3,10 @@ import logging
import os
import sys
import config
import kinematics
from path.lib import point_rotate_z, matrix_mul
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))]:
@ -18,6 +22,55 @@ def show_detail(path, result):
for i, p in enumerate(result):
print("{:2d} {:5.2f}, {:5.2f}, {:5.2f}".format(i, p[0], p[1], p[2]))
def verify_points(pt):
angles = kinematics.ik(pt)
ok = True
failed = []
for i, angle in enumerate(angles):
if angle < config.angleLimitation[i][0] or angle > config.angleLimitation[i][1]:
ok = False
failed.append((i, angle))
return ok, failed
def verify_path(path, params):
data, mode, _, _ = params
print("Verifying {}...".format(path))
all_ok = True
if mode == "shift":
# data: float[6][N][3]
assert(len(data) == 6)
for i in range(len(data[0])):
for j in range(6):
pt = [config.defaultPosition[j][k] - config.mountPosition[j][k] + data[j][i][k] for k in range(3)]
pt = point_rotate_z(pt, config.defaultAngle[j])
ok, failed = verify_points(pt)
if not ok:
print("{}, {} failed: {}".format(i, j, failed))
all_ok = False
elif mode == "matrix":
# data: np.matrix[N]
for i in range(len(data)):
for j in range(6):
pt = matrix_mul(data[i], config.defaultPosition[j])
for k in range(3):
pt[k] -= config.mountPosition[j][k]
pt = point_rotate_z(pt, config.defaultAngle[j])
ok, failed = verify_points(pt)
if not ok:
print("{}, {} failed: {}".format(i, j, failed))
all_ok = False
return all_ok
def generate_c_body(path, params):
data, mode, dur, entries = params
result = "\nconst Locations {}_paths[] {{\n".format(path)
@ -77,6 +130,12 @@ if __name__ == '__main__':
# generate all paths
results = {path: generator() for path, generator in paths.items()}
# verify all path is within safe angles
verified = [1 for path, data in results.items() if not verify_path(path, data)]
if len(verified) > 0:
print("There were errors, exit...")
else:
# output results
with open(args.out_path, "w") as f:
print("//", file=f)
@ -92,4 +151,3 @@ if __name__ == '__main__':
print("Result written to {}".format(args.out_path))

0
software/pathTool/src/path/__init__.py
Unescape Escape View File

4
software/pathTool/src/path/backward.py
Unescape Escape View File

@ -1,7 +1,9 @@
from collections import deque
from lib import semicircle_generator
from forward import g_steps, g_radius
g_steps = 20
g_radius = 25
def path_generator():
assert (g_steps % 4) == 0

27
software/pathTool/src/path/climb.py
Unescape Escape View File

@ -0,0 +1,27 @@
from collections import deque
from lib import semicircle2_generator
g_steps = 20
y_radius = 20
z_radius = 80
x_radius = 30
z_shift = -30
def path_generator():
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
rpath = [(x, y, z + z_shift) for x, y, z in semicircle2_generator(g_steps, y_radius, z_radius, x_radius)]
lpath = [(x, y, z + z_shift) for x, y, z in semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)]
mir_rpath = deque(rpath)
mir_rpath.rotate(halfsteps)
mir_lpath = deque(lpath)
mir_lpath.rotate(halfsteps)
return [rpath, mir_rpath, rpath, mir_lpath, lpath, mir_lpath, ], "shift", 30, (0, halfsteps)

2
software/pathTool/src/path/forward.py
Unescape Escape View File

@ -4,7 +4,7 @@ from collections import deque
from lib import semicircle_generator
g_steps = 20
g_radius = 20
g_radius = 25
def path_generator():
assert (g_steps % 4) == 0

25
software/pathTool/src/path/forwardfast.py
Unescape Escape View File

@ -0,0 +1,25 @@
from collections import deque
from lib import semicircle2_generator
g_steps = 20
y_radius = 50
z_radius = 30
x_radius = 10
def path_generator():
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius)
lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)
mir_rpath = deque(rpath)
mir_rpath.rotate(halfsteps)
mir_lpath = deque(lpath)
mir_lpath.rotate(halfsteps)
return [rpath, mir_rpath, rpath, mir_lpath, lpath, mir_lpath, ], "shift", 20, (0, halfsteps)

33
software/pathTool/src/path/lib.py
Unescape Escape View File

@ -32,6 +32,35 @@ def semicircle_generator(radius, steps, reverse=False):
return result
def semicircle2_generator(steps, y_radius, z_radius, x_radius, 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, y_radius - i*y_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 = y_radius * math.cos(angle)
z = z_radius * math.sin(angle)
x = x_radius * math.sin(angle)
result.append((x, 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([
@ -59,6 +88,10 @@ def get_rotate_z_matrix(angle):
[0, 0, 0, 1],
])
def matrix_mul(m, pt):
ptx = list(pt) + [1]
return list((m * np.matrix(ptx).T).T.flat)[:-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]

2
software/pathTool/src/path/shiftleft.py
Unescape Escape View File

@ -5,7 +5,7 @@ from lib import semicircle_generator
from lib import path_rotate_z
g_steps = 20
g_radius = 20
g_radius = 25
def path_generator():
assert (g_steps % 4) == 0

2
software/pathTool/src/path/shiftright.py
Unescape Escape View File

@ -5,7 +5,7 @@ from lib import semicircle_generator
from lib import path_rotate_z
g_steps = 20
g_radius = 20
g_radius = 25
def path_generator():
assert (g_steps % 4) == 0

2
software/pathTool/src/path/turnleft.py
Unescape Escape View File

@ -5,7 +5,7 @@ from lib import semicircle_generator
from lib import path_rotate_z
g_steps = 20
g_radius = 20
g_radius = 25
def path_generator():
assert (g_steps % 4) == 0

2
software/pathTool/src/path/turnright.py
Unescape Escape View File

@ -5,7 +5,7 @@ from lib import semicircle_generator
from lib import path_rotate_z
g_steps = 20
g_radius = 20
g_radius = 25
def path_generator():
assert (g_steps % 4) == 0

35
software/pathTool/src/path/twist.py
Unescape Escape View File

@ -0,0 +1,35 @@
from collections import deque
import math
from lib import get_rotate_x_matrix, get_rotate_z_matrix
g_steps = 20
raise_angle = 3
twist_x_angle = 20
twise_y_angle = 12
def path_generator():
assert (g_steps % 4) == 0
result = []
quarter = int(g_steps / 4)
step_x_angle = twist_x_angle / quarter
step_y_angle = twise_y_angle / quarter
m = get_rotate_x_matrix(raise_angle)
for i in range(quarter):
result.append(m * get_rotate_z_matrix(i*step_x_angle) * get_rotate_x_matrix(i*step_y_angle))
for i in range(quarter):
result.append(m * get_rotate_z_matrix((quarter-i)*step_x_angle) * get_rotate_x_matrix((quarter-i)*step_y_angle))
for i in range(quarter):
result.append(m * get_rotate_z_matrix(-i*step_x_angle) * get_rotate_x_matrix(i*step_y_angle))
for i in range(quarter):
result.append(m * get_rotate_z_matrix((-quarter+i)*step_x_angle) * get_rotate_x_matrix((quarter-i)*step_y_angle))
return result, "matrix", 50, [0, 10]
Write Preview
Loading…
Cancel
Save