SHOW:
|
|
- or go back to the newest paste.
| 1 | ' {$STAMP BS2}
| |
| 2 | ' {$PBASIC 2.5}
| |
| 3 | ' LineFollowWithCheckQtis.bs2 | |
| 4 | ' Navigates based on values acquired with the | |
| 5 | ' Ceck_Qtis subroutine. | |
| 6 | ||
| 7 | ||
| 8 | - | acoustic VAR Byte |
| 8 | + | |
| 9 | - | counter VAR Word |
| 9 | + | acoustic VAR Nib |
| 10 | OUTB = %1111 ' Set OUTB bits to 1 | |
| 11 | ||
| 12 | - | PAUSE 1000 |
| 12 | + | |
| 13 | - | DEBUG CLS |
| 13 | + | |
| 14 | IF (IN14 = 0) AND (IN15 = 0) THEN | |
| 15 | - | DO |
| 15 | + | |
| 16 | GOSUB Blink_LED | |
| 17 | - | DEBUG HOME, "IN1 = ", BIN IN1 |
| 17 | + | |
| 18 | - | PAUSE 100 |
| 18 | + | |
| 19 | GOSUB Blink_LED | |
| 20 | ELSEIF (IN15 = 0) THEN | |
| 21 | - | IF (IN1 = 1) THEN |
| 21 | + | |
| 22 | - | endif |
| 22 | + | |
| 23 | ELSE | |
| 24 | GOSUB Check_Qtis | |
| 25 | - | FREQOUT 10, 3000, 3000 |
| 25 | + | |
| 26 | ||
| 27 | GOSUB Check_Qtis ' Get QTI states | |
| 28 | ||
| 29 | SELECT qtis ' Control servo | |
| 30 | ' speeds/directions | |
| 31 | CASE %1000 ' Rotate right | |
| 32 | PULSOUT 13, 650 | |
| 33 | PULSOUT 12, 650 | |
| 34 | CASE %1100 ' Pivot right | |
| 35 | PULSOUT 13, 750 | |
| 36 | PULSOUT 12, 650 | |
| 37 | CASE %0100 ' Curve right | |
| 38 | PULSOUT 13, 800 | |
| 39 | PULSOUT 12, 650 | |
| 40 | CASE %0110 ' Straight ahead | |
| 41 | PULSOUT 13, 850 | |
| 42 | PULSOUT 12, 650 | |
| 43 | CASE %0010 ' Curve left | |
| 44 | PULSOUT 13, 850 | |
| 45 | PULSOUT 12, 700 | |
| 46 | CASE %0011 ' Pivot left | |
| 47 | PULSOUT 13, 850 | |
| 48 | PULSOUT 12, 750 | |
| 49 | CASE %0001 ' Rotate left | |
| 50 | PULSOUT 13, 850 | |
| 51 | PULSOUT 12, 850 | |
| 52 | CASE %0000 | |
| 53 | PULSOUT 13, 800 | |
| 54 | PULSOUT 12, 800 | |
| 55 | PAUSE 40 | |
| 56 | PULSOUT 13, 600 | |
| 57 | - | CASE %1111 |
| 57 | + | |
| 58 | PAUSE 40 | |
| 59 | CASE ELSE ' Do nothing | |
| 60 | PAUSE 3 | |
| 61 | ENDSELECT | |
| 62 | ||
| 63 | LOOP | |
| 64 | ||
| 65 | Check_Qtis: | |
| 66 | ||
| 67 | ' Result -> qtis variable. | |
| 68 | ' 0 means white surface | |
| 69 | ' 1 means black surface. | |
| 70 | ||
| 71 | DIRB = %1111 ' P7..P4 -> output | |
| 72 | PAUSE 0 ' Delay = 230 us | |
| 73 | DIRB = %0000 ' P7..P4 -> input | |
| 74 | PAUSE 0 ' Delay = 230 us | |
| 75 | qtis = INB ' Store QTI output | |
| 76 | ' in INB | |
| 77 | RETURN | |
| 78 | ||
| 79 | Acoustic_Turn: | |
| 80 | ||
| 81 | FOR acoustic = 1 TO 80 | |
| 82 | PULSOUT 13, 650 | |
| 83 | PULSOUT 12, 650 | |
| 84 | PAUSE 20 | |
| 85 | NEXT | |
| 86 | END | |
| 87 | RETURN | |
| 88 | Blink_LED: | |
| 89 | ||
| 90 | HIGH 0 | |
| 91 | PAUSE 500 | |
| 92 | LOW 0 | |
| 93 | PAUSE 500 | |
| 94 | HIGH 0 | |
| 95 | PAUSE 500 | |
| 96 | LOW 0 | |
| 97 | PAUSE 500 | |
| 98 | - | FOR acoustic = 1 TO 66 |
| 98 | + | HIGH 0 |
| 99 | PAUSE 500 | |
| 100 | LOW 0 | |
| 101 | PAUSE 500 | |
| 102 | HIGH 0 | |
| 103 | PAUSE 500 | |
| 104 | LOW 0 | |
| 105 | PAUSE 500 | |
| 106 | - | FOR counter = 1 TO 5 |
| 106 | + | HIGH 0 |
| 107 | - | HIGH 0 |
| 107 | + | PAUSE 500 |
| 108 | - | PAUSE 500 |
| 108 | + | LOW 0 |
| 109 | - | LOW 0 |
| 109 | + | PAUSE 500 |
| 110 | - | PAUSE 500 |
| 110 | + | END |
| 111 | RETURN |
