'Declare variables
x1 VAR BYTE
x2 VAR BYTE
y1 VAR BYTE 'The movement is adjusted by varying the Y1 AND Y2 numbers in each movement.
y2 VAR BYTE
lp VAR BYTE

'Declare array
b VAR BYTE [12]
Initialize array variables
b(0) = 148 'Right ankle’ (vertical)
b(1) = 121 'Right ankle’ (horizontal)
b(2) = 204 'Right knee’
b(3) = 126 'Right hip’
b(4) = 150 'Left ankle’ (vertical)
b(5) = 178 'Left ankle’ (horizontal)
b(6) = 101 'Left knee’
b(7) = 180 'Left hip’
b(8) = 0 'counter’
b(9) = 0 'counter’
b(10) = 0 'counter’
b(11) = 0 'Dummy value’



Start:
'Holding loop that holds upright position 3 seconds before moving

b(8) = b(8) + 1
GoSub servoout
IF b(8) < 180 Then GoTo start
b(8) = 0 'Reset the loop counter’
'__________________
For b(10) = 1 TO 3 'Take 3 steps forward’
'__________________
'Leg movement For one whole step
'__________________

'First movement
x1 = 0 'Servomotor 0
x2 = 4 'Servomotor 4
y1 = 129 'Tilt right ankle (horizontal)
y2 = 135 'Tilt left ankle (horizontal)
lp = 106 'Loop counter
GoSub m1
'___________________

'Second movement
x1 = 5 'Servomotor 5
x2 = 6 'Servomotor 6
y1 = 2 'Left ankle (vertical)
y2 = 70 'left knee
lp = 140 'Loop counter
GoSub m3
'___________________

'Third movement
x1 = 4 'Servomotor 4
x2 = 7 'Servomotor 7
y1 = 150 'Left ankle
y2 = 160 'Left hip
lp = 75 'Loop counter
GoSub m3
'___________________

'Fourth movement
x1 = 1 'Servomotor 1
x2 = 5 'Servomotor 5
y1 = 132 'Straighten ankle
y2 = 200 'Straighten ankle
lp = 90 'Loop counter
GoSub m3
'___________________

'Fifth movement
x1 = 6 'Servomotor 6
x2 = 0 'Servomotor 0
y1 = 85 'Left knee
y2 = 140 'Straighten ankle
lp = 96 'Loop counter
GoSub m2
'___________________


'Sixth movement
x1 = 0 'Servomotor 0
x2 = 4 'Servomotor 4
y1 = 167 'Tilt right ankle
y2 = 169 'Tilt left ankle
lp = 80 'Loop counter
GoSub m2
'___________________

'Seventh movement
x1 = 6 'Servomotor 6
x2 = 5 'Servomotor 5
y1 = 101 'Tilt knee
y2 = 178 'Tilt ankle
lp = 95 'Loop counter
GoSub m3
'___________________

'Eighth movement
x1 = 2 'Servomotor 2
x2 = 1 'Servomotor 1
y1 = 244 'Right ankle
y2 = 104 'Right knee
lp = 140 'Loop counter
GoSub m3
'___________________


'Ninth movement
x1 = 3 'Servomotor 3
x2 = 7 'Servomotor 7
y1 = 140 'Right hip
y2 = 180 'Left hip
lp = 80 'Loop counter
GoSub m2
'___________________

'Tenth movement
x1 = 1 'Servomotor 1
x2 = 2 'Servomotor 2
y1 = 121 'Right ankle
y2 = 204 'Right knee
lp = 150 'Loop counter
GoSub m3
'___________________

'Eleventh movement
x1 = 0 'Servomotor 0
x2 = 4 'Servomotor 4
y1 = 121 'Straighten right ankle
y2 = 204 'Straighten left ankle
lp = 150 'Loop counter
GoSub m1
'___________________

'Twelfth movement
x1 = 5 'Servomotor 5
x2 = 6 'Servomotor 6
y1 = 217 'Left ankle
y2 = 70 'left knee
lp = 144 'Loop counter
GoSub m3
'___________________

'Thirteenth movement
x1 = 4 'Servomotor 4
x2 = 3 'Servomotor 3
y1 = 133 'Left ankle
y2 = 126 'Right hip
lp = 66 'Loop counter
GoSub m1
'___________________

'Fourteenth movement
x1 = 6 'Servomotor 6
x2 = 5 'Servomotor 5
y1 = 101 'Left knee
y2 = 178 'Left ankle
lp = 144 'Loop counter
GoSub m3
‘___________________


'Fifteenth movement
x1 = 0 'Servomotor 0
x2 = 4 'Servomotor 4
y1 = 148 'Left knee
y2 = 150 'Left ankle
lp = 115 'Loop counter
GoSub m2
'___________________
Next b(10) 'Next Step
'___________________

Hold:
GoSub servoout
GoTo hold

‘____________________

Servoout:
'Output servomotor position(s)
PORTB = 0 'Prevent signal inversion

Right Leg
PulsOut PORTB.0, b(0) 'Send current servo 1 position out
PulsOut PORTB.1, b(1) 'Send current servo 2 position out
PulsOut PORTB.2, b(2) 'Send current servo 3 position out
PulsOut PORTB.3, b(3) 'Send current servo 4 position out

Left Leg
PulsOut PORTB.4, b(4) 'Send current servo 5 position out
PulsOut PORTB.5, b(5) 'Send current servo 6 position out
PulsOut PORTB.6, b(6) 'Send current servo 7 position out
PulsOut PORTB.7, b(7) 'Send current servo 8 position out
Pause 5 '5 milisescond delay to generate 50Hz signal
Return 'To servomotors
‘_____________________

'The delay routines
m1: '(Negative increment (s) -, -)
b(8) = b(8) + 1
IF b(9) = 2 Then m12
b(9) = b(9) + 1
GoTo m13

m12:
b(x1) = b(x1) - 1
b(x2) = b(x2) -1
b(9) = 0

m13:
IF b(x1) < y1 Then
b(x1) = y1
EndIF
IF b(x2) < y2 Then
b(x2) = y2
EndIF
GoSub servoout
IF b(8) < lp Then m1
b(x1) = y1
b(x2) = y2
b(8) = 0
b(9) = 0
Return
'______________________
M2: '(Positive increment (s) +, +)
b(8) = b(8) + 1
IF b(9) = 2 Then m22
b(9) = b(9) + 1
GoTo m23

m22:
b(x1) = b(x1) + 1
b(x2) = b(x2) + 1
b(9) = 0

m23:
IF b(x1) > y1 Then
b(x1) = y1
EndIF
IF b(x2) > y2 Then
b(x2) = y2
EndIF
GoSub servoout
IF b(8) < lp Then m2
b(x1) = y1
b(x2) = y2
b(8) = 0
b(9) = 0
Return
'______________________

M3: '(Positive-Negative increment (s) +, -)
b(8) = b(8) + 1
IF b(9) = 2 Then m32
b(9) = b(9) + 1
GoTo m33

m32:
b(x1) = b(x1) + 1
b(x2) = b(x2) -1
b(9) = 0

m33:
IF b(x1) > y1 Then
b(x1) = y1
EndIF
IF b(x2) < y2 Then
b(x2) = y2
EndIF
GoSub servoout
IF b(8) < lp Then m3
b(x1) = y1
b(x2) = y2
b(8) = 0
b(9) = 0
Return
'______________________