10 'Set all 8255 ports to output mode

20 POKE &H7FFF,128

30 'Set servo positions before enableing them

40 GOSUB 70:GOTO 210

50 '

60 'Set Arm to home position

70 S1=128:S2=105

80 S3=124:S4=179

90 S5=168:S6=60

100 S7=110

110 SERVO 1,S1

120 SERVO 2,S2

130 SERVO 3,S3

140 SERVO 4,S4

150 SERVO 5,S5

160 SERVO 6,S6

170 SERVO 7,S7

180 RETURN

190 '

200 'Set hardware address for servos

210 SERVO 0,&H7FFD

220 'Define GameBoy Button bits

230 R=1:L=2:U=4

240 D=8:A=16:B=32

250 S=64:T=128

260 Z=2

270 'Display currently selected servo

280 GOSUB 530

290 '

300 'Main Control Loop

310 X=0:Y=0

320 IF KEYPAD(U)=1 THEN Y=1

330 IF KEYPAD(D)=1 THEN Y=-1

340 IF KEYPAD(L)=1 THEN X=-1

350 IF KEYPAD(R)=1 THEN X=1

360 IF KEYPAD(S)=1 THEN GOSUB 510

370 IF KEYPAD(A)=1 THEN S1=128:S3=124:SERVO 1,S1:SERVO 3,S3

380 IF KEYPAD(B)=1 THEN S1=128:S3=124:SERVO 1,S1:SERVO 3,S3

390 IF ABS(X)+ABS(Y)=0 THEN 310

400 '

410 'Only control currently selected servo

420 IF Z=2 THEN S2=S2+X:SERVO 2,S2

430 IF Z=3 THEN S1=S1-X-Y:S3=S3-X+Y:SERVO 1,S1:SERVO 3,S3

440 IF Z=4 THEN S4=S4+Y:SERVO 4,S4

450 IF Z=5 THEN S5=S5-Y:SERVO 5,S5

460 IF Z=6 THEN S6=S6+Y:SERVO 6,S6

470 IF Z=7 THEN S7=S7+X+Y:SERVO 7,S7

480 GOTO 310

490 '

500 'Select another servo to control

510 Z=Z+1

520 IF Z=8 THEN Z=2

530 IF Z=2 THEN PRINT "Base Rotate"

540 IF Z=3 THEN PRINT "Wheels"

550 IF Z=4 THEN PRINT "Shoulder"

560 IF Z=5 THEN PRINT "Elbow"

570 IF Z=6 THEN PRINT "Wrist"

580 IF Z=7 THEN PRINT "Hand"

590 DELAY 25

600 RETURN