' {$STAMP BS2} ' {$PBASIC 2.5} '===== Variabelen & constanten ======================================================================================== constante CON 2260 a VAR Bit 'variabele a voor linker kleurensensor b VAR Bit 'variabele b voor rechter kleurensensor afstand VAR Word 'De afstand tot een object tijd VAR Word roodL VAR Word groenL VAR Word blauwL VAR Word kleurL VAR Word roodR VAR Word groenR VAR Word blauwR VAR Word kleurR VAR Word teller VAR Word sio1 PIN 14 'Seriële in/uitgang Links sio2 PIN 15 'Seriële in/uitgang Rechts baud CON 119 + 32768 '119+32768 Qtis VAR Nib ' zwart/wit status OUTB = %1111 ' zet OUTB bits naar 1 '===== Hoofdprogramma ================================================================================================= DO ' Hoofd DO... LOOP GOSUB Afstandsmeter ' Meet de afstand tot een object GOSUB Check_Qtis ' verkrijg QTI status GOSUB Lijnvolger GOSUB Kleurdetectie ' Verkrijg kleuren info van de sensoren ' DEBUG "A",DEC1 a, "B", DEC1 b, HOME ' DEBUG CR, DEC3 afstand, " cm" LOOP '===== Subroutines ==================================================================================================== Check_Qtis: ' Resultaat -> qtis variable ' 0 betekent een wit oppervlak ' 1 betekent een zwart oppervlak DIRB = %1111 ' P7...P4 -> output PAUSE 0 ' Delay = 230 us DIRB = %0000 ' P7...P4 -> input PAUSE 0 ' Delay = 230 us Qtis = INB ' Sla QTI outputs op in INB RETURN '---------------------------------------------------------------------------------------- Lijnvolger: GOSUB Check_Qtis GOSUB Afstandsmeter ' Verkrijg kleuren info van de sensoren SELECT Qtis ' Controleer servo snelheden/richtingen CASE %1000 ' Draai rechts PULSOUT 12, 650 PULSOUT 13, 650 CASE %1100 ' Draaipunt rechts PULSOUT 12, 750 PULSOUT 13, 650 CASE %0100 ' Buig rechts PULSOUT 12, 800 PULSOUT 13, 650 CASE %0110 ' Rechtdoor PULSOUT 12, 850 PULSOUT 13, 650 CASE %0010 ' Draai links PULSOUT 12, 850 PULSOUT 13, 700 CASE %0011 ' Draaipunt links PULSOUT 12, 850 PULSOUT 13, 750 CASE %0001 ' Buig links PULSOUT 12, 850 PULSOUT 13, 850 CASE ELSE ' Doe niks PAUSE 0 ENDSELECT GOSUB Kleurdetectie IF afstand <=016 THEN GOSUB Ontwijken RETURN '------------------------------------------------------------------------------------------ Afstandsmeter: PULSOUT 9, 5 PULSIN 9, 1, tijd afstand = constante ** tijd ' DEBUG HOME, DEC3 afstand, " cm" RETURN '------------------------------------------------------------------------------------------ Ontwijken: FOR teller = 1 TO 16 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT GOSUB check_qtis DO WHILE (Qtis=%0000) GOSUB check_qtis PULSOUT 12, 850 ' Veer right PULSOUT 13, 740 PAUSE 20 LOOP RETURN '------------------------------------------------------------------------------------------ Kleurdetectie: DO 'DEBUG "A",DEC1 a, "B", DEC1 b, CR ' Laat de kleur status zien 0 = wit oppervlak; 1 = groen oppervlak GOSUB sensor_L ' Verkrijg de kleurstatus van de linker sensor GOSUB sensor_R ' Verkrijg de kleurstatus van de rechter sensor IF a=1 AND b=0 THEN GOSUB Afdraaien_L ' Als enkel de linker sensor op groen staat naar de subroutine voor links afdraaien gaan ELSEIF a=0 AND b=1 THEN GOSUB Afdraaien_R ' Als enkel de linker sensor op groen staat naar de subroutine voor links afdraaien gaan ELSEIF a=1 AND b=1 THEN GOSUB Blikdetectie ELSE GOTO lijnvolger ENDIF RETURN LOOP '----------------------------------------------------------------------------------------------------------------------------------- Sensor_L: GOSUB ResetL ' Reset de ColorPAL FOR teller = 1 TO 2 SEROUT sio1, baud, ["= (00 $ m) !"] SERIN sio1, baud, [WAIT("$"), HEX3 roodL, HEX3 groenL, HEX3 blauwL] ' Verkrijg de RGB data van de sensor ' DEBUG "R", DEC4 roodL, " G", DEC4 groenL, " B", DEC4 blauwL, CR ' Laat de RGB waarde zien ' DEBUG sio1, CR kleurL= roodL+groenL+blauwL IF kleurL <= 0300 THEN a=1 ELSE a=0 ENDIF NEXT RETURN ResetL: ' Reset: zend een lange stop naar de ColorPAL LOW sio1 ' Zet Soi1 op laag zodat de restspanning volledig verdwijnt INPUT sio1 ' Zet PIN terug op input DO UNTIL sio1 : LOOP ' Wacht tot de ColoPAL de pin op hoog zet LOW sio1 ' Zet de pin op laag PAUSE 50 ' Houdt de pin laag voor 80ms voor naar directe modus over te schakelen INPUT sio1 ' Zet pin op input PAUSE 10 ' Wacht nog 10ms RETURN '-------------------------------------------------------------------------------------------------------------------------------------- Sensor_R: GOSUB ResetR ' Reset de ColorPAL FOR teller = 1 TO 2 SEROUT sio2, baud, ["= (00 $ m) !"] SERIN sio2, baud, [WAIT("$"), HEX3 roodR, HEX3 groenR, HEX3 blauwR] ' Verkrijg de RGB data van de sensor 'DEBUG "R", DEC4 roodR, " G", DEC4 groenR, " B", DEC4 blauwR, CR ' Output it. 'EBUG sio2, CR kleurR= roodR+groenR+blauwR IF kleurR <= 300 THEN b=1 ELSE b=0 ENDIF NEXT RETURN ResetR: ' Reset: zend een lange stop naar de ColorPAL LOW sio2 ' Zet Soi2 op laag zodat de restspanning volledig verdwijnt INPUT sio2 ' Zet PIN terug op input DO UNTIL sio2 : LOOP ' Wacht tot de ColoPAL de pin op hoog zet LOW sio2 ' Zet de pin op laag PAUSE 50 ' Houdt de pin laag voor 80ms voor naar directe modus over te schakelen INPUT sio2 ' Zet pin op input PAUSE 10 ' Wacht nog 10ms RETURN '-------------------------------------------------------------------------------------------------------------------------- Afdraaien_L: 'Draai 90° naar links FOR teller = 1 TO 48 PULSOUT 12, 700 PULSOUT 13, 650 PAUSE 20 NEXT RETURN '-------------------------------------------------------------------------------------------------------------------------- Afdraaien_R: ' Draai 90° naar Rechts FOR teller = 1 TO 48 PULSOUT 12, 850 PULSOUT 13, 700 PAUSE 20 NEXT RETURN '-------------------------------------------------------------------------------------------------------------------------- Blikdetectie: FOR teller=1 TO 30 PULSOUT 12, 650 PULSOUT 13, 650 PAUSE 20 NEXT GOSUB Zoek_blik '------------------------------------------------------------------------------------------------------------------------ Zoek_blik: PULSOUT 9, 5 PULSIN 9, 1, tijd afstand = constante ** tijd DO WHILE afstand > 045 PULSOUT 12, 850 PULSOUT 13, 850 PULSOUT 9, 5 PULSIN 9, 1, tijd afstand = constante ** tijd DEBUG HOME, DEC3 afstand, " cm" LOOP FOR teller = 1 TO 2 PULSOUT 12, 850 PULSOUT 13, 850 PAUSE 15 NEXT FOR teller = 1 TO 100 PULSOUT 12, 850 PULSOUT 13, 650 PAUSE 20 NEXT RETURN