;
; Robot command file for stacking/restacking 3 blocks
; Only suitable for use with robot Hera
;
; define home positions
;
L [ A -14, 20, 79, 55, -73, 45, 1 ]
L [ A -13, 23, 80, 54, -73, 45, 2 ]
L [ A -13, 23, 80, 54, -73, 22, 3 ]
L [ A -13, 12, 80, 55, -73, 22, 4 ]
L [ A 17, 12, 79, 54, -73, 22, 5 ]
L [ A 18, 29, 79, 54, -95, 22, 6 ]
L [ A 18, 29, 79, 55, -95, 45, 7 ]
L [ A 19, 19, 80, 55, -95, 45, 8 ]
L [ A -13, 26, 79, 54, -73, 45, 9 ]
L [ A -13, 26, 79, 54, -73, 22, 10 ]
L [ A -14, 21, 79, 54, -73, 22, 11 ]
L [ A 18, 24, 81, 55, -91, 22, 12 ]
L [ A 17, 24, 82, 54, -91, 22, 13 ]
L [ A 18, 24, 82, 55, -91, 45, 14 ]
L [ A 17, 19, 82, 54, -91, 45, 15 ]
L [ A -14, 28, 81, 53, -73, 45, 16 ]
L [ A -14, 28, 81, 53, -73, 22, 17 ]
L [ A -14, 18, 81, 53, -73, 22, 18 ]
L [ A 18, 19, 85, 53, -84, 22, 19 ]
L [ A 17, 20, 85, 53, -84, 22, 20 ]
L [ A 18, 20, 85, 54, -84, 45, 21 ]
L [ A 17, 15, 85, 53, -84, 45, 22 ]
;
; macro to move top block of first stack to bottom of second stack
;
A = { H[1] H[2] H[3] H[4] H[5] H[6] H[7] H[8] } 
; 
; macro to move mid block of first stack to mid of second stack
;
U = { H[1] H[9] H[10] H[11] H[12] H[13] H[14] H[15] } 
;
; macro to move bottom block of first stack to top of second stack
;
V = { H[1] H[16] H[17] H[18] H[19] H[20] H[21] H[22] } 
;
; macro to home robot, move blocks from first to second stack
; and then home robot again
;
M = { H[0] %A %U %V H[0] } 
;
; macro to move top block of second stack to bottom of first stack
;
F = { H[22] H[21] H[20] H[19] H[18] H[17] H[16] H[1] } 
;
; macro to move mid block of second stack to mid of first stack
;
J = { H[15] H[14] H[13] H[12] H[11] H[10] H[9] H[1] } 
;
; macro to move bottom block of second stack to top of first stack
;
K = { H[8] H[7] H[6] H[5] H[4] H[3] H[2] H[1] } 
;
; macro to home robot, move blocks from second to first stack
; and then home robot again
;
N = { H[0] %F %J %K H[0] } 
;
; macro to perform stack/unstack sequence
;
S = { %M %N }
;
; move robot to home position
;
P[A0,10,15,0,2,25] 
