#square2.bas
new
10 b$="1": rem b$ = Board ID
20 rem s = Servo number
30 rem p = Servo position
50 open "com1:", 9600 as #5
100 rem cycle thru motors @ stop
110 gosub 1200: rem stop servos
150 gosub 1000: rem wait
200 rem move forward
210 s = 2: p = 255: rem servo 2 full "cw"
220 gosub 1100: rem servo position
230 s = 3: p = 1: rem servo 3 full "ccw"
240 gosub 1100: rem servo position
250 gosub 1000: rem wait
260 gosub 1200: rem stop servos
270 gosub 1000: rem wait
300 rem sideways - left
310 s = 1: v = 1: rem servo 1 full "cw"
315 gosub 1500: rem servo speed
320 gosub 1100: rem servo position
330 s = 2: v = -.5: rem servo 2 @ "ccw"
335 gosub 1500: rem servo speed
340 gosub 1100: rem servo position
345 s = 3: v = -.5: rem servo 3 @ "ccw"
350 gosub 1500: rem servo speed
360 gosub 1100: rem servo position
370 gosub 1000: rem wait
380 gosub 1200: rem stop servos
390 gosub 1000: rem wait
400 rem move backward
410 s = 2: p = 1: rem servo 2 full "cw"
420 gosub 1100: rem servo position
430 s = 3: p = 255: rem servo 3 full "ccw"
440 gosub 1100: rem servo position
450 gosub 1000: rem wait
460 gosub 1200: rem stop servos
470 gosub 1000: rem wait
500 rem sideways - right
510 s = 1:  v= -1: rem servo 1 "ccw"
515 gosub 1500: rem servo speed
520 gosub 1100: rem servo position
530 s = 2: v = .5: rem servo 2 @½ "cw"
535 gosub 1500: rem servo speed
540 gosub 1100: rem servo position
545 s = 3: v = .5: rem servo 3 @½ "cw"
550 gosub 1500: rem servo speed
560 gosub 1100: rem servo position
570 gosub 1000: rem wait
580 gosub 1200: rem stop servos
590 rem gosub 1000: rem wait
900 goto 2000: rem close & end
1000 rem wait subroutine
1010 for w=1 to 5000: next w:
1020 return
1100 rem servo position subroutine
1110 print #5, "BD" + b$ + "SV" + str$(s) + "M" + str$(p)
1150 return
1200 rem all motors stop
1210 p = 0: rem servo stop
1220 for s=1 to 3: gosub 1100: next s
1230 return
1500 rem speed calc.
1550 p = 39.8053 * v^3  - 12.6083 * v^2 + 22.1197 * v + 128.4262
1590 return
2000 close #5: end
run