-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpositions.py
100 lines (78 loc) · 6.4 KB
/
positions.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
#POSITIONS
# rotate +ve = roll the cube forwards like a front bike wheel rolling forwards
# lift +ve = arms up
# grab +ve = arms in
# time = time taken to move to the position
from robot_1 import INITIALISATION, ARMS_UP_OUT_THE_WAY, ARMS_WIDE_ZERO, ARMS_ON_CUBE_ZERO, LIFT_CUBE_ZERO, RELEASE_CUBE_ZERO, ARMS_WIDE_POSTIVE, ARMS_ON_CUBE_90, LIFT_CUBE_90, TURN_CUBE_90, DOWN_CUBE_90, RELEASE_CUBE_90, ARMS_WIDE_NEGATIVE, ARMS_ON_CUBE_NEGATIVE_90, LIFT_CUBE_NEGATIVE_90, TURN_CUBE_NEGATIVE_90, DOWN_CUBE_NEGATIVE_90, RELEASE_CUBE_NEGATIVE_90, PREPARE_SECOND_ROTATE_180, ARMS_ON_CUBE_180_FAST, ARMS_ON_CUBE_NEGATIVE_180, LIFT_CUBE_NEGATIVE_180, TURN_CUBE_NEGATIVE_180, DOWN_CUBE_NEGATIVE_180, RELEASE_CUBE_NEGATIVE_180, PREPARE_SECOND_ROTATE_NEGATIVE_180, ARMS_ON_CUBE_NEGATIVE_180_FAST, LIFT_TIME_INDENT, ARMS_CLOSED_PROTECTED, ARMS_UP_OUT_THE_WAY_WAIT
# INITIALISATION = {'rotate': 0, 'lift': 0, 'grab': 0, 'time': 1}
# ARMS_UP_OUT_THE_WAY = {'rotate': 0, 'lift': 100, 'grab': - 75, 'time': 1}
# ARMS_WIDE_ZERO = {'rotate': 0, 'lift': - 100, 'grab': - 70, 'time': 1}
# ARMS_ON_CUBE_ZERO = {'rotate': 0, 'lift': - 100, 'grab': 35, 'time': 2}
# LIFT_CUBE_ZERO = {'rotate': 60, 'lift': 0, 'grab': 35, 'time': 1}
# RELEASE_CUBE_ZERO = {'rotate': 60, 'lift': 0, 'grab': -20, 'time': 0.5}
# ARMS_WIDE_POSTIVE = {'rotate': - 100, 'lift': - 60, 'grab': - 70, 'time': 1}
# ARMS_ON_CUBE_90 = {'rotate': - 100, 'lift': - 60, 'grab': 35, 'time': 2}
# LIFT_CUBE_90 = {'rotate': - 100, 'lift': 80, 'grab': 35, 'time': 2}
# TURN_CUBE_90 = {'rotate': 100, 'lift': 80, 'grab': 35, 'time': 2}
# DOWN_CUBE_90 = {'rotate': 100, 'lift': -20, 'grab': 35, 'time': 1}
# RELEASE_CUBE_90 = {'rotate': 100, 'lift': -20, 'grab': -20, 'time': 0.5}
# ARMS_WIDE_NEGATIVE = {'rotate': 100, 'lift': - 50, 'grab': - 70, 'time': 1}
# ARMS_ON_CUBE_NEGATIVE_90 = {'rotate': 100, 'lift': - 50, 'grab': 35, 'time': 2}
# LIFT_CUBE_NEGATIVE_90 = {'rotate': 100, 'lift': 60, 'grab': 35, 'time': 2}
# TURN_CUBE_NEGATIVE_90 = {'rotate': 10, 'lift': 60, 'grab': 35, 'time': 2}
# DOWN_CUBE_NEGATIVE_90 = {'rotate': -80, 'lift': -10, 'grab': 35, 'time': 1}
# RELEASE_CUBE_NEGATIVE_90 = {'rotate': -80, 'lift': -10, 'grab': -20, 'time': 0.5}
# PREPARE_SECOND_ROTATE_180 = {'rotate': - 100, 'lift': - 60, 'grab': -20, 'time': 0.5}
# ARMS_ON_CUBE_180_FAST = {'rotate': -100, 'lift': - 60, 'grab': 35, 'time': 0.5}
# ARMS_ON_CUBE_NEGATIVE_180 = {'rotate': 100, 'lift': - 70, 'grab': 35, 'time': 2}
# LIFT_CUBE_NEGATIVE_180 = {'rotate': 100, 'lift': 40, 'grab': 35, 'time': 2}
# TURN_CUBE_NEGATIVE_180 = {'rotate': - 100, 'lift': 40, 'grab': 35, 'time': 2}
# DOWN_CUBE_NEGATIVE_180 = {'rotate': - 100, 'lift': -35, 'grab': 35, 'time': 1}
# RELEASE_CUBE_NEGATIVE_180 = {'rotate': - 100, 'lift': 0, 'grab': -20, 'time': 0.5}
# PREPARE_SECOND_ROTATE_NEGATIVE_180 = {'rotate': 100, 'lift': - 40, 'grab': -20, 'time': 0.5}
# ARMS_ON_CUBE_NEGATIVE_180_FAST = {'rotate': 100, 'lift': - 50, 'grab': 35, 'time': 0.5}
def getSequenceTime(sequence):
time = 0
for position in sequence:
time += position['time']
return time
TEST_SEQUENCE_ZERO = [ARMS_WIDE_ZERO, ARMS_ON_CUBE_ZERO, LIFT_CUBE_ZERO, RELEASE_CUBE_ZERO]
TEST_SEQUENCE_90 = [ARMS_WIDE_POSTIVE, ARMS_ON_CUBE_90, LIFT_CUBE_90, TURN_CUBE_90, DOWN_CUBE_90, RELEASE_CUBE_90, ARMS_UP_OUT_THE_WAY]
TEST_SEQUENCE_180 = [ARMS_WIDE_POSTIVE, ARMS_ON_CUBE_90, LIFT_CUBE_90, TURN_CUBE_90, DOWN_CUBE_90, RELEASE_CUBE_90, PREPARE_SECOND_ROTATE_180, ARMS_ON_CUBE_180_FAST, LIFT_CUBE_90, TURN_CUBE_90, DOWN_CUBE_90, RELEASE_CUBE_90, ARMS_UP_OUT_THE_WAY]
TEST_SEQUENCE_NEGATIVE_180 = [ARMS_WIDE_NEGATIVE, ARMS_ON_CUBE_NEGATIVE_180, LIFT_CUBE_NEGATIVE_180, TURN_CUBE_NEGATIVE_180, DOWN_CUBE_NEGATIVE_180, RELEASE_CUBE_NEGATIVE_180, PREPARE_SECOND_ROTATE_NEGATIVE_180, ARMS_ON_CUBE_NEGATIVE_180_FAST, LIFT_CUBE_NEGATIVE_90, TURN_CUBE_NEGATIVE_90, DOWN_CUBE_NEGATIVE_90, RELEASE_CUBE_NEGATIVE_90, ARMS_UP_OUT_THE_WAY]
TEST_SEQUENCE_NEGATIVE_90 = [ARMS_WIDE_NEGATIVE, ARMS_ON_CUBE_NEGATIVE_90, LIFT_CUBE_NEGATIVE_90, TURN_CUBE_NEGATIVE_90, DOWN_CUBE_NEGATIVE_90, RELEASE_CUBE_NEGATIVE_90, ARMS_UP_OUT_THE_WAY]
PHASE_0_ZERO = [ARMS_WIDE_ZERO]
PHASE_1_ZERO = [ARMS_ON_CUBE_ZERO, LIFT_CUBE_ZERO]
PHASE_2_ZERO = []
PHASE_3_ZERO = [RELEASE_CUBE_ZERO]
PHASE_4_ZERO = [ARMS_UP_OUT_THE_WAY, ARMS_UP_OUT_THE_WAY_WAIT, ARMS_CLOSED_PROTECTED]
PHASES_ZERO = [PHASE_0_ZERO, PHASE_1_ZERO, PHASE_2_ZERO, PHASE_3_ZERO, PHASE_4_ZERO]
PHASE_0_90 = [ARMS_WIDE_POSTIVE]
PHASE_1_90 = [ARMS_ON_CUBE_90, LIFT_CUBE_90]
PHASE_2_90 = [TURN_CUBE_90]
PHASE_3_90 = [DOWN_CUBE_90, RELEASE_CUBE_90]
PHASE_4_90 = [ARMS_UP_OUT_THE_WAY, ARMS_UP_OUT_THE_WAY_WAIT, ARMS_CLOSED_PROTECTED]
PHASES_90 = [PHASE_0_90, PHASE_1_90, PHASE_2_90, PHASE_3_90, PHASE_4_90]
PHASE_0_180 = [ARMS_WIDE_POSTIVE]
PHASE_1_180 = [ARMS_ON_CUBE_90, LIFT_CUBE_90, TURN_CUBE_90, DOWN_CUBE_90, RELEASE_CUBE_90, PREPARE_SECOND_ROTATE_180, ARMS_ON_CUBE_180_FAST, LIFT_CUBE_90]
PHASE_2_180 = [TURN_CUBE_90]
PHASE_3_180 = [DOWN_CUBE_90, RELEASE_CUBE_90]
PHASE_4_180 = [ARMS_UP_OUT_THE_WAY, ARMS_UP_OUT_THE_WAY_WAIT, ARMS_CLOSED_PROTECTED]
PHASES_180 = [PHASE_0_180, PHASE_1_180, PHASE_2_180, PHASE_3_180, PHASE_4_180]
PHASE_0_NEGATIVE_90 = [ARMS_WIDE_NEGATIVE]
PHASE_1_NEGATIVE_90 = [ARMS_ON_CUBE_NEGATIVE_90, LIFT_CUBE_NEGATIVE_90]
PHASE_2_NEGATIVE_90 = [TURN_CUBE_NEGATIVE_90]
PHASE_3_NEGATIVE_90 = [DOWN_CUBE_NEGATIVE_90, RELEASE_CUBE_NEGATIVE_90]
PHASE_4_NEGATIVE_90 = [ARMS_UP_OUT_THE_WAY, ARMS_UP_OUT_THE_WAY_WAIT, ARMS_CLOSED_PROTECTED]
PHASES_NEGATIVE_90 = [PHASE_0_NEGATIVE_90, PHASE_1_NEGATIVE_90, PHASE_2_NEGATIVE_90, PHASE_3_NEGATIVE_90, PHASE_4_NEGATIVE_90]
PHASES = {0: PHASES_ZERO, 90: PHASES_90, 180: PHASES_180, - 90: PHASES_NEGATIVE_90}
LIFT_ZERO_TIME = getSequenceTime(PHASE_1_ZERO) - LIFT_TIME_INDENT
LIFT_90_TIME = getSequenceTime(PHASE_1_90) - LIFT_TIME_INDENT
LIFT_180_TIME = getSequenceTime(PHASE_1_180) - LIFT_TIME_INDENT + 1 #because sequence is so long 1 is needed due to waiting between moves
LIFT_NEGATIVE_90_TIME = getSequenceTime(PHASE_1_NEGATIVE_90) - LIFT_TIME_INDENT
LIFT_TIMES = {0: LIFT_ZERO_TIME, 90: LIFT_90_TIME, 180: LIFT_180_TIME, - 90: LIFT_NEGATIVE_90_TIME}
DOWN_ZERO_TIME = getSequenceTime(PHASE_3_ZERO)
DOWN_90_TIME = getSequenceTime(PHASE_3_90)
DOWN_NEGATIVE_90_TIME = getSequenceTime(PHASE_3_NEGATIVE_90)
DOWN_180_TIME = getSequenceTime(PHASE_3_180)
DOWN_TIMES = {0: DOWN_ZERO_TIME, 90: DOWN_90_TIME, 180: DOWN_180_TIME, - 90: DOWN_NEGATIVE_90_TIME}