Programa metafit robotica

10 'PROGRAMA IDENTIFICACION DE COLOR

20 mov P99
30 dly 1
40 hopen 1
50 dly 1.5
60 spd 20
70 DEF IO PART_AV = BIT,3 'SENSOR OPTICO DE PRESENCIA'
80 DEF IO B1 = BIT,1 'SENSOR OPTICO DE COLOR DE LA PIEZA'
90 DLY 1
100 HOPEN 1
110 DLY 1.5
120 IF PART_AV = 1 THEN 130 ELSE 90
130 MOV P01,-30
140 dly 1
150 SPD 10
160 MOV P01
170 dly 1.5
180 HCLOSE 1
190 dly 1.5
200 MOV P01,-30
210 SPD 20
220 MOV P99
230 dly 1
240 spd 20
250 MOV P02,-30
260 dly 1
270 SPD 10
280 MOV P02
290 dly 1
300 HOPEN 1
310 DLY 1.5
320 spd 10
330 MOV P02,-30
340 spd 5
350 mov P03, -30
360 dly 1
370 MOV P03
380 DLY 2
390 IF B1=1 THEN 400 ELSE 420
400 GOSUB *ROJO
410 GOTO 90
420 GOSUB *NEGRO
430 GOTO 90
440 '440 END
450 *ROJO
460 mov P03, -30
470 dly 1
480 MOV P02,-30
490 dly 1
500 SPD 10
510 mov P02
520 dly 2
530 HCLOSE 1
540 DLY 1.5
550 MOV P02,-30
560 dly 1
570 SPD 20
580 mov P99
590 DLY 1
600 MOV P04,-30
610 DLY 1
620 SPD 10
630 MOV P04
640 DLY 1
650 HOPEN 1
660 DLY 1.5
670 MOV P04,-30
680 DLY 1.0
690 SPD 20
700 MOV P99
710 RETURN
720 *NEGRO
730 mov P03, -30
740 dly 1
750 MOV P02,-30
760 DLY 1
770 SPD 10
780 MOV P02
790 DLY 1
800 HCLOSE 1
810 DLY 1.5
820 MOV P02,-30
830 DLY 1
840 SPD 20
850 MOV P99
860 dly 1
870 MOV P05,-30
880 DLY 1.0
890 SPD 10
900 MOV P05
910 DLY 1.0
920 hopen 1
930 dly 1.5
940 mov P05, -30
950 dly 1
960 spd 20
970 mov P99
980 RETURN
990 END