Problem with comparison.
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.BAK |
|---|
| 0,0 → 1,158 |
| #include ".\cidla.h" |
| //#include <stdlib.h> |
| #use rs232(baud=9600,parity=N,xmit=PIN_B3,bits=8,restart_wdt) |
| #define IRRX PIN_B0 |
| #define TRESHOLD_MAX 70 // rozhodovaci uroven pro cidla cerna/bila |
| #define TRESHOLD_MIN 50 |
| #define CIHLA 10 // doba, po kterou musi byt detekovana cihla |
| unsigned int8 radius; // co cidla vidi |
| unsigned int8 last_radius; // rozsah |
| unsigned int8 last_cidla; // co cidla videla minule |
| unsigned int8 shure; // citac doby, po kterou musi byt detekovana cihla |
| //tuning |
| /*#define PULZACE 3 // urcuje rychlost pulzovani pomoci PWM |
| //Vystup PWM je na PIN_B3 |
| //////////////////////////////////////////////////////////////////////////////// |
| void pulzovani() // postupne rozsvecuje a zhasina podsvetleni |
| { |
| unsigned int8 i,n; |
| for(n=0;n<=3;n++) |
| { |
| for(i=0;i<255;i++) {set_pwm1_duty(i); Delay_ms(PULZACE);} // rozsvecovani |
| for(i=255;i>0;i--) {set_pwm1_duty(i); Delay_ms(PULZACE);} // zhasinani |
| } |
| } |
| */ |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| int8 cidla; |
| unsigned int8 a; |
| unsigned int8 n; |
| setup_adc_ports(sAN0|sAN1|sAN2|sAN3|sAN4|sAN5|sAN6|VSS_VDD); |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED); |
| setup_timer_2(T2_DISABLED,0,1); |
| setup_comparator(NC_NC_NC_NC); |
| setup_vref(FALSE); |
| Delay_ms(500); |
| setup_spi(SPI_SLAVE|SPI_H_TO_L|SPI_SS_DISABLED); |
| // diagnostika |
| printf("\n\r"); |
| Delay_ms(100); |
| printf("***\n\r"); |
| Delay_ms(100); |
| for (n=0; n<=6; n++) |
| { |
| set_adc_channel(n); |
| Delay_ms(100); |
| a=read_adc(); |
| printf("sensor %u - %u\n\r",n,a); |
| } |
| shure=0; |
| while(true) |
| { |
| set_adc_channel(0); |
| cidla=0; |
| Delay_us(10); |
| a=read_adc(); |
| set_adc_channel(1); |
| if(a<TRESHOLD_MAX) //hystereze cidel |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000001); |
| } |
| else cidla |= 0b00000001; |
| } |
| a=read_adc(); |
| set_adc_channel(2); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000010); |
| } |
| else cidla |= 0b00000010; |
| } |
| a=read_adc(); |
| set_adc_channel(3); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000100); |
| } |
| else cidla |= 0b00000100; |
| } |
| a=read_adc(); |
| set_adc_channel(4); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00001000); |
| } |
| else cidla |= 0b00001000; |
| } |
| a=read_adc(); |
| set_adc_channel(5); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00010000); |
| } |
| else cidla |= 0b00010000; |
| } |
| a=read_adc(); |
| set_adc_channel(6); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00100000); |
| } |
| else cidla |= 0b00100000; |
| } |
| a=read_adc(); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |=(last_cidla & 0b01000000); |
| } |
| else cidla |= 0b01000000; |
| } |
| last_cidla=cidla; |
| if (!input(IRRX)) {if (shure<255) shure++;} else {shure=0;}; |
| if (shure>CIHLA) cidla|=0b10000000; |
| cidla=~cidla; |
| spi_write(cidla); |
| } |
| } |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.HEX |
|---|
| 0,0 → 1,72 |
| :1000000000308A00B12800000A108A100A11820705 |
| :100010000A340D3400340A108A100A1182072A3477 |
| :100020002A342A340A340D3400340A108A100A1192 |
| :100030008207733465346E3473346F347234203411 |
| :100040002534753420342D342034253475340A3465 |
| :100050000D34003427308400000803194028013093 |
| :10006000F800BF30F7006400F70B3328F80B312895 |
| :100070004A30F700F70B3A2800006400800B2F2865 |
| :10008000003483168611831286110830F8000000B0 |
| :1000900000000000F8175B28F813A90C0318861558 |
| :1000A000031C861178175B287813F80B4D28000085 |
| :1000B0000000000086151C308400840B5D280000C1 |
| :1000C00000006400F81B4C28781B542800342A08D0 |
| :1000D000F801290203186F282908F7007B28F70187 |
| :1000E0000830AB00A90DF70D2A0877020318F700B6 |
| :1000F000F80DAB0B7228003478082708A90064308B |
| :10010000AA0067207708A70078083030031D8F28E1 |
| :10011000A81C9628A8199628281A20309228A811D9 |
| :1001200028122814F8077808A90041202708A900F8 |
| :100130000A30AA0067207708A70078083030031D2E |
| :10014000A728A819AB28281CAB28281A2030F807A4 |
| :100150007808A90041203030A7072708A9004120CE |
| :10016000003484011F308305603083168F008611B0 |
| :100170008312861583161F129F121B0880399B005D |
| :1001800007309C001F129F121B0880397F389B008C |
| :100190001F1383121F179F1783169F1383121F1499 |
| :1001A00083160108C7390838810083129001003096 |
| :1001B000F800920000308316920007309C0005087A |
| :1001C00064000230F700F70BE328000000001C0871 |
| :1001D00083120D1383169D0102308312A600FA309C |
| :1001E000A7002A20A60BEF28941283160611861466 |
| :1001F0000616353083129400403083169400831223 |
| :10020000A60126080420003803190A29A60AA90015 |
| :10021000412001296430A7002A20A60126080B20CE |
| :10022000003803191729A60AA90041200E296430B5 |
| :10023000A7002A20A5012508063C031C5729250DE7 |
| :10024000F700F70DF70DF830F7051F08C7397704E9 |
| :100250009F006430A7002A201F151F192D291E0892 |
| :10026000A400A60126081520A60AF700A90041202F |
| :1002700007302602031D32292508A7001B30A800DD |
| :100280007C202030A90041202D30A90041202030C1 |
| :10029000A90041202408A7001B30A8007C200A30B8 |
| :1002A000A90041200D30A9004120A50A1B29A20167 |
| :1002B0000030F8001F08C73978049F00A3016400CC |
| :1002C0000230F700F70B6229000000001F151F190C |
| :1002D00067291E08A4000830F8001F08C7397804F1 |
| :1002E0009F002408453C031C7E292408323C031847 |
| :1002F0007D2921080139A3047E2923141F151F1904 |
| :100300007F291E08A4001030F8001F08C7397804A0 |
| :100310009F002408453C031C96292408323C0318FE |
| :10032000952921080239A3049629A3141F151F1922 |
| :1003300097291E08A4001830F8001F08C739780450 |
| :100340009F002408453C031CAE292408323C0318B6 |
| :10035000AD2921080439A304AE2923151F151F193F |
| :10036000AF291E08A4002030F8001F08C739780400 |
| :100370009F002408453C031CC6292408323C03186E |
| :10038000C52921080839A304C629A3151F151F195B |
| :10039000C7291E08A4002830F8001F08C7397804B0 |
| :1003A0009F002408453C031CDE292408323C031826 |
| :1003B000DD2921081039A304DE2923161F151F1972 |
| :1003C000DF291E08A4003030F8001F08C739780460 |
| :1003D0009F002408453C031CF6292408323C0318DE |
| :1003E000F52921082039A304F629A3161F151F1982 |
| :1003F000F7291E08A4002408453C031C082A2408E9 |
| :10040000323C0318072A21084039A304082A23177D |
| :100410002308A1008316061483120618142A220F3B |
| :10042000122A132AA20A152AA20122080A3C031C36 |
| :10043000A317A3092308930083161418212A8312F3 |
| :080440001C2A831258296300F5 |
| :04400E00383FFC3FFC |
| :00000001FF |
| ;PIC16F88 |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.LST |
|---|
| 0,0 → 1,620 |
| CCS PCM C Compiler, Version 3.245, 27853 05-XI-06 20:45 |
| Filename: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.lst |
| ROM used: 548 words (13%) |
| Largest free fragment is 2048 |
| RAM used: 12 (7%) at main() level |
| 17 (10%) worst case |
| Stack: 2 locations |
| * |
| 0000: MOVLW 00 |
| 0001: MOVWF 0A |
| 0002: GOTO 0B1 |
| 0003: NOP |
| .................... #include ".\cidla.h" |
| .................... #include <16F88.h> |
| .................... //////// Standard Header file for the PIC16F88 device //////////////// |
| .................... #device PIC16F88 |
| .................... #list |
| .................... |
| .................... #device adc=8 |
| .................... #fuses NOWDT,INTRC_IO, NOPUT, MCLR, NOBROWNOUT, NOLVP, NOCPD, NOWRT, NODEBUG, NOPROTECT, NOFCMEN, NOIESO |
| .................... #use delay(clock=4000000,RESTART_WDT) |
| * |
| 002A: MOVLW 27 |
| 002B: MOVWF 04 |
| 002C: MOVF 00,W |
| 002D: BTFSC 03.2 |
| 002E: GOTO 040 |
| 002F: MOVLW 01 |
| 0030: MOVWF 78 |
| 0031: MOVLW BF |
| 0032: MOVWF 77 |
| 0033: CLRWDT |
| 0034: DECFSZ 77,F |
| 0035: GOTO 033 |
| 0036: DECFSZ 78,F |
| 0037: GOTO 031 |
| 0038: MOVLW 4A |
| 0039: MOVWF 77 |
| 003A: DECFSZ 77,F |
| 003B: GOTO 03A |
| 003C: NOP |
| 003D: CLRWDT |
| 003E: DECFSZ 00,F |
| 003F: GOTO 02F |
| 0040: RETLW 00 |
| .................... |
| .................... |
| .................... //#include <stdlib.h> |
| .................... |
| .................... #use rs232(baud=9600,parity=N,xmit=PIN_B3,bits=8,restart_wdt) |
| 0041: BSF 03.5 |
| 0042: BCF 06.3 |
| 0043: BCF 03.5 |
| 0044: BCF 06.3 |
| 0045: MOVLW 08 |
| 0046: MOVWF 78 |
| 0047: NOP |
| 0048: NOP |
| 0049: NOP |
| 004A: BSF 78.7 |
| 004B: GOTO 05B |
| 004C: BCF 78.7 |
| 004D: RRF 29,F |
| 004E: BTFSC 03.0 |
| 004F: BSF 06.3 |
| 0050: BTFSS 03.0 |
| 0051: BCF 06.3 |
| 0052: BSF 78.6 |
| 0053: GOTO 05B |
| 0054: BCF 78.6 |
| 0055: DECFSZ 78,F |
| 0056: GOTO 04D |
| 0057: NOP |
| 0058: NOP |
| 0059: NOP |
| 005A: BSF 06.3 |
| 005B: MOVLW 1C |
| 005C: MOVWF 04 |
| 005D: DECFSZ 04,F |
| 005E: GOTO 05D |
| 005F: NOP |
| 0060: NOP |
| 0061: CLRWDT |
| 0062: BTFSC 78.7 |
| 0063: GOTO 04C |
| 0064: BTFSC 78.6 |
| 0065: GOTO 054 |
| 0066: RETLW 00 |
| .................... |
| .................... #define IRRX PIN_B0 |
| .................... |
| .................... #define TRESHOLD_MAX 70 // rozhodovaci uroven pro cidla cerna/bila |
| .................... #define TRESHOLD_MIN 50 |
| .................... #define CIHLA 10 // doba, po kterou musi byt detekovana cihla |
| .................... |
| .................... unsigned int8 last_radius; // rozsah |
| .................... unsigned int8 last_cidla; // co cidla videla minule |
| .................... unsigned int8 shure; // citac doby, po kterou musi byt detekovana cihla |
| .................... |
| .................... //tuning |
| .................... /*#define PULZACE 3 // urcuje rychlost pulzovani pomoci PWM |
| .................... |
| .................... //Vystup PWM je na PIN_B3 |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| .................... void pulzovani() // postupne rozsvecuje a zhasina podsvetleni |
| .................... { |
| .................... unsigned int8 i,n; |
| .................... for(n=0;n<=3;n++) |
| .................... { |
| .................... for(i=0;i<255;i++) {set_pwm1_duty(i); Delay_ms(PULZACE);} // rozsvecovani |
| .................... for(i=255;i>0;i--) {set_pwm1_duty(i); Delay_ms(PULZACE);} // zhasinani |
| .................... } |
| .................... } |
| .................... */ |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| .................... void main() |
| .................... { |
| * |
| 00B1: CLRF 04 |
| 00B2: MOVLW 1F |
| 00B3: ANDWF 03,F |
| 00B4: MOVLW 60 |
| 00B5: BSF 03.5 |
| 00B6: MOVWF 0F |
| 00B7: BCF 06.3 |
| 00B8: BCF 03.5 |
| 00B9: BSF 06.3 |
| 00BA: BSF 03.5 |
| 00BB: BCF 1F.4 |
| 00BC: BCF 1F.5 |
| 00BD: MOVF 1B,W |
| 00BE: ANDLW 80 |
| 00BF: MOVWF 1B |
| 00C0: MOVLW 07 |
| 00C1: MOVWF 1C |
| .................... int8 cidla; |
| .................... unsigned int8 a; |
| .................... unsigned int8 n; |
| .................... |
| .................... setup_adc_ports(sAN0|sAN1|sAN2|sAN3|sAN4|sAN5|sAN6|VSS_VDD); |
| 00C2: BCF 1F.4 |
| 00C3: BCF 1F.5 |
| 00C4: MOVF 1B,W |
| 00C5: ANDLW 80 |
| 00C6: IORLW 7F |
| 00C7: MOVWF 1B |
| .................... setup_adc(ADC_CLOCK_INTERNAL); |
| 00C8: BCF 1F.6 |
| 00C9: BCF 03.5 |
| 00CA: BSF 1F.6 |
| 00CB: BSF 1F.7 |
| 00CC: BSF 03.5 |
| 00CD: BCF 1F.7 |
| 00CE: BCF 03.5 |
| 00CF: BSF 1F.0 |
| .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| 00D0: BSF 03.5 |
| 00D1: MOVF 01,W |
| 00D2: ANDLW C7 |
| 00D3: IORLW 08 |
| 00D4: MOVWF 01 |
| .................... setup_timer_1(T1_DISABLED); |
| 00D5: BCF 03.5 |
| 00D6: CLRF 10 |
| .................... setup_timer_2(T2_DISABLED,0,1); |
| 00D7: MOVLW 00 |
| 00D8: MOVWF 78 |
| 00D9: MOVWF 12 |
| 00DA: MOVLW 00 |
| 00DB: BSF 03.5 |
| 00DC: MOVWF 12 |
| .................... setup_comparator(NC_NC_NC_NC); |
| 00DD: MOVLW 07 |
| 00DE: MOVWF 1C |
| 00DF: MOVF 05,W |
| 00E0: CLRWDT |
| 00E1: MOVLW 02 |
| 00E2: MOVWF 77 |
| 00E3: DECFSZ 77,F |
| 00E4: GOTO 0E3 |
| 00E5: NOP |
| 00E6: NOP |
| 00E7: MOVF 1C,W |
| 00E8: BCF 03.5 |
| 00E9: BCF 0D.6 |
| .................... setup_vref(FALSE); |
| 00EA: BSF 03.5 |
| 00EB: CLRF 1D |
| .................... |
| .................... Delay_ms(500); |
| 00EC: MOVLW 02 |
| 00ED: BCF 03.5 |
| 00EE: MOVWF 26 |
| 00EF: MOVLW FA |
| 00F0: MOVWF 27 |
| 00F1: CALL 02A |
| 00F2: DECFSZ 26,F |
| 00F3: GOTO 0EF |
| .................... setup_spi(SPI_SLAVE|SPI_H_TO_L|SPI_SS_DISABLED); |
| 00F4: BCF 14.5 |
| 00F5: BSF 03.5 |
| 00F6: BCF 06.2 |
| 00F7: BSF 06.1 |
| 00F8: BSF 06.4 |
| 00F9: MOVLW 35 |
| 00FA: BCF 03.5 |
| 00FB: MOVWF 14 |
| 00FC: MOVLW 40 |
| 00FD: BSF 03.5 |
| 00FE: MOVWF 14 |
| .................... |
| .................... // diagnostika |
| .................... printf("\n\r"); |
| 00FF: BCF 03.5 |
| 0100: CLRF 26 |
| 0101: MOVF 26,W |
| 0102: CALL 004 |
| 0103: IORLW 00 |
| 0104: BTFSC 03.2 |
| 0105: GOTO 10A |
| 0106: INCF 26,F |
| 0107: MOVWF 29 |
| 0108: CALL 041 |
| 0109: GOTO 101 |
| .................... Delay_ms(100); |
| 010A: MOVLW 64 |
| 010B: MOVWF 27 |
| 010C: CALL 02A |
| .................... printf("***\n\r"); |
| 010D: CLRF 26 |
| 010E: MOVF 26,W |
| 010F: CALL 00B |
| 0110: IORLW 00 |
| 0111: BTFSC 03.2 |
| 0112: GOTO 117 |
| 0113: INCF 26,F |
| 0114: MOVWF 29 |
| 0115: CALL 041 |
| 0116: GOTO 10E |
| .................... Delay_ms(100); |
| 0117: MOVLW 64 |
| 0118: MOVWF 27 |
| 0119: CALL 02A |
| .................... for (n=0; n<=6; n++) |
| 011A: CLRF 25 |
| 011B: MOVF 25,W |
| 011C: SUBLW 06 |
| 011D: BTFSS 03.0 |
| 011E: GOTO 157 |
| .................... { |
| .................... set_adc_channel(n); |
| 011F: RLF 25,W |
| 0120: MOVWF 77 |
| 0121: RLF 77,F |
| 0122: RLF 77,F |
| 0123: MOVLW F8 |
| 0124: ANDWF 77,F |
| 0125: MOVF 1F,W |
| 0126: ANDLW C7 |
| 0127: IORWF 77,W |
| 0128: MOVWF 1F |
| .................... Delay_ms(100); |
| 0129: MOVLW 64 |
| 012A: MOVWF 27 |
| 012B: CALL 02A |
| .................... a=read_adc(); |
| 012C: BSF 1F.2 |
| 012D: BTFSC 1F.2 |
| 012E: GOTO 12D |
| 012F: MOVF 1E,W |
| 0130: MOVWF 24 |
| .................... printf("sensor %u - %u\n\r",n,a); |
| 0131: CLRF 26 |
| 0132: MOVF 26,W |
| 0133: CALL 015 |
| 0134: INCF 26,F |
| 0135: MOVWF 77 |
| 0136: MOVWF 29 |
| 0137: CALL 041 |
| 0138: MOVLW 07 |
| 0139: SUBWF 26,W |
| 013A: BTFSS 03.2 |
| 013B: GOTO 132 |
| 013C: MOVF 25,W |
| 013D: MOVWF 27 |
| 013E: MOVLW 1B |
| 013F: MOVWF 28 |
| 0140: CALL 07C |
| 0141: MOVLW 20 |
| 0142: MOVWF 29 |
| 0143: CALL 041 |
| 0144: MOVLW 2D |
| 0145: MOVWF 29 |
| 0146: CALL 041 |
| 0147: MOVLW 20 |
| 0148: MOVWF 29 |
| 0149: CALL 041 |
| 014A: MOVF 24,W |
| 014B: MOVWF 27 |
| 014C: MOVLW 1B |
| 014D: MOVWF 28 |
| 014E: CALL 07C |
| 014F: MOVLW 0A |
| 0150: MOVWF 29 |
| 0151: CALL 041 |
| 0152: MOVLW 0D |
| 0153: MOVWF 29 |
| 0154: CALL 041 |
| .................... } |
| 0155: INCF 25,F |
| 0156: GOTO 11B |
| .................... |
| .................... shure=0; |
| 0157: CLRF 22 |
| .................... while(true) |
| .................... { |
| .................... set_adc_channel(0); |
| 0158: MOVLW 00 |
| 0159: MOVWF 78 |
| 015A: MOVF 1F,W |
| 015B: ANDLW C7 |
| 015C: IORWF 78,W |
| 015D: MOVWF 1F |
| .................... cidla=0; |
| 015E: CLRF 23 |
| .................... Delay_us(10); |
| 015F: CLRWDT |
| 0160: MOVLW 02 |
| 0161: MOVWF 77 |
| 0162: DECFSZ 77,F |
| 0163: GOTO 162 |
| 0164: NOP |
| 0165: NOP |
| .................... a=read_adc(); |
| 0166: BSF 1F.2 |
| 0167: BTFSC 1F.2 |
| 0168: GOTO 167 |
| 0169: MOVF 1E,W |
| 016A: MOVWF 24 |
| .................... |
| .................... set_adc_channel(1); |
| 016B: MOVLW 08 |
| 016C: MOVWF 78 |
| 016D: MOVF 1F,W |
| 016E: ANDLW C7 |
| 016F: IORWF 78,W |
| 0170: MOVWF 1F |
| .................... if(a<TRESHOLD_MAX) //hystereze cidel |
| 0171: MOVF 24,W |
| 0172: SUBLW 45 |
| 0173: BTFSS 03.0 |
| 0174: GOTO 17E |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 0175: MOVF 24,W |
| 0176: SUBLW 32 |
| 0177: BTFSC 03.0 |
| 0178: GOTO 17D |
| .................... { |
| .................... cidla |= (last_cidla & 0b00000001); |
| 0179: MOVF 21,W |
| 017A: ANDLW 01 |
| 017B: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b00000001; |
| 017C: GOTO 17E |
| 017D: BSF 23.0 |
| .................... } |
| .................... |
| .................... a=read_adc(); |
| 017E: BSF 1F.2 |
| 017F: BTFSC 1F.2 |
| 0180: GOTO 17F |
| 0181: MOVF 1E,W |
| 0182: MOVWF 24 |
| .................... |
| .................... set_adc_channel(2); |
| 0183: MOVLW 10 |
| 0184: MOVWF 78 |
| 0185: MOVF 1F,W |
| 0186: ANDLW C7 |
| 0187: IORWF 78,W |
| 0188: MOVWF 1F |
| .................... if(a<TRESHOLD_MAX) |
| 0189: MOVF 24,W |
| 018A: SUBLW 45 |
| 018B: BTFSS 03.0 |
| 018C: GOTO 196 |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 018D: MOVF 24,W |
| 018E: SUBLW 32 |
| 018F: BTFSC 03.0 |
| 0190: GOTO 195 |
| .................... { |
| .................... cidla |= (last_cidla & 0b00000010); |
| 0191: MOVF 21,W |
| 0192: ANDLW 02 |
| 0193: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b00000010; |
| 0194: GOTO 196 |
| 0195: BSF 23.1 |
| .................... } |
| .................... |
| .................... a=read_adc(); |
| 0196: BSF 1F.2 |
| 0197: BTFSC 1F.2 |
| 0198: GOTO 197 |
| 0199: MOVF 1E,W |
| 019A: MOVWF 24 |
| .................... |
| .................... set_adc_channel(3); |
| 019B: MOVLW 18 |
| 019C: MOVWF 78 |
| 019D: MOVF 1F,W |
| 019E: ANDLW C7 |
| 019F: IORWF 78,W |
| 01A0: MOVWF 1F |
| .................... if(a<TRESHOLD_MAX) |
| 01A1: MOVF 24,W |
| 01A2: SUBLW 45 |
| 01A3: BTFSS 03.0 |
| 01A4: GOTO 1AE |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 01A5: MOVF 24,W |
| 01A6: SUBLW 32 |
| 01A7: BTFSC 03.0 |
| 01A8: GOTO 1AD |
| .................... { |
| .................... cidla |= (last_cidla & 0b00000100); |
| 01A9: MOVF 21,W |
| 01AA: ANDLW 04 |
| 01AB: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b00000100; |
| 01AC: GOTO 1AE |
| 01AD: BSF 23.2 |
| .................... } |
| .................... |
| .................... a=read_adc(); |
| 01AE: BSF 1F.2 |
| 01AF: BTFSC 1F.2 |
| 01B0: GOTO 1AF |
| 01B1: MOVF 1E,W |
| 01B2: MOVWF 24 |
| .................... |
| .................... set_adc_channel(4); |
| 01B3: MOVLW 20 |
| 01B4: MOVWF 78 |
| 01B5: MOVF 1F,W |
| 01B6: ANDLW C7 |
| 01B7: IORWF 78,W |
| 01B8: MOVWF 1F |
| .................... if(a<TRESHOLD_MAX) |
| 01B9: MOVF 24,W |
| 01BA: SUBLW 45 |
| 01BB: BTFSS 03.0 |
| 01BC: GOTO 1C6 |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 01BD: MOVF 24,W |
| 01BE: SUBLW 32 |
| 01BF: BTFSC 03.0 |
| 01C0: GOTO 1C5 |
| .................... { |
| .................... cidla |= (last_cidla & 0b00001000); |
| 01C1: MOVF 21,W |
| 01C2: ANDLW 08 |
| 01C3: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b00001000; |
| 01C4: GOTO 1C6 |
| 01C5: BSF 23.3 |
| .................... } |
| .................... a=read_adc(); |
| 01C6: BSF 1F.2 |
| 01C7: BTFSC 1F.2 |
| 01C8: GOTO 1C7 |
| 01C9: MOVF 1E,W |
| 01CA: MOVWF 24 |
| .................... |
| .................... set_adc_channel(5); |
| 01CB: MOVLW 28 |
| 01CC: MOVWF 78 |
| 01CD: MOVF 1F,W |
| 01CE: ANDLW C7 |
| 01CF: IORWF 78,W |
| 01D0: MOVWF 1F |
| .................... |
| .................... if(a<TRESHOLD_MAX) |
| 01D1: MOVF 24,W |
| 01D2: SUBLW 45 |
| 01D3: BTFSS 03.0 |
| 01D4: GOTO 1DE |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 01D5: MOVF 24,W |
| 01D6: SUBLW 32 |
| 01D7: BTFSC 03.0 |
| 01D8: GOTO 1DD |
| .................... { |
| .................... cidla |= (last_cidla & 0b00010000); |
| 01D9: MOVF 21,W |
| 01DA: ANDLW 10 |
| 01DB: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b00010000; |
| 01DC: GOTO 1DE |
| 01DD: BSF 23.4 |
| .................... } |
| .................... a=read_adc(); |
| 01DE: BSF 1F.2 |
| 01DF: BTFSC 1F.2 |
| 01E0: GOTO 1DF |
| 01E1: MOVF 1E,W |
| 01E2: MOVWF 24 |
| .................... |
| .................... set_adc_channel(6); |
| 01E3: MOVLW 30 |
| 01E4: MOVWF 78 |
| 01E5: MOVF 1F,W |
| 01E6: ANDLW C7 |
| 01E7: IORWF 78,W |
| 01E8: MOVWF 1F |
| .................... if(a<TRESHOLD_MAX) |
| 01E9: MOVF 24,W |
| 01EA: SUBLW 45 |
| 01EB: BTFSS 03.0 |
| 01EC: GOTO 1F6 |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 01ED: MOVF 24,W |
| 01EE: SUBLW 32 |
| 01EF: BTFSC 03.0 |
| 01F0: GOTO 1F5 |
| .................... { |
| .................... cidla |= (last_cidla & 0b00100000); |
| 01F1: MOVF 21,W |
| 01F2: ANDLW 20 |
| 01F3: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b00100000; |
| 01F4: GOTO 1F6 |
| 01F5: BSF 23.5 |
| .................... } |
| .................... a=read_adc(); |
| 01F6: BSF 1F.2 |
| 01F7: BTFSC 1F.2 |
| 01F8: GOTO 1F7 |
| 01F9: MOVF 1E,W |
| 01FA: MOVWF 24 |
| .................... |
| .................... if(a<TRESHOLD_MAX) |
| 01FB: MOVF 24,W |
| 01FC: SUBLW 45 |
| 01FD: BTFSS 03.0 |
| 01FE: GOTO 208 |
| .................... { |
| .................... if(a>TRESHOLD_MIN) |
| 01FF: MOVF 24,W |
| 0200: SUBLW 32 |
| 0201: BTFSC 03.0 |
| 0202: GOTO 207 |
| .................... { |
| .................... cidla |=(last_cidla & 0b01000000); |
| 0203: MOVF 21,W |
| 0204: ANDLW 40 |
| 0205: IORWF 23,F |
| .................... } |
| .................... else cidla |= 0b01000000; |
| 0206: GOTO 208 |
| 0207: BSF 23.6 |
| .................... } |
| .................... |
| .................... last_cidla=cidla; |
| 0208: MOVF 23,W |
| 0209: MOVWF 21 |
| .................... |
| .................... if (!input(IRRX)) {if (shure<255) shure++;} else {shure=0;}; |
| 020A: BSF 03.5 |
| 020B: BSF 06.0 |
| 020C: BCF 03.5 |
| 020D: BTFSC 06.0 |
| 020E: GOTO 214 |
| 020F: INCFSZ 22,W |
| 0210: GOTO 212 |
| 0211: GOTO 213 |
| 0212: INCF 22,F |
| 0213: GOTO 215 |
| 0214: CLRF 22 |
| .................... if (shure>CIHLA) cidla|=0b10000000; |
| 0215: MOVF 22,W |
| 0216: SUBLW 0A |
| 0217: BTFSS 03.0 |
| 0218: BSF 23.7 |
| .................... |
| .................... cidla=~cidla; |
| 0219: COMF 23,F |
| .................... spi_write(cidla); |
| 021A: MOVF 23,W |
| 021B: MOVWF 13 |
| 021C: BSF 03.5 |
| 021D: BTFSC 14.0 |
| 021E: GOTO 221 |
| 021F: BCF 03.5 |
| 0220: GOTO 21C |
| .................... } |
| 0221: BCF 03.5 |
| 0222: GOTO 158 |
| .................... } |
| 0223: SLEEP |
| Configuration Fuses: |
| Word 1: 3F38 NOWDT NOPUT MCLR NOBROWNOUT NOLVP NOCPD NOWRT NODEBUG CCPB0 NOPROTECT INTRC_IO |
| Word 2: 3FFC NOFCMEN NOIESO |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.PJT |
|---|
| 0,0 → 1,44 |
| [PROJECT] |
| Target=cidla.HEX |
| Development_Mode= |
| Processor=0x688F |
| ToolSuite=CCS |
| [Directories] |
| Include=C:\Program Files\PICC\devices\;C:\Program Files\PICC\dr |
| Library= |
| LinkerScript= |
| [Target Data] |
| FileList=D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.c |
| BuildTool=C-COMPILER |
| OptionString=+FM |
| AdditionalOptionString= |
| BuildRequired=1 |
| [cidla.c] |
| Type=4 |
| Path= |
| FileList= |
| BuildTool= |
| OptionString= |
| AdditionalOptionString= |
| [mru-list] |
| 1=cidla.c |
| [Windows] |
| 0=0000 cidla.c 0 0 796 451 3 0 |
| [Opened Files] |
| 1=D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.c |
| 2= |
| 3=C:\Program Files\PICC\drivers\stdlib.h |
| 4=C:\Program Files\PICC\drivers\stddef.h |
| 5=C:\Program Files\PICC\drivers\string.h |
| 6=C:\Program Files\PICC\drivers\ctype.h |
| 7=D:\KAKLIK\programy\PIC_C\roboti\istrobot\3Orbis\cidla\cidla.h |
| 8= |
| [Units] |
| Count=1 |
| 1=D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.c (main) |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.SYM |
|---|
| 0,0 → 1,59 |
| 015 CCP_1_LOW |
| 015-016 CCP_1 |
| 016 CCP_1_HIGH |
| 020 last_radius |
| 021 last_cidla |
| 022 shure |
| 023 main.cidla |
| 024 main.a |
| 025 main.n |
| 026 main.@SCRATCH |
| 027 @PRINTF_U_9600_51_41.P1 |
| 027 @delay_ms1.P2 |
| 028 @PRINTF_U_9600_51_41.P1 |
| 029 @PUTCHAR_1_.P1 |
| 029 @DIV88.P2 |
| 02A @DIV88.P1 |
| 02B @DIV88.@SCRATCH |
| 077 @SCRATCH |
| 078 @SCRATCH |
| 078 _RETURN_ |
| 079 @SCRATCH |
| 07A @SCRATCH |
| 07B @SCRATCH |
| 09C.6 C1OUT |
| 09C.7 C2OUT |
| 002A @delay_ms1 |
| 0041 @PUTCHAR_1_ |
| 00B1 main |
| 0004 @const10062 |
| 000B @const10067 |
| 0015 @const10080 |
| 0067 @DIV88 |
| 007C @PRINTF_U_9600_51_41 |
| 00B1 @cinit |
| Project Files: |
| D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.c |
| D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.h |
| C:\Program Files\PICC\devices\16F88.h |
| Units: |
| D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.c (main) |
| Compiler Settings: |
| Processor: PIC16F88 |
| Pointer Size: 8 |
| ADC Range: 0-255 |
| Opt Level: 9 |
| Short,Int,Long: 1,8,16 |
| Output Files: |
| Errors: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.err |
| INHX8: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.hex |
| Symbols: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.sym |
| List: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.lst |
| Debug/COFF: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.cof |
| Call Tree: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.tre |
| Statistics: D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.sta |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.c |
|---|
| 0,0 → 1,157 |
| #include ".\cidla.h" |
| //#include <stdlib.h> |
| #use rs232(baud=9600,parity=N,xmit=PIN_B3,bits=8,restart_wdt) |
| #define IRRX PIN_B0 |
| #define TRESHOLD_MAX 70 // rozhodovaci uroven pro cidla cerna/bila |
| #define TRESHOLD_MIN 50 |
| #define CIHLA 10 // doba, po kterou musi byt detekovana cihla |
| unsigned int8 last_radius; // rozsah |
| unsigned int8 last_cidla; // co cidla videla minule |
| unsigned int8 shure; // citac doby, po kterou musi byt detekovana cihla |
| //tuning |
| /*#define PULZACE 3 // urcuje rychlost pulzovani pomoci PWM |
| //Vystup PWM je na PIN_B3 |
| //////////////////////////////////////////////////////////////////////////////// |
| void pulzovani() // postupne rozsvecuje a zhasina podsvetleni |
| { |
| unsigned int8 i,n; |
| for(n=0;n<=3;n++) |
| { |
| for(i=0;i<255;i++) {set_pwm1_duty(i); Delay_ms(PULZACE);} // rozsvecovani |
| for(i=255;i>0;i--) {set_pwm1_duty(i); Delay_ms(PULZACE);} // zhasinani |
| } |
| } |
| */ |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| int8 cidla; |
| unsigned int8 a; |
| unsigned int8 n; |
| setup_adc_ports(sAN0|sAN1|sAN2|sAN3|sAN4|sAN5|sAN6|VSS_VDD); |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED); |
| setup_timer_2(T2_DISABLED,0,1); |
| setup_comparator(NC_NC_NC_NC); |
| setup_vref(FALSE); |
| Delay_ms(500); |
| setup_spi(SPI_SLAVE|SPI_H_TO_L|SPI_SS_DISABLED); |
| // diagnostika |
| printf("\n\r"); |
| Delay_ms(100); |
| printf("***\n\r"); |
| Delay_ms(100); |
| for (n=0; n<=6; n++) |
| { |
| set_adc_channel(n); |
| Delay_ms(100); |
| a=read_adc(); |
| printf("sensor %u - %u\n\r",n,a); |
| } |
| shure=0; |
| while(true) |
| { |
| set_adc_channel(0); |
| cidla=0; |
| Delay_us(10); |
| a=read_adc(); |
| set_adc_channel(1); |
| if(a<TRESHOLD_MAX) //hystereze cidel |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000001); |
| } |
| else cidla |= 0b00000001; |
| } |
| a=read_adc(); |
| set_adc_channel(2); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000010); |
| } |
| else cidla |= 0b00000010; |
| } |
| a=read_adc(); |
| set_adc_channel(3); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000100); |
| } |
| else cidla |= 0b00000100; |
| } |
| a=read_adc(); |
| set_adc_channel(4); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00001000); |
| } |
| else cidla |= 0b00001000; |
| } |
| a=read_adc(); |
| set_adc_channel(5); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00010000); |
| } |
| else cidla |= 0b00010000; |
| } |
| a=read_adc(); |
| set_adc_channel(6); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00100000); |
| } |
| else cidla |= 0b00100000; |
| } |
| a=read_adc(); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |=(last_cidla & 0b01000000); |
| } |
| else cidla |= 0b01000000; |
| } |
| last_cidla=cidla; |
| if (!input(IRRX)) {if (shure<255) shure++;} else {shure=0;}; |
| if (shure>CIHLA) cidla|=0b10000000; |
| cidla=~cidla; |
| spi_write(cidla); |
| } |
| } |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.cof |
|---|
| Cannot display: file marked as a binary type. |
| svn:mime-type = application/octet-stream |
| Property changes: |
| Added: svn:mime-type |
| +application/octet-stream |
| \ No newline at end of property |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.err |
|---|
| 0,0 → 1,2 |
| No Errors |
| 0 Errors, 0 Warnings. |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.h |
|---|
| 0,0 → 1,5 |
| #include <16F88.h> |
| #device adc=8 |
| #fuses NOWDT,INTRC_IO, NOPUT, MCLR, NOBROWNOUT, NOLVP, NOCPD, NOWRT, NODEBUG, NOPROTECT, NOFCMEN, NOIESO |
| #use delay(clock=4000000,RESTART_WDT) |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.sta |
|---|
| 0,0 → 1,35 |
| ROM used: 548 (13%) |
| 548 (13%) including unused fragments |
| 1 Average locations per line |
| 6 Average locations per statement |
| RAM used: 12 (7%) at main() level |
| 17 (10%) worst case |
| Lines Stmts % Files |
| ----- ----- --- ----- |
| 158 92 100 D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.c |
| 6 0 0 D:\KAKLIK\projekty\roboti\istrobot\3Orbis\cidla\cidla.h |
| 279 0 0 C:\Program Files\PICC\devices\16F88.h |
| ----- ----- |
| 886 184 Total |
| Page ROM % RAM Functions: |
| ---- --- --- --- ---------- |
| 0 23 4 1 @delay_ms1 |
| 0 38 7 1 @PUTCHAR_1_ |
| 0 371 68 4 main |
| 0 7 1 0 @const10062 |
| 0 10 2 0 @const10067 |
| 0 21 4 0 @const10080 |
| 0 21 4 3 @DIV88 |
| 0 53 10 2 @PRINTF_U_9600_51_41 |
| Segment Used Free |
| --------- ---- ---- |
| 00000-00003 4 0 |
| 00004-007FF 544 1500 |
| 00800-00FFF 0 2048 |
| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.tre |
|---|
| 0,0 → 1,30 |
| ÀÄcidla |
| ÀÄmain 0/371 Ram=4 |
| ÃÄ??0?? |
| ÃÄ@delay_ms1 0/23 Ram=1 |
| ÃÄ@const10062 0/7 Ram=0 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@delay_ms1 0/23 Ram=1 |
| ÃÄ@const10067 0/10 Ram=0 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@delay_ms1 0/23 Ram=1 |
| ÃÄ@delay_ms1 0/23 Ram=1 |
| ÃÄ@const10080 0/21 Ram=0 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@PRINTF_U_9600_51_41 0/53 Ram=2 |
| ³ ÃÄ@DIV88 0/21 Ram=3 |
| ³ ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ³ ÃÄ@DIV88 0/21 Ram=3 |
| ³ ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ³ ÀÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@PRINTF_U_9600_51_41 0/53 Ram=2 |
| ³ ÃÄ@DIV88 0/21 Ram=3 |
| ³ ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ³ ÃÄ@DIV88 0/21 Ram=3 |
| ³ ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ³ ÀÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÃÄ@PUTCHAR_1_ 0/38 Ram=1 |
| ÀÄ@PUTCHAR_1_ 0/38 Ram=1 |
| /Designs/ROBOTS/3Orbis/SW/cidla/macro.ini |
|---|
| /Designs/ROBOTS/3Orbis/SW/main.BAK |
|---|
| 0,0 → 1,288 |
| #include ".\main.h" |
| #define KOLMO1 225 // predni kolecko sroubem dopredu |
| #define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu |
| #define STRED 128 // sredni poloha zataceciho kolecka |
| #define BEAR1 10//10 // 3 stupne zataceni |
| #define BEAR2 25//25 |
| #define BEAR3 45//45 |
| #define R 100 // Rozumna rychlost |
| #define R17 200 // X nasobek rozumne rychlosti |
| #define PRED_CIHLOU 100 // rychlost pri dalkove detekci cihly |
| //#define L1 1 // cara vlevo |
| #define L2 2 // cara vlevo |
| #define L3 3 // cara vlevo |
| #define S 0 // cara mezi sensory |
| //#define R1 -1 // cara vpravo |
| #define R2 -2 // cara vpravo |
| #define R3 -3 // cara vpravo |
| // servo |
| #define SERVO PIN_B5 |
| // kroutitka |
| #define CERVENA 4 // AN4 |
| //#define CERNA 5 // AN5 |
| //#define ZELENA 6 // AN6 |
| #define MODRA 2 // AN2 |
| // IR |
| #define IRTX PIN_B2 |
| #define CIHLA PIN_A3 |
| //motory |
| #define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred |
| #define FL output_low(PIN_A1); output_high(PIN_A0) |
| #define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad |
| #define BL output_low(PIN_A0); output_high(PIN_A1) |
| #define STOPR output_low(PIN_A6);output_low(PIN_A7) |
| #define STOPL output_low(PIN_A0);output_low(PIN_A1) |
| //HID |
| #define LED1 PIN_B1 //oranzova |
| #define LED2 PIN_B2 //zluta |
| #define STROBE PIN_B0 |
| //#define SW1 PIN_A2 // Motory On/off |
| unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
| signed int8 line = S; // na ktere strane byla detekovana cara |
| //unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
| unsigned int8 uhel; // urcuje aktualni uhel zataceni |
| unsigned int8 speed; // maximalni povolena rychlost |
| unsigned int8 turn; // rychlost toceni |
| unsigned int8 rovinka; // pocitadlo na zjisteni rovinky |
| signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
| signed int16 Rmotor; // a pravem motoru |
| // makro pro PWM pro motory |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} else {stop##motor;} |
| //////////////////////////////////////////////////////////////////////////////// |
| #int_TIMER2 |
| TIMER2_isr() // ovladani serva |
| { |
| unsigned int8 n; |
| output_high(SERVO); |
| delay_us(1000); |
| for(n=uhel; n>0; n--) Delay_us(2); |
| output_low(SERVO); |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| int8 IRcheck() // potvrdi detekci cihly |
| { |
| output_high(IRTX); // vypne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| if(bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
| { |
| output_low(IRTX); // zapne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| if(!bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
| { |
| output_high(IRTX); // vypne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| output_low(IRTX); // zapne vysilac IR |
| if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla |
| } |
| }; |
| output_low(IRTX); // zapne vysilac IR |
| return 0; // vrat 0, kdyz je detekovano ruseni |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| #include ".\objizdka_centrovani.c" |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| unsigned int8 n; |
| unsigned int8 i,j; |
| unsigned int8 last_sensors; |
| unsigned int8 RozumnaRychlost; |
| setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED|T1_DIV_BY_8); |
| setup_timer_2(T2_DIV_BY_16,140,16); |
| setup_oscillator(OSC_8MHZ|OSC_INTRC); |
| STOPR; STOPL; // zastav motory |
| Lmotor=0;Rmotor=0; |
| uhel = STRED; // nastav zadni kolecko na stred |
| rovinka = 0; |
| enable_interrupts(INT_TIMER2); |
| enable_interrupts(GLOBAL); |
| output_low(IRTX); // zapni IR vysilac |
| delay_ms(2000); // musime pockat na diagnostiku slave CPU |
| //nastaveni rychlosti |
| set_adc_channel(CERVENA); |
| Delay_ms(1); |
| RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63 |
| set_adc_channel(MODRA); |
| Delay_ms(1); |
| turn=speed-32+(read_adc()>>2); // rychlost toceni +-32; kroutitko dava 0-63 |
| speed=R17; |
| while(true) |
| { |
| GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor |
| delay_us(2000); // cekani na SLAVE, nez pripravi data od cidel |
| last_sensors=sensors; |
| output_low(STROBE); // vypni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; // neguj prijata data |
| output_high(STROBE); // zobraz data na posuvnem registru |
| i=0; // havarijni kod |
| for (n=0; n<=6; n++) |
| { |
| if(bit_test(sensors,n)) i++; |
| } |
| if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly |
| { |
| BL; BR; |
| delay_ms(300); |
| STOPR; STOPL; |
| While(true); |
| }; |
| /* |
| if (!input(CIHLA)) // dalkova detekce cihly |
| { |
| speed=PRED_CIHLOU; |
| } |
| else |
| { |
| speed=RozumnaRychlost; |
| } |
| */ |
| if (bit_test(sensors,7)) // detekce cihly |
| { |
| BR;BL; |
| Delay_ms(400); |
| STOPR;STOPL; |
| // if (1==IRcheck()) // kontrola, jestli nebylo rusene cidlo |
| { |
| Delay_ms(100); |
| cikcak(); |
| delay_ms(100); |
| objizdka(); // objede cihlu |
| } |
| } |
| if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl. |
| if(bit_test(sensors,3)) //...|...// |
| { |
| uhel=STRED; |
| Lmotor=speed; |
| Rmotor=speed; |
| line=S; |
| if (rovinka < 255) rovinka++; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| uhel=STRED - BEAR3; |
| Lmotor=0; |
| Rmotor=turn; |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| uhel=STRED + BEAR3; |
| Rmotor=0; |
| Lmotor=turn; |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| uhel=STRED - BEAR2; |
| Lmotor=speed-70; |
| Rmotor=speed; |
| line=L2; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| uhel=STRED + BEAR2; |
| Rmotor=speed-70; |
| Lmotor=speed; |
| line=R2; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| uhel=STRED - BEAR1; |
| Lmotor=speed-20; |
| Rmotor=speed; |
| line=L2; |
| if (rovinka<255) rovinka++; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| uhel=STRED + BEAR1; |
| Rmotor=speed-20; |
| Lmotor=speed; |
| line=L2; |
| if (rovinka<255) rovinka++; |
| continue; |
| } |
| if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate |
| { |
| if (rovinka>50) |
| { |
| BL; BR; |
| Delay_ms(100); |
| if (rovinka > 250 || speed > 170) delay_ms(50); |
| }; |
| rovinka=0; |
| speed=R17; |
| }; |
| } |
| } |
| /Designs/ROBOTS/3Orbis/SW/main.HEX |
|---|
| 0,0 → 1,148 |
| :1000000000308A006C2A0000FF00030E8301A1006B |
| :100010007F08A0000A08A8008A01A00E0408A20018 |
| :100020007708A3007808A4007908A5007A08A6003C |
| :100030007B08A700831383128C308400801C222845 |
| :100040008C183528220884002308F7002408F800BB |
| :100050002508F9002608FA002708FB0028088A006E |
| :10006000210E8300FF0E7F0E09008A1147280830F9 |
| :10007000BD02031C46283D3084000310800C00089C |
| :100080000319462844286400800B43280034831653 |
| :1000900086128312861664000930BC006D30BD00E4 |
| :1000A0003720BC0B4E282C08BB00BB0803195E2868 |
| :1000B0006400000000000000BB0355288316861270 |
| :1000C000831286128C108A1122283A30840000088C |
| :1000D00003197C280230F800BF30F7006400F70BEA |
| :1000E0006E28F80B6C289630F700F70B7528000087 |
| :1000F00000006400800B6A280034B901E130AC00D4 |
| :100100006430BA0065202B08023C031D89280330A7 |
| :10011000AB00AB08031D8E280330AB002B08FE3C60 |
| :10012000031D9428FD30AB000330BA006520831610 |
| :100130000610831206101308930183161418A228C0 |
| :1001400083129D2883121308AA00AA098316061099 |
| :10015000831206140330BA0065202B08033C031DEC |
| :10016000DB280108A03C031CBE288316051083125F |
| :1001700005108316851083128514C6288316051072 |
| :100180008312051083168510831285100108A03C88 |
| :10019000031CD32883168513831285138316051336 |
| :1001A00083120517DB2883160513831205138316A4 |
| :1001B0008513831285132B08FD3C031D09290108B3 |
| :1001C000A03C031CEC288316051383120513831629 |
| :1001D000851383128517F4288316051383120513DC |
| :1001E00083168513831285130108A03C031C012983 |
| :1001F000831685108312851083160510831205144B |
| :1002000009298316051083120510831685108312A1 |
| :100210008510AB08031D1E298316051083120510D7 |
| :100220008316851083128510831605138312051318 |
| :100230008316851383128513B90A1F29B90139085A |
| :10024000633C031C592983160610831206101308F9 |
| :100250009301831614182E29831229298312130857 |
| :10026000AA00AA098316061083120614AA1D3A29A9 |
| :10027000AB01AA282A1C3F290330AB00AA282A1F59 |
| :100280004429FD30AB00AA28AA1C49290330AB0041 |
| :10029000AA28AA1E4E29FD30AB00AA282A1D5329E0 |
| :1002A0000330AB00AA282A1E5829FD30AB00AA282B |
| :1002B000AA28003483168510831285108316051032 |
| :1002C00083120514831605138312051383168513F1 |
| :1002D000831285170230B9009630BA006520B90B39 |
| :1002E0006C2983160510831205108316851083125E |
| :1002F00085148316851383128513831605138312C1 |
| :1003000005176430BA0065208316051083120510A6 |
| :100310008316851083128510831605138312051327 |
| :1003200083168513831285138030AC0083168510E5 |
| :100330008312851083160510831205148316851306 |
| :100340008312851383160513831205170230B90033 |
| :10035000BE30BA006520B90BA8294930AC0083161D |
| :1003600005108312051083168510831285108316DD |
| :100370008513831285138316051383120517BE3068 |
| :10038000BA0065208030AC00831685138312851374 |
| :1003900083160513831205178316851083128510A3 |
| :1003A00083160510831205140230B9009630BA0086 |
| :1003B0006520B90BD6294930AC008316851383120A |
| :1003C00085138316051383120517831605108312F0 |
| :1003D00005108316851083128510BE30BA00652083 |
| :1003E0008030AC00831685138312851383160513A2 |
| :1003F0008312051783168510831285108316051046 |
| :10040000831205146430BA0065202A08FE390319E6 |
| :10041000182A1308930183161418102A83120B2A22 |
| :1004200083121308AA00AA090330BA006520052A1E |
| :100430008316051083120510831685108312851408 |
| :1004400083160513831205138316851383128517EC |
| :10045000FA30BA006520E130AC00831685108312B3 |
| :100460008510831605108312051483160513831255 |
| :10047000051383168513831285172A08FE3903197D |
| :10048000502A1308930183161418482A8312432A0A |
| :1004900083121308AA00AA090330BA0065203D2A76 |
| :1004A000831605108312051083168510831285109C |
| :1004B0008316051383120513831685138312851380 |
| :1004C0000230B9009630BA006520B90B622AFD30BF |
| :1004D000AB007D208A11CF2B84011F308305703043 |
| :1004E00083168F001F129F121B0880399B00073054 |
| :1004F0009C008312AB0183161F129F121B088039C8 |
| :1005000074389B001F1383121F179F1783169F13A6 |
| :1005100083121F14941283160611861406123130AA |
| :10052000831294000030831694000108C7390838FC |
| :1005300081003030831290007830F8000638920045 |
| :100540008C308316920072308F00051383120513CE |
| :1005500083168513831285138316051083120510E5 |
| :100560008316851083128510B101B001B301B20169 |
| :100570008030AC00AF0183168C14C03083128B0422 |
| :1005800083160611831206110830B900FA30BA003A |
| :100590006520B90BC62A2030F8001F08C739780437 |
| :1005A0009F000130BA0065201F151F19D52A1E08AB |
| :1005B000F700F70CF70C3F30F7057708643EB800FA |
| :1005C0001030F8001F08C73978049F000130BA00C6 |
| :1005D000652020302D02B9001F151F19ED2A1E08B5 |
| :1005E000F700F70CF70C3F30F70577083907AE0036 |
| :1005F000C830AD000108801F002BB11F082B022B53 |
| :10060000B11B112BB108031D082B3002031C112B49 |
| :100610008316851083128510831605108312051426 |
| :10062000192B83160510831205108316851083126B |
| :1006300085100108801F1F2BB31F272B212BB31BF5 |
| :10064000302BB308031D272B3202031C302B8316DB |
| :100650008513831285138316051383120517382B10 |
| :1006600083160513831205138316851383128513CE |
| :1006700064000130BA0065200930B900A9018B1B64 |
| :10068000A9178B136D30BD003720A91B8B17B90B31 |
| :100690003E2B2A08B7008316061083120610130893 |
| :1006A000930183161418562B8312512B83121308AF |
| :1006B000AA00AA098316061083120614B501B40114 |
| :1006C0003408063C031C722B2A08F7003408F80093 |
| :1006D00003196E2B0310F70CF80B6A2B7718B50A69 |
| :1006E000B40A602B3508043C03189E2B83160510B2 |
| :1006F0008312051083168510831285148316051343 |
| :100700008312051383168513831285170230B900EF |
| :100710009630BA006520B90B882B83160513831217 |
| :100720000513831685138312851383160510831210 |
| :10073000051083168510831285109D2BAA1FCF2BC1 |
| :1007400083160513831205138316851383128517E9 |
| :1007500083160510831205108316851083128514E5 |
| :100760000230B900C830BA006520B90BB22B83162D |
| :1007700005138312051383168513831285138316BD |
| :1007800005108312051083168510831285106430BE |
| :10079000BA0065207D206430BA0065205A292D08F2 |
| :1007A0003802031CAD03AA1DE72B8030AC00FA0110 |
| :1007B0002D08B0007A08B100FA012D08B2007A08BD |
| :1007C000B300AB012F0FE52BE62BAF0AFA2A2A1C48 |
| :1007D000F52B5330AC00B101B001FA012E08B20084 |
| :1007E0007A08B3000330AB00FA2A2A1F032CAD307D |
| :1007F000AC00B301B201FA012E08B0007A08B100D2 |
| :10080000FD30AB00FA2AAA1C152C6730AC0046302C |
| :100810002D02FA01B0007A08B100FA012D08B200E9 |
| :100820007A08B3000230AB00FA2AAA1E272C9930AE |
| :10083000AC0046302D02FA01B2007A08B300FA018A |
| :100840002D08B0007A08B100FE30AB00FA2A2A1D4C |
| :100850003D2C7630AC0014302D02FA01B0007A083D |
| :10086000B100FA012D08B2007A08B3000230AB00E3 |
| :100870002F0F3B2C3C2CAF0AFA2A2A1E532C8A300D |
| :10088000AC0014302D02FA01B2007A08B300FA016C |
| :100890002D08B0007A08B1000230AB002F0F512CA8 |
| :1008A000522CAF0AFA2A2B08033C03195B2C2B08A5 |
| :1008B000FD3C031D802C2F08323C03187D2C831631 |
| :1008C0000510831205108316851083128514831674 |
| :1008D000051383120513831685138312851764305D |
| :1008E000BA0065202F08FA3C031C7A2C2D08AA3C7C |
| :1008F00003187D2C3230BA006520AF01C830AD003E |
| :04090000FA2A63006C |
| :04400E00383FFC3FFC |
| :00000001FF |
| ;PIC16F88 |
| /Designs/ROBOTS/3Orbis/SW/main.LST |
|---|
| 0,0 → 1,1625 |
| CCS PCM C Compiler, Version 3.245, 27853 30-IV-06 21:17 |
| Filename: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.lst |
| ROM used: 1154 words (28%) |
| Largest free fragment is 2048 |
| RAM used: 32 (18%) at main() level |
| 35 (20%) worst case |
| Stack: 5 worst case (3 in main + 2 for interrupts) |
| * |
| 0000: MOVLW 00 |
| 0001: MOVWF 0A |
| 0002: GOTO 26C |
| 0003: NOP |
| 0004: MOVWF 7F |
| 0005: SWAPF 03,W |
| 0006: CLRF 03 |
| 0007: MOVWF 21 |
| 0008: MOVF 7F,W |
| 0009: MOVWF 20 |
| 000A: MOVF 0A,W |
| 000B: MOVWF 28 |
| 000C: CLRF 0A |
| 000D: SWAPF 20,F |
| 000E: MOVF 04,W |
| 000F: MOVWF 22 |
| 0010: MOVF 77,W |
| 0011: MOVWF 23 |
| 0012: MOVF 78,W |
| 0013: MOVWF 24 |
| 0014: MOVF 79,W |
| 0015: MOVWF 25 |
| 0016: MOVF 7A,W |
| 0017: MOVWF 26 |
| 0018: MOVF 7B,W |
| 0019: MOVWF 27 |
| 001A: BCF 03.7 |
| 001B: BCF 03.5 |
| 001C: MOVLW 8C |
| 001D: MOVWF 04 |
| 001E: BTFSS 00.1 |
| 001F: GOTO 022 |
| 0020: BTFSC 0C.1 |
| 0021: GOTO 035 |
| 0022: MOVF 22,W |
| 0023: MOVWF 04 |
| 0024: MOVF 23,W |
| 0025: MOVWF 77 |
| 0026: MOVF 24,W |
| 0027: MOVWF 78 |
| 0028: MOVF 25,W |
| 0029: MOVWF 79 |
| 002A: MOVF 26,W |
| 002B: MOVWF 7A |
| 002C: MOVF 27,W |
| 002D: MOVWF 7B |
| 002E: MOVF 28,W |
| 002F: MOVWF 0A |
| 0030: SWAPF 21,W |
| 0031: MOVWF 03 |
| 0032: SWAPF 7F,F |
| 0033: SWAPF 7F,W |
| 0034: RETFIE |
| 0035: BCF 0A.3 |
| 0036: GOTO 047 |
| .................... #include ".\main.h" |
| .................... #include <16F88.h> |
| .................... //////// Standard Header file for the PIC16F88 device //////////////// |
| .................... #device PIC16F88 |
| .................... #list |
| .................... |
| .................... #device adc=8 |
| .................... |
| .................... #FUSES NOWDT //No Watch Dog Timer |
| .................... #FUSES INTRC_IO |
| .................... #FUSES NOPUT //No Power Up Timer |
| .................... #FUSES MCLR //Master Clear pin enabled |
| .................... #FUSES NOBROWNOUT //Reset when brownout detected |
| .................... #FUSES NOLVP //Low Voltage Programming on B3(PIC16) or B5(PIC18) |
| .................... #FUSES NOCPD //No EE protection |
| .................... #FUSES NOWRT //Program memory not write protected |
| .................... #FUSES NODEBUG //No Debug mode for ICD |
| .................... #FUSES NOPROTECT //Code not protected from reading |
| .................... #FUSES NOFCMEN //Fail-safe clock monitor enabled |
| .................... #FUSES NOIESO //Internal External Switch Over mode enabled |
| .................... |
| .................... #use delay(clock=8000000,RESTART_WDT) |
| 0037: MOVLW 08 |
| 0038: SUBWF 3D,F |
| 0039: BTFSS 03.0 |
| 003A: GOTO 046 |
| 003B: MOVLW 3D |
| 003C: MOVWF 04 |
| 003D: BCF 03.0 |
| 003E: RRF 00,F |
| 003F: MOVF 00,W |
| 0040: BTFSC 03.2 |
| 0041: GOTO 046 |
| 0042: GOTO 044 |
| 0043: CLRWDT |
| 0044: DECFSZ 00,F |
| 0045: GOTO 043 |
| 0046: RETLW 00 |
| * |
| 0065: MOVLW 3A |
| 0066: MOVWF 04 |
| 0067: MOVF 00,W |
| 0068: BTFSC 03.2 |
| 0069: GOTO 07C |
| 006A: MOVLW 02 |
| 006B: MOVWF 78 |
| 006C: MOVLW BF |
| 006D: MOVWF 77 |
| 006E: CLRWDT |
| 006F: DECFSZ 77,F |
| 0070: GOTO 06E |
| 0071: DECFSZ 78,F |
| 0072: GOTO 06C |
| 0073: MOVLW 96 |
| 0074: MOVWF 77 |
| 0075: DECFSZ 77,F |
| 0076: GOTO 075 |
| 0077: NOP |
| 0078: NOP |
| 0079: CLRWDT |
| 007A: DECFSZ 00,F |
| 007B: GOTO 06A |
| 007C: RETLW 00 |
| .................... |
| .................... |
| .................... |
| .................... #define KOLMO1 225 // predni kolecko sroubem dopredu |
| .................... #define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu |
| .................... #define STRED 128 // sredni poloha zataceciho kolecka |
| .................... #define BEAR1 10//10 // 3 stupne zataceni |
| .................... #define BEAR2 25//25 |
| .................... #define BEAR3 45//45 |
| .................... #define R 100 // Rozumna rychlost |
| .................... #define R17 200 // X nasobek rozumne rychlosti |
| .................... #define PRED_CIHLOU 100 // rychlost pri dalkove detekci cihly |
| .................... //#define L1 1 // cara vlevo |
| .................... #define L2 2 // cara vlevo |
| .................... #define L3 3 // cara vlevo |
| .................... #define S 0 // cara mezi sensory |
| .................... //#define R1 -1 // cara vpravo |
| .................... #define R2 -2 // cara vpravo |
| .................... #define R3 -3 // cara vpravo |
| .................... |
| .................... // servo |
| .................... #define SERVO PIN_B5 |
| .................... |
| .................... // kroutitka |
| .................... #define CERVENA 4 // AN4 |
| .................... //#define CERNA 5 // AN5 |
| .................... //#define ZELENA 6 // AN6 |
| .................... #define MODRA 2 // AN2 |
| .................... |
| .................... // IR |
| .................... #define IRTX PIN_B2 |
| .................... #define CIHLA PIN_A3 |
| .................... |
| .................... //motory |
| .................... #define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred |
| .................... #define FL output_low(PIN_A1); output_high(PIN_A0) |
| .................... #define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad |
| .................... #define BL output_low(PIN_A0); output_high(PIN_A1) |
| .................... #define STOPR output_low(PIN_A6);output_low(PIN_A7) |
| .................... #define STOPL output_low(PIN_A0);output_low(PIN_A1) |
| .................... |
| .................... //HID |
| .................... #define LED1 PIN_B1 //oranzova |
| .................... #define LED2 PIN_B2 //zluta |
| .................... |
| .................... #define STROBE PIN_B0 |
| .................... //#define SW1 PIN_A2 // Motory On/off |
| .................... |
| .................... unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
| .................... signed int8 line = S; // na ktere strane byla detekovana cara |
| * |
| 0279: BCF 03.5 |
| 027A: CLRF 2B |
| .................... //unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
| .................... unsigned int8 uhel; // urcuje aktualni uhel zataceni |
| .................... unsigned int8 speed; // maximalni povolena rychlost |
| .................... unsigned int8 turn; // rychlost toceni |
| .................... unsigned int8 rovinka; // pocitadlo na zjisteni rovinky |
| .................... |
| .................... signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
| .................... signed int16 Rmotor; // a pravem motoru |
| .................... |
| .................... // makro pro PWM pro motory |
| .................... #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| .................... {direction##motor;} else {stop##motor;} |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| .................... #int_TIMER2 |
| .................... TIMER2_isr() // ovladani serva |
| .................... { |
| .................... unsigned int8 n; |
| .................... |
| .................... output_high(SERVO); |
| * |
| 0047: BSF 03.5 |
| 0048: BCF 06.5 |
| 0049: BCF 03.5 |
| 004A: BSF 06.5 |
| .................... delay_us(1000); |
| 004B: CLRWDT |
| 004C: MOVLW 09 |
| 004D: MOVWF 3C |
| 004E: MOVLW 6D |
| 004F: MOVWF 3D |
| 0050: CALL 037 |
| 0051: DECFSZ 3C,F |
| 0052: GOTO 04E |
| .................... for(n=uhel; n>0; n--) Delay_us(2); |
| 0053: MOVF 2C,W |
| 0054: MOVWF 3B |
| 0055: MOVF 3B,F |
| 0056: BTFSC 03.2 |
| 0057: GOTO 05E |
| 0058: CLRWDT |
| 0059: NOP |
| 005A: NOP |
| 005B: NOP |
| 005C: DECF 3B,F |
| 005D: GOTO 055 |
| .................... output_low(SERVO); |
| 005E: BSF 03.5 |
| 005F: BCF 06.5 |
| 0060: BCF 03.5 |
| 0061: BCF 06.5 |
| .................... } |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| 0062: BCF 0C.1 |
| 0063: BCF 0A.3 |
| 0064: GOTO 022 |
| .................... int8 IRcheck() // potvrdi detekci cihly |
| .................... { |
| .................... output_high(IRTX); // vypne vysilac IR |
| .................... delay_ms(10); |
| .................... |
| .................... output_low(STROBE); |
| .................... sensors = spi_read(0); // cteni senzoru |
| .................... sensors=~sensors; |
| .................... output_high(STROBE); |
| .................... |
| .................... if(bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
| .................... { |
| .................... output_low(IRTX); // zapne vysilac IR |
| .................... delay_ms(10); |
| .................... |
| .................... output_low(STROBE); |
| .................... sensors = spi_read(0); // cteni senzoru |
| .................... sensors=~sensors; |
| .................... output_high(STROBE); |
| .................... |
| .................... if(!bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
| .................... { |
| .................... output_high(IRTX); // vypne vysilac IR |
| .................... delay_ms(10); |
| .................... |
| .................... output_low(STROBE); |
| .................... sensors = spi_read(0); // cteni senzoru |
| .................... sensors=~sensors; |
| .................... output_high(STROBE); |
| .................... |
| .................... output_low(IRTX); // zapne vysilac IR |
| .................... |
| .................... if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla |
| .................... } |
| .................... }; |
| .................... output_low(IRTX); // zapne vysilac IR |
| .................... return 0; // vrat 0, kdyz je detekovano ruseni |
| .................... } |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| .................... #include ".\objizdka_centrovani_R.c" |
| .................... #define DOLEVA 0 |
| .................... #define DOPRAVA 1 |
| .................... |
| .................... void cikcak() |
| .................... { |
| .................... unsigned int8 i=0; |
| * |
| 007D: CLRF 39 |
| .................... |
| .................... uhel=KOLMO1; // aby se dalo tocit na miste |
| 007E: MOVLW E1 |
| 007F: MOVWF 2C |
| .................... Delay_ms(100); |
| 0080: MOVLW 64 |
| 0081: MOVWF 3A |
| 0082: CALL 065 |
| .................... |
| .................... if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| 0083: MOVF 2B,W |
| 0084: SUBLW 02 |
| 0085: BTFSS 03.2 |
| 0086: GOTO 089 |
| 0087: MOVLW 03 |
| 0088: MOVWF 2B |
| .................... if (line==S) line=L3; |
| 0089: MOVF 2B,F |
| 008A: BTFSS 03.2 |
| 008B: GOTO 08E |
| 008C: MOVLW 03 |
| 008D: MOVWF 2B |
| .................... if (line==R2) line=R3; |
| 008E: MOVF 2B,W |
| 008F: SUBLW FE |
| 0090: BTFSS 03.2 |
| 0091: GOTO 094 |
| 0092: MOVLW FD |
| 0093: MOVWF 2B |
| .................... |
| .................... Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| 0094: MOVLW 03 |
| 0095: MOVWF 3A |
| 0096: CALL 065 |
| .................... |
| .................... output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| 0097: BSF 03.5 |
| 0098: BCF 06.0 |
| 0099: BCF 03.5 |
| 009A: BCF 06.0 |
| .................... sensors = spi_read(0); // cteni senzoru |
| 009B: MOVF 13,W |
| 009C: CLRF 13 |
| 009D: BSF 03.5 |
| 009E: BTFSC 14.0 |
| 009F: GOTO 0A2 |
| 00A0: BCF 03.5 |
| 00A1: GOTO 09D |
| 00A2: BCF 03.5 |
| 00A3: MOVF 13,W |
| 00A4: MOVWF 2A |
| .................... sensors=~sensors; |
| 00A5: COMF 2A,F |
| .................... output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| 00A6: BSF 03.5 |
| 00A7: BCF 06.0 |
| 00A8: BCF 03.5 |
| 00A9: BSF 06.0 |
| .................... |
| .................... while(true) |
| .................... { |
| .................... Delay_ms(3); |
| 00AA: MOVLW 03 |
| 00AB: MOVWF 3A |
| 00AC: CALL 065 |
| .................... |
| .................... if (line==L3) |
| 00AD: MOVF 2B,W |
| 00AE: SUBLW 03 |
| 00AF: BTFSS 03.2 |
| 00B0: GOTO 0DB |
| .................... { |
| .................... GO(L,B,160);GO(R,F,160); |
| 00B1: MOVF 01,W |
| 00B2: SUBLW A0 |
| 00B3: BTFSS 03.0 |
| 00B4: GOTO 0BE |
| 00B5: BSF 03.5 |
| 00B6: BCF 05.0 |
| 00B7: BCF 03.5 |
| 00B8: BCF 05.0 |
| 00B9: BSF 03.5 |
| 00BA: BCF 05.1 |
| 00BB: BCF 03.5 |
| 00BC: BSF 05.1 |
| 00BD: GOTO 0C6 |
| 00BE: BSF 03.5 |
| 00BF: BCF 05.0 |
| 00C0: BCF 03.5 |
| 00C1: BCF 05.0 |
| 00C2: BSF 03.5 |
| 00C3: BCF 05.1 |
| 00C4: BCF 03.5 |
| 00C5: BCF 05.1 |
| 00C6: MOVF 01,W |
| 00C7: SUBLW A0 |
| 00C8: BTFSS 03.0 |
| 00C9: GOTO 0D3 |
| 00CA: BSF 03.5 |
| 00CB: BCF 05.7 |
| 00CC: BCF 03.5 |
| 00CD: BCF 05.7 |
| 00CE: BSF 03.5 |
| 00CF: BCF 05.6 |
| 00D0: BCF 03.5 |
| 00D1: BSF 05.6 |
| 00D2: GOTO 0DB |
| 00D3: BSF 03.5 |
| 00D4: BCF 05.6 |
| 00D5: BCF 03.5 |
| 00D6: BCF 05.6 |
| 00D7: BSF 03.5 |
| 00D8: BCF 05.7 |
| 00D9: BCF 03.5 |
| 00DA: BCF 05.7 |
| .................... }; |
| .................... if (line==R3) |
| 00DB: MOVF 2B,W |
| 00DC: SUBLW FD |
| 00DD: BTFSS 03.2 |
| 00DE: GOTO 109 |
| .................... { |
| .................... GO(R,B,160);GO(L,F,160); |
| 00DF: MOVF 01,W |
| 00E0: SUBLW A0 |
| 00E1: BTFSS 03.0 |
| 00E2: GOTO 0EC |
| 00E3: BSF 03.5 |
| 00E4: BCF 05.6 |
| 00E5: BCF 03.5 |
| 00E6: BCF 05.6 |
| 00E7: BSF 03.5 |
| 00E8: BCF 05.7 |
| 00E9: BCF 03.5 |
| 00EA: BSF 05.7 |
| 00EB: GOTO 0F4 |
| 00EC: BSF 03.5 |
| 00ED: BCF 05.6 |
| 00EE: BCF 03.5 |
| 00EF: BCF 05.6 |
| 00F0: BSF 03.5 |
| 00F1: BCF 05.7 |
| 00F2: BCF 03.5 |
| 00F3: BCF 05.7 |
| 00F4: MOVF 01,W |
| 00F5: SUBLW A0 |
| 00F6: BTFSS 03.0 |
| 00F7: GOTO 101 |
| 00F8: BSF 03.5 |
| 00F9: BCF 05.1 |
| 00FA: BCF 03.5 |
| 00FB: BCF 05.1 |
| 00FC: BSF 03.5 |
| 00FD: BCF 05.0 |
| 00FE: BCF 03.5 |
| 00FF: BSF 05.0 |
| 0100: GOTO 109 |
| 0101: BSF 03.5 |
| 0102: BCF 05.0 |
| 0103: BCF 03.5 |
| 0104: BCF 05.0 |
| 0105: BSF 03.5 |
| 0106: BCF 05.1 |
| 0107: BCF 03.5 |
| 0108: BCF 05.1 |
| .................... }; |
| .................... if (line==S) {STOPL;STOPR; i++;} else i=0; |
| 0109: MOVF 2B,F |
| 010A: BTFSS 03.2 |
| 010B: GOTO 11E |
| 010C: BSF 03.5 |
| 010D: BCF 05.0 |
| 010E: BCF 03.5 |
| 010F: BCF 05.0 |
| 0110: BSF 03.5 |
| 0111: BCF 05.1 |
| 0112: BCF 03.5 |
| 0113: BCF 05.1 |
| 0114: BSF 03.5 |
| 0115: BCF 05.6 |
| 0116: BCF 03.5 |
| 0117: BCF 05.6 |
| 0118: BSF 03.5 |
| 0119: BCF 05.7 |
| 011A: BCF 03.5 |
| 011B: BCF 05.7 |
| 011C: INCF 39,F |
| 011D: GOTO 11F |
| 011E: CLRF 39 |
| .................... |
| .................... if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| 011F: MOVF 39,W |
| 0120: SUBLW 63 |
| 0121: BTFSS 03.0 |
| 0122: GOTO 159 |
| .................... |
| .................... output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| 0123: BSF 03.5 |
| 0124: BCF 06.0 |
| 0125: BCF 03.5 |
| 0126: BCF 06.0 |
| .................... sensors = spi_read(0); // cteni senzoru |
| 0127: MOVF 13,W |
| 0128: CLRF 13 |
| 0129: BSF 03.5 |
| 012A: BTFSC 14.0 |
| 012B: GOTO 12E |
| 012C: BCF 03.5 |
| 012D: GOTO 129 |
| 012E: BCF 03.5 |
| 012F: MOVF 13,W |
| 0130: MOVWF 2A |
| .................... sensors=~sensors; |
| 0131: COMF 2A,F |
| .................... output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| 0132: BSF 03.5 |
| 0133: BCF 06.0 |
| 0134: BCF 03.5 |
| 0135: BSF 06.0 |
| .................... |
| .................... if(bit_test(sensors,3)) //...|...// |
| 0136: BTFSS 2A.3 |
| 0137: GOTO 13A |
| .................... { |
| .................... line=S; |
| 0138: CLRF 2B |
| .................... continue; |
| 0139: GOTO 0AA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| 013A: BTFSS 2A.0 |
| 013B: GOTO 13F |
| .................... { |
| .................... line=L3; |
| 013C: MOVLW 03 |
| 013D: MOVWF 2B |
| .................... continue; |
| 013E: GOTO 0AA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,6)) //......|// |
| 013F: BTFSS 2A.6 |
| 0140: GOTO 144 |
| .................... { |
| .................... line=R3; |
| 0141: MOVLW FD |
| 0142: MOVWF 2B |
| .................... continue; |
| 0143: GOTO 0AA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,1)) //.|.....// |
| 0144: BTFSS 2A.1 |
| 0145: GOTO 149 |
| .................... { |
| .................... line=L3; |
| 0146: MOVLW 03 |
| 0147: MOVWF 2B |
| .................... continue; |
| 0148: GOTO 0AA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,5)) //.....|.// |
| 0149: BTFSS 2A.5 |
| 014A: GOTO 14E |
| .................... { |
| .................... line=R3; |
| 014B: MOVLW FD |
| 014C: MOVWF 2B |
| .................... continue; |
| 014D: GOTO 0AA |
| .................... } |
| .................... |
| .................... if (bit_test(sensors,2)) //..|....// |
| 014E: BTFSS 2A.2 |
| 014F: GOTO 153 |
| .................... { |
| .................... line=L3; |
| 0150: MOVLW 03 |
| 0151: MOVWF 2B |
| .................... continue; |
| 0152: GOTO 0AA |
| .................... } |
| .................... |
| .................... if (bit_test(sensors,4)) //....|..// |
| 0153: BTFSS 2A.4 |
| 0154: GOTO 158 |
| .................... { |
| .................... line=R3; |
| 0155: MOVLW FD |
| 0156: MOVWF 2B |
| .................... continue; |
| 0157: GOTO 0AA |
| .................... } |
| .................... } |
| 0158: GOTO 0AA |
| .................... } |
| 0159: RETLW 00 |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| .................... void objizdka() |
| .................... { |
| .................... FL;BR; // doprava 90 |
| 015A: BSF 03.5 |
| 015B: BCF 05.1 |
| 015C: BCF 03.5 |
| 015D: BCF 05.1 |
| 015E: BSF 03.5 |
| 015F: BCF 05.0 |
| 0160: BCF 03.5 |
| 0161: BSF 05.0 |
| 0162: BSF 03.5 |
| 0163: BCF 05.6 |
| 0164: BCF 03.5 |
| 0165: BCF 05.6 |
| 0166: BSF 03.5 |
| 0167: BCF 05.7 |
| 0168: BCF 03.5 |
| 0169: BSF 05.7 |
| .................... Delay_ms(300); |
| 016A: MOVLW 02 |
| 016B: MOVWF 39 |
| 016C: MOVLW 96 |
| 016D: MOVWF 3A |
| 016E: CALL 065 |
| 016F: DECFSZ 39,F |
| 0170: GOTO 16C |
| .................... BL;FR; // brzdi z toceni |
| 0171: BSF 03.5 |
| 0172: BCF 05.0 |
| 0173: BCF 03.5 |
| 0174: BCF 05.0 |
| 0175: BSF 03.5 |
| 0176: BCF 05.1 |
| 0177: BCF 03.5 |
| 0178: BSF 05.1 |
| 0179: BSF 03.5 |
| 017A: BCF 05.7 |
| 017B: BCF 03.5 |
| 017C: BCF 05.7 |
| 017D: BSF 03.5 |
| 017E: BCF 05.6 |
| 017F: BCF 03.5 |
| 0180: BSF 05.6 |
| .................... Delay_ms(100); |
| 0181: MOVLW 64 |
| 0182: MOVWF 3A |
| 0183: CALL 065 |
| .................... STOPL;STOPR; |
| 0184: BSF 03.5 |
| 0185: BCF 05.0 |
| 0186: BCF 03.5 |
| 0187: BCF 05.0 |
| 0188: BSF 03.5 |
| 0189: BCF 05.1 |
| 018A: BCF 03.5 |
| 018B: BCF 05.1 |
| 018C: BSF 03.5 |
| 018D: BCF 05.6 |
| 018E: BCF 03.5 |
| 018F: BCF 05.6 |
| 0190: BSF 03.5 |
| 0191: BCF 05.7 |
| 0192: BCF 03.5 |
| 0193: BCF 05.7 |
| .................... |
| .................... uhel=STRED; |
| 0194: MOVLW 80 |
| 0195: MOVWF 2C |
| .................... FL;FR; |
| 0196: BSF 03.5 |
| 0197: BCF 05.1 |
| 0198: BCF 03.5 |
| 0199: BCF 05.1 |
| 019A: BSF 03.5 |
| 019B: BCF 05.0 |
| 019C: BCF 03.5 |
| 019D: BSF 05.0 |
| 019E: BSF 03.5 |
| 019F: BCF 05.7 |
| 01A0: BCF 03.5 |
| 01A1: BCF 05.7 |
| 01A2: BSF 03.5 |
| 01A3: BCF 05.6 |
| 01A4: BCF 03.5 |
| 01A5: BSF 05.6 |
| .................... Delay_ms(380); // rovne |
| 01A6: MOVLW 02 |
| 01A7: MOVWF 39 |
| 01A8: MOVLW BE |
| 01A9: MOVWF 3A |
| 01AA: CALL 065 |
| 01AB: DECFSZ 39,F |
| 01AC: GOTO 1A8 |
| .................... |
| .................... uhel=STRED-55; |
| 01AD: MOVLW 49 |
| 01AE: MOVWF 2C |
| .................... STOPL;FR; |
| 01AF: BSF 03.5 |
| 01B0: BCF 05.0 |
| 01B1: BCF 03.5 |
| 01B2: BCF 05.0 |
| 01B3: BSF 03.5 |
| 01B4: BCF 05.1 |
| 01B5: BCF 03.5 |
| 01B6: BCF 05.1 |
| 01B7: BSF 03.5 |
| 01B8: BCF 05.7 |
| 01B9: BCF 03.5 |
| 01BA: BCF 05.7 |
| 01BB: BSF 03.5 |
| 01BC: BCF 05.6 |
| 01BD: BCF 03.5 |
| 01BE: BSF 05.6 |
| .................... Delay_ms(190); // doleva |
| 01BF: MOVLW BE |
| 01C0: MOVWF 3A |
| 01C1: CALL 065 |
| .................... |
| .................... uhel=STRED; |
| 01C2: MOVLW 80 |
| 01C3: MOVWF 2C |
| .................... FR;FL; |
| 01C4: BSF 03.5 |
| 01C5: BCF 05.7 |
| 01C6: BCF 03.5 |
| 01C7: BCF 05.7 |
| 01C8: BSF 03.5 |
| 01C9: BCF 05.6 |
| 01CA: BCF 03.5 |
| 01CB: BSF 05.6 |
| 01CC: BSF 03.5 |
| 01CD: BCF 05.1 |
| 01CE: BCF 03.5 |
| 01CF: BCF 05.1 |
| 01D0: BSF 03.5 |
| 01D1: BCF 05.0 |
| 01D2: BCF 03.5 |
| 01D3: BSF 05.0 |
| .................... Delay_ms(300); // rovne |
| 01D4: MOVLW 02 |
| 01D5: MOVWF 39 |
| 01D6: MOVLW 96 |
| 01D7: MOVWF 3A |
| 01D8: CALL 065 |
| 01D9: DECFSZ 39,F |
| 01DA: GOTO 1D6 |
| .................... |
| .................... uhel=STRED-55; |
| 01DB: MOVLW 49 |
| 01DC: MOVWF 2C |
| .................... FR;STOPL; |
| 01DD: BSF 03.5 |
| 01DE: BCF 05.7 |
| 01DF: BCF 03.5 |
| 01E0: BCF 05.7 |
| 01E1: BSF 03.5 |
| 01E2: BCF 05.6 |
| 01E3: BCF 03.5 |
| 01E4: BSF 05.6 |
| 01E5: BSF 03.5 |
| 01E6: BCF 05.0 |
| 01E7: BCF 03.5 |
| 01E8: BCF 05.0 |
| 01E9: BSF 03.5 |
| 01EA: BCF 05.1 |
| 01EB: BCF 03.5 |
| 01EC: BCF 05.1 |
| .................... Delay_ms(190); // doleva |
| 01ED: MOVLW BE |
| 01EE: MOVWF 3A |
| 01EF: CALL 065 |
| .................... |
| .................... uhel=STRED; |
| 01F0: MOVLW 80 |
| 01F1: MOVWF 2C |
| .................... FR;FL; |
| 01F2: BSF 03.5 |
| 01F3: BCF 05.7 |
| 01F4: BCF 03.5 |
| 01F5: BCF 05.7 |
| 01F6: BSF 03.5 |
| 01F7: BCF 05.6 |
| 01F8: BCF 03.5 |
| 01F9: BSF 05.6 |
| 01FA: BSF 03.5 |
| 01FB: BCF 05.1 |
| 01FC: BCF 03.5 |
| 01FD: BCF 05.1 |
| 01FE: BSF 03.5 |
| 01FF: BCF 05.0 |
| 0200: BCF 03.5 |
| 0201: BSF 05.0 |
| .................... Delay_ms(100); // rovne |
| 0202: MOVLW 64 |
| 0203: MOVWF 3A |
| 0204: CALL 065 |
| .................... |
| .................... While((sensors & 0b11111110)!=0) //dokud neni cara |
| .................... { |
| 0205: MOVF 2A,W |
| 0206: ANDLW FE |
| 0207: BTFSC 03.2 |
| 0208: GOTO 218 |
| .................... sensors = spi_read(0); // cteni senzoru |
| 0209: MOVF 13,W |
| 020A: CLRF 13 |
| 020B: BSF 03.5 |
| 020C: BTFSC 14.0 |
| 020D: GOTO 210 |
| 020E: BCF 03.5 |
| 020F: GOTO 20B |
| 0210: BCF 03.5 |
| 0211: MOVF 13,W |
| 0212: MOVWF 2A |
| .................... sensors=~sensors; |
| 0213: COMF 2A,F |
| .................... Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| 0214: MOVLW 03 |
| 0215: MOVWF 3A |
| 0216: CALL 065 |
| .................... } |
| 0217: GOTO 205 |
| .................... BL;BR; // zabrzdi |
| 0218: BSF 03.5 |
| 0219: BCF 05.0 |
| 021A: BCF 03.5 |
| 021B: BCF 05.0 |
| 021C: BSF 03.5 |
| 021D: BCF 05.1 |
| 021E: BCF 03.5 |
| 021F: BSF 05.1 |
| 0220: BSF 03.5 |
| 0221: BCF 05.6 |
| 0222: BCF 03.5 |
| 0223: BCF 05.6 |
| 0224: BSF 03.5 |
| 0225: BCF 05.7 |
| 0226: BCF 03.5 |
| 0227: BSF 05.7 |
| .................... Delay_ms(250); |
| 0228: MOVLW FA |
| 0229: MOVWF 3A |
| 022A: CALL 065 |
| .................... |
| .................... uhel=KOLMO1; |
| 022B: MOVLW E1 |
| 022C: MOVWF 2C |
| .................... FL;BR; // doprava |
| 022D: BSF 03.5 |
| 022E: BCF 05.1 |
| 022F: BCF 03.5 |
| 0230: BCF 05.1 |
| 0231: BSF 03.5 |
| 0232: BCF 05.0 |
| 0233: BCF 03.5 |
| 0234: BSF 05.0 |
| 0235: BSF 03.5 |
| 0236: BCF 05.6 |
| 0237: BCF 03.5 |
| 0238: BCF 05.6 |
| 0239: BSF 03.5 |
| 023A: BCF 05.7 |
| 023B: BCF 03.5 |
| 023C: BSF 05.7 |
| .................... While((sensors & 0b11111110)!=0) //dokud neni cara |
| .................... { |
| 023D: MOVF 2A,W |
| 023E: ANDLW FE |
| 023F: BTFSC 03.2 |
| 0240: GOTO 250 |
| .................... sensors = spi_read(0); // cteni senzoru |
| 0241: MOVF 13,W |
| 0242: CLRF 13 |
| 0243: BSF 03.5 |
| 0244: BTFSC 14.0 |
| 0245: GOTO 248 |
| 0246: BCF 03.5 |
| 0247: GOTO 243 |
| 0248: BCF 03.5 |
| 0249: MOVF 13,W |
| 024A: MOVWF 2A |
| .................... sensors=~sensors; |
| 024B: COMF 2A,F |
| .................... Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| 024C: MOVLW 03 |
| 024D: MOVWF 3A |
| 024E: CALL 065 |
| .................... } |
| 024F: GOTO 23D |
| .................... STOPL;STOPR; // zabrzdi |
| 0250: BSF 03.5 |
| 0251: BCF 05.0 |
| 0252: BCF 03.5 |
| 0253: BCF 05.0 |
| 0254: BSF 03.5 |
| 0255: BCF 05.1 |
| 0256: BCF 03.5 |
| 0257: BCF 05.1 |
| 0258: BSF 03.5 |
| 0259: BCF 05.6 |
| 025A: BCF 03.5 |
| 025B: BCF 05.6 |
| 025C: BSF 03.5 |
| 025D: BCF 05.7 |
| 025E: BCF 03.5 |
| 025F: BCF 05.7 |
| .................... Delay_ms(300); |
| 0260: MOVLW 02 |
| 0261: MOVWF 39 |
| 0262: MOVLW 96 |
| 0263: MOVWF 3A |
| 0264: CALL 065 |
| 0265: DECFSZ 39,F |
| 0266: GOTO 262 |
| .................... // delay_ms(250); |
| .................... |
| .................... line=R3; |
| 0267: MOVLW FD |
| 0268: MOVWF 2B |
| .................... cikcak(); |
| 0269: CALL 07D |
| .................... } |
| 026A: BCF 0A.3 |
| 026B: GOTO 3CF (RETURN) |
| .................... |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////////// |
| .................... void main() |
| .................... { |
| 026C: CLRF 04 |
| 026D: MOVLW 1F |
| 026E: ANDWF 03,F |
| 026F: MOVLW 70 |
| 0270: BSF 03.5 |
| 0271: MOVWF 0F |
| 0272: BCF 1F.4 |
| 0273: BCF 1F.5 |
| 0274: MOVF 1B,W |
| 0275: ANDLW 80 |
| 0276: MOVWF 1B |
| 0277: MOVLW 07 |
| 0278: MOVWF 1C |
| .................... |
| .................... unsigned int8 n; |
| .................... unsigned int8 i,j; |
| .................... unsigned int8 last_sensors; |
| .................... unsigned int8 RozumnaRychlost; |
| .................... |
| .................... setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka |
| * |
| 027B: BSF 03.5 |
| 027C: BCF 1F.4 |
| 027D: BCF 1F.5 |
| 027E: MOVF 1B,W |
| 027F: ANDLW 80 |
| 0280: IORLW 74 |
| 0281: MOVWF 1B |
| .................... setup_adc(ADC_CLOCK_INTERNAL); |
| 0282: BCF 1F.6 |
| 0283: BCF 03.5 |
| 0284: BSF 1F.6 |
| 0285: BSF 1F.7 |
| 0286: BSF 03.5 |
| 0287: BCF 1F.7 |
| 0288: BCF 03.5 |
| 0289: BSF 1F.0 |
| .................... setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16); |
| 028A: BCF 14.5 |
| 028B: BSF 03.5 |
| 028C: BCF 06.2 |
| 028D: BSF 06.1 |
| 028E: BCF 06.4 |
| 028F: MOVLW 31 |
| 0290: BCF 03.5 |
| 0291: MOVWF 14 |
| 0292: MOVLW 00 |
| 0293: BSF 03.5 |
| 0294: MOVWF 14 |
| .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| 0295: MOVF 01,W |
| 0296: ANDLW C7 |
| 0297: IORLW 08 |
| 0298: MOVWF 01 |
| .................... setup_timer_1(T1_DISABLED|T1_DIV_BY_8); |
| 0299: MOVLW 30 |
| 029A: BCF 03.5 |
| 029B: MOVWF 10 |
| .................... setup_timer_2(T2_DIV_BY_16,140,16); |
| 029C: MOVLW 78 |
| 029D: MOVWF 78 |
| 029E: IORLW 06 |
| 029F: MOVWF 12 |
| 02A0: MOVLW 8C |
| 02A1: BSF 03.5 |
| 02A2: MOVWF 12 |
| .................... setup_oscillator(OSC_8MHZ|OSC_INTRC); |
| 02A3: MOVLW 72 |
| 02A4: MOVWF 0F |
| .................... |
| .................... STOPR; STOPL; // zastav motory |
| 02A5: BCF 05.6 |
| 02A6: BCF 03.5 |
| 02A7: BCF 05.6 |
| 02A8: BSF 03.5 |
| 02A9: BCF 05.7 |
| 02AA: BCF 03.5 |
| 02AB: BCF 05.7 |
| 02AC: BSF 03.5 |
| 02AD: BCF 05.0 |
| 02AE: BCF 03.5 |
| 02AF: BCF 05.0 |
| 02B0: BSF 03.5 |
| 02B1: BCF 05.1 |
| 02B2: BCF 03.5 |
| 02B3: BCF 05.1 |
| .................... Lmotor=0;Rmotor=0; |
| 02B4: CLRF 31 |
| 02B5: CLRF 30 |
| 02B6: CLRF 33 |
| 02B7: CLRF 32 |
| .................... |
| .................... uhel = STRED; // nastav zadni kolecko na stred |
| 02B8: MOVLW 80 |
| 02B9: MOVWF 2C |
| .................... rovinka = 0; |
| 02BA: CLRF 2F |
| .................... |
| .................... enable_interrupts(INT_TIMER2); |
| 02BB: BSF 03.5 |
| 02BC: BSF 0C.1 |
| .................... enable_interrupts(GLOBAL); |
| 02BD: MOVLW C0 |
| 02BE: BCF 03.5 |
| 02BF: IORWF 0B,F |
| .................... |
| .................... output_low(IRTX); // zapni IR vysilac |
| 02C0: BSF 03.5 |
| 02C1: BCF 06.2 |
| 02C2: BCF 03.5 |
| 02C3: BCF 06.2 |
| .................... |
| .................... delay_ms(2000); // musime pockat na diagnostiku slave CPU |
| 02C4: MOVLW 08 |
| 02C5: MOVWF 39 |
| 02C6: MOVLW FA |
| 02C7: MOVWF 3A |
| 02C8: CALL 065 |
| 02C9: DECFSZ 39,F |
| 02CA: GOTO 2C6 |
| .................... |
| .................... //nastaveni rychlosti |
| .................... set_adc_channel(CERVENA); |
| 02CB: MOVLW 20 |
| 02CC: MOVWF 78 |
| 02CD: MOVF 1F,W |
| 02CE: ANDLW C7 |
| 02CF: IORWF 78,W |
| 02D0: MOVWF 1F |
| .................... |
| .................... Delay_ms(1); |
| 02D1: MOVLW 01 |
| 02D2: MOVWF 3A |
| 02D3: CALL 065 |
| .................... RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63 |
| 02D4: BSF 1F.2 |
| 02D5: BTFSC 1F.2 |
| 02D6: GOTO 2D5 |
| 02D7: MOVF 1E,W |
| 02D8: MOVWF 77 |
| 02D9: RRF 77,F |
| 02DA: RRF 77,F |
| 02DB: MOVLW 3F |
| 02DC: ANDWF 77,F |
| 02DD: MOVF 77,W |
| 02DE: ADDLW 64 |
| 02DF: MOVWF 38 |
| .................... set_adc_channel(MODRA); |
| 02E0: MOVLW 10 |
| 02E1: MOVWF 78 |
| 02E2: MOVF 1F,W |
| 02E3: ANDLW C7 |
| 02E4: IORWF 78,W |
| 02E5: MOVWF 1F |
| .................... Delay_ms(1); |
| 02E6: MOVLW 01 |
| 02E7: MOVWF 3A |
| 02E8: CALL 065 |
| .................... turn=speed-32+(read_adc()>>2); // rychlost toceni +-32; kroutitko dava 0-63 |
| 02E9: MOVLW 20 |
| 02EA: SUBWF 2D,W |
| 02EB: MOVWF 39 |
| 02EC: BSF 1F.2 |
| 02ED: BTFSC 1F.2 |
| 02EE: GOTO 2ED |
| 02EF: MOVF 1E,W |
| 02F0: MOVWF 77 |
| 02F1: RRF 77,F |
| 02F2: RRF 77,F |
| 02F3: MOVLW 3F |
| 02F4: ANDWF 77,F |
| 02F5: MOVF 77,W |
| 02F6: ADDWF 39,W |
| 02F7: MOVWF 2E |
| .................... |
| .................... speed=R17; |
| 02F8: MOVLW C8 |
| 02F9: MOVWF 2D |
| .................... |
| .................... while(true) |
| .................... { |
| .................... |
| .................... GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor |
| 02FA: MOVF 01,W |
| 02FB: BTFSS 00.7 |
| 02FC: GOTO 300 |
| 02FD: BTFSS 31.7 |
| 02FE: GOTO 308 |
| 02FF: GOTO 302 |
| 0300: BTFSC 31.7 |
| 0301: GOTO 311 |
| 0302: MOVF 31,F |
| 0303: BTFSS 03.2 |
| 0304: GOTO 308 |
| 0305: SUBWF 30,W |
| 0306: BTFSS 03.0 |
| 0307: GOTO 311 |
| 0308: BSF 03.5 |
| 0309: BCF 05.1 |
| 030A: BCF 03.5 |
| 030B: BCF 05.1 |
| 030C: BSF 03.5 |
| 030D: BCF 05.0 |
| 030E: BCF 03.5 |
| 030F: BSF 05.0 |
| 0310: GOTO 319 |
| 0311: BSF 03.5 |
| 0312: BCF 05.0 |
| 0313: BCF 03.5 |
| 0314: BCF 05.0 |
| 0315: BSF 03.5 |
| 0316: BCF 05.1 |
| 0317: BCF 03.5 |
| 0318: BCF 05.1 |
| 0319: MOVF 01,W |
| 031A: BTFSS 00.7 |
| 031B: GOTO 31F |
| 031C: BTFSS 33.7 |
| 031D: GOTO 327 |
| 031E: GOTO 321 |
| 031F: BTFSC 33.7 |
| 0320: GOTO 330 |
| 0321: MOVF 33,F |
| 0322: BTFSS 03.2 |
| 0323: GOTO 327 |
| 0324: SUBWF 32,W |
| 0325: BTFSS 03.0 |
| 0326: GOTO 330 |
| 0327: BSF 03.5 |
| 0328: BCF 05.7 |
| 0329: BCF 03.5 |
| 032A: BCF 05.7 |
| 032B: BSF 03.5 |
| 032C: BCF 05.6 |
| 032D: BCF 03.5 |
| 032E: BSF 05.6 |
| 032F: GOTO 338 |
| 0330: BSF 03.5 |
| 0331: BCF 05.6 |
| 0332: BCF 03.5 |
| 0333: BCF 05.6 |
| 0334: BSF 03.5 |
| 0335: BCF 05.7 |
| 0336: BCF 03.5 |
| 0337: BCF 05.7 |
| .................... |
| .................... delay_us(2000); // cekani na SLAVE, nez pripravi data od cidel |
| 0338: CLRWDT |
| 0339: MOVLW 01 |
| 033A: MOVWF 3A |
| 033B: CALL 065 |
| 033C: MOVLW 09 |
| 033D: MOVWF 39 |
| 033E: CLRF 29 |
| 033F: BTFSC 0B.7 |
| 0340: BSF 29.7 |
| 0341: BCF 0B.7 |
| 0342: MOVLW 6D |
| 0343: MOVWF 3D |
| 0344: CALL 037 |
| 0345: BTFSC 29.7 |
| 0346: BSF 0B.7 |
| 0347: DECFSZ 39,F |
| 0348: GOTO 33E |
| .................... |
| .................... last_sensors=sensors; |
| 0349: MOVF 2A,W |
| 034A: MOVWF 37 |
| .................... |
| .................... output_low(STROBE); // vypni zobrazovani na posuvnem registru |
| 034B: BSF 03.5 |
| 034C: BCF 06.0 |
| 034D: BCF 03.5 |
| 034E: BCF 06.0 |
| .................... sensors = spi_read(0); // cteni senzoru |
| 034F: MOVF 13,W |
| 0350: CLRF 13 |
| 0351: BSF 03.5 |
| 0352: BTFSC 14.0 |
| 0353: GOTO 356 |
| 0354: BCF 03.5 |
| 0355: GOTO 351 |
| 0356: BCF 03.5 |
| 0357: MOVF 13,W |
| 0358: MOVWF 2A |
| .................... sensors=~sensors; // neguj prijata data |
| 0359: COMF 2A,F |
| .................... output_high(STROBE); // zobraz data na posuvnem registru |
| 035A: BSF 03.5 |
| 035B: BCF 06.0 |
| 035C: BCF 03.5 |
| 035D: BSF 06.0 |
| .................... |
| .................... i=0; // havarijni kod |
| 035E: CLRF 35 |
| .................... for (n=0; n<=6; n++) |
| 035F: CLRF 34 |
| 0360: MOVF 34,W |
| 0361: SUBLW 06 |
| 0362: BTFSS 03.0 |
| 0363: GOTO 372 |
| .................... { |
| .................... if(bit_test(sensors,n)) i++; |
| 0364: MOVF 2A,W |
| 0365: MOVWF 77 |
| 0366: MOVF 34,W |
| 0367: MOVWF 78 |
| 0368: BTFSC 03.2 |
| 0369: GOTO 36E |
| 036A: BCF 03.0 |
| 036B: RRF 77,F |
| 036C: DECFSZ 78,F |
| 036D: GOTO 36A |
| 036E: BTFSC 77.0 |
| 036F: INCF 35,F |
| .................... } |
| 0370: INCF 34,F |
| 0371: GOTO 360 |
| .................... if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly |
| 0372: MOVF 35,W |
| 0373: SUBLW 04 |
| 0374: BTFSC 03.0 |
| 0375: GOTO 39E |
| .................... { |
| .................... BL; BR; |
| 0376: BSF 03.5 |
| 0377: BCF 05.0 |
| 0378: BCF 03.5 |
| 0379: BCF 05.0 |
| 037A: BSF 03.5 |
| 037B: BCF 05.1 |
| 037C: BCF 03.5 |
| 037D: BSF 05.1 |
| 037E: BSF 03.5 |
| 037F: BCF 05.6 |
| 0380: BCF 03.5 |
| 0381: BCF 05.6 |
| 0382: BSF 03.5 |
| 0383: BCF 05.7 |
| 0384: BCF 03.5 |
| 0385: BSF 05.7 |
| .................... delay_ms(300); |
| 0386: MOVLW 02 |
| 0387: MOVWF 39 |
| 0388: MOVLW 96 |
| 0389: MOVWF 3A |
| 038A: CALL 065 |
| 038B: DECFSZ 39,F |
| 038C: GOTO 388 |
| .................... STOPR; STOPL; |
| 038D: BSF 03.5 |
| 038E: BCF 05.6 |
| 038F: BCF 03.5 |
| 0390: BCF 05.6 |
| 0391: BSF 03.5 |
| 0392: BCF 05.7 |
| 0393: BCF 03.5 |
| 0394: BCF 05.7 |
| 0395: BSF 03.5 |
| 0396: BCF 05.0 |
| 0397: BCF 03.5 |
| 0398: BCF 05.0 |
| 0399: BSF 03.5 |
| 039A: BCF 05.1 |
| 039B: BCF 03.5 |
| 039C: BCF 05.1 |
| .................... While(true); |
| 039D: GOTO 39D |
| .................... }; |
| .................... |
| .................... /* |
| .................... if (!input(CIHLA)) // dalkova detekce cihly |
| .................... { |
| .................... speed=PRED_CIHLOU; |
| .................... } |
| .................... else |
| .................... { |
| .................... speed=RozumnaRychlost; |
| .................... } |
| .................... */ |
| .................... if (bit_test(sensors,7)) // detekce cihly |
| 039E: BTFSS 2A.7 |
| 039F: GOTO 3CF |
| .................... { |
| .................... BR;BL; |
| 03A0: BSF 03.5 |
| 03A1: BCF 05.6 |
| 03A2: BCF 03.5 |
| 03A3: BCF 05.6 |
| 03A4: BSF 03.5 |
| 03A5: BCF 05.7 |
| 03A6: BCF 03.5 |
| 03A7: BSF 05.7 |
| 03A8: BSF 03.5 |
| 03A9: BCF 05.0 |
| 03AA: BCF 03.5 |
| 03AB: BCF 05.0 |
| 03AC: BSF 03.5 |
| 03AD: BCF 05.1 |
| 03AE: BCF 03.5 |
| 03AF: BSF 05.1 |
| .................... Delay_ms(400); |
| 03B0: MOVLW 02 |
| 03B1: MOVWF 39 |
| 03B2: MOVLW C8 |
| 03B3: MOVWF 3A |
| 03B4: CALL 065 |
| 03B5: DECFSZ 39,F |
| 03B6: GOTO 3B2 |
| .................... STOPR;STOPL; |
| 03B7: BSF 03.5 |
| 03B8: BCF 05.6 |
| 03B9: BCF 03.5 |
| 03BA: BCF 05.6 |
| 03BB: BSF 03.5 |
| 03BC: BCF 05.7 |
| 03BD: BCF 03.5 |
| 03BE: BCF 05.7 |
| 03BF: BSF 03.5 |
| 03C0: BCF 05.0 |
| 03C1: BCF 03.5 |
| 03C2: BCF 05.0 |
| 03C3: BSF 03.5 |
| 03C4: BCF 05.1 |
| 03C5: BCF 03.5 |
| 03C6: BCF 05.1 |
| .................... // if (1==IRcheck()) // kontrola, jestli nebylo rusene cidlo |
| .................... { |
| .................... Delay_ms(100); |
| 03C7: MOVLW 64 |
| 03C8: MOVWF 3A |
| 03C9: CALL 065 |
| .................... cikcak(); |
| 03CA: CALL 07D |
| .................... delay_ms(100); |
| 03CB: MOVLW 64 |
| 03CC: MOVWF 3A |
| 03CD: CALL 065 |
| .................... objizdka(); // objede cihlu |
| 03CE: GOTO 15A |
| .................... } |
| .................... } |
| .................... |
| .................... if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl. |
| 03CF: MOVF 2D,W |
| 03D0: SUBWF 38,W |
| 03D1: BTFSS 03.0 |
| 03D2: DECF 2D,F |
| .................... |
| .................... if(bit_test(sensors,3)) //...|...// |
| 03D3: BTFSS 2A.3 |
| 03D4: GOTO 3E7 |
| .................... { |
| .................... uhel=STRED; |
| 03D5: MOVLW 80 |
| 03D6: MOVWF 2C |
| .................... Lmotor=speed; |
| 03D7: CLRF 7A |
| 03D8: MOVF 2D,W |
| 03D9: MOVWF 30 |
| 03DA: MOVF 7A,W |
| 03DB: MOVWF 31 |
| .................... Rmotor=speed; |
| 03DC: CLRF 7A |
| 03DD: MOVF 2D,W |
| 03DE: MOVWF 32 |
| 03DF: MOVF 7A,W |
| 03E0: MOVWF 33 |
| .................... line=S; |
| 03E1: CLRF 2B |
| .................... if (rovinka < 255) rovinka++; |
| 03E2: INCFSZ 2F,W |
| 03E3: GOTO 3E5 |
| 03E4: GOTO 3E6 |
| 03E5: INCF 2F,F |
| .................... continue; |
| 03E6: GOTO 2FA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| 03E7: BTFSS 2A.0 |
| 03E8: GOTO 3F5 |
| .................... { |
| .................... uhel=STRED - BEAR3; |
| 03E9: MOVLW 53 |
| 03EA: MOVWF 2C |
| .................... Lmotor=0; |
| 03EB: CLRF 31 |
| 03EC: CLRF 30 |
| .................... Rmotor=turn; |
| 03ED: CLRF 7A |
| 03EE: MOVF 2E,W |
| 03EF: MOVWF 32 |
| 03F0: MOVF 7A,W |
| 03F1: MOVWF 33 |
| .................... line=L3; |
| 03F2: MOVLW 03 |
| 03F3: MOVWF 2B |
| .................... continue; |
| 03F4: GOTO 2FA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,6)) //......|// |
| 03F5: BTFSS 2A.6 |
| 03F6: GOTO 403 |
| .................... { |
| .................... uhel=STRED + BEAR3; |
| 03F7: MOVLW AD |
| 03F8: MOVWF 2C |
| .................... Rmotor=0; |
| 03F9: CLRF 33 |
| 03FA: CLRF 32 |
| .................... Lmotor=turn; |
| 03FB: CLRF 7A |
| 03FC: MOVF 2E,W |
| 03FD: MOVWF 30 |
| 03FE: MOVF 7A,W |
| 03FF: MOVWF 31 |
| .................... line=R3; |
| 0400: MOVLW FD |
| 0401: MOVWF 2B |
| .................... continue; |
| 0402: GOTO 2FA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,1)) //.|.....// |
| 0403: BTFSS 2A.1 |
| 0404: GOTO 415 |
| .................... { |
| .................... uhel=STRED - BEAR2; |
| 0405: MOVLW 67 |
| 0406: MOVWF 2C |
| .................... Lmotor=speed-70; |
| 0407: MOVLW 46 |
| 0408: SUBWF 2D,W |
| 0409: CLRF 7A |
| 040A: MOVWF 30 |
| 040B: MOVF 7A,W |
| 040C: MOVWF 31 |
| .................... Rmotor=speed; |
| 040D: CLRF 7A |
| 040E: MOVF 2D,W |
| 040F: MOVWF 32 |
| 0410: MOVF 7A,W |
| 0411: MOVWF 33 |
| .................... line=L2; |
| 0412: MOVLW 02 |
| 0413: MOVWF 2B |
| .................... continue; |
| 0414: GOTO 2FA |
| .................... } |
| .................... |
| .................... if(bit_test(sensors,5)) //.....|.// |
| 0415: BTFSS 2A.5 |
| 0416: GOTO 427 |
| .................... { |
| .................... uhel=STRED + BEAR2; |
| 0417: MOVLW 99 |
| 0418: MOVWF 2C |
| .................... Rmotor=speed-70; |
| 0419: MOVLW 46 |
| 041A: SUBWF 2D,W |
| 041B: CLRF 7A |
| 041C: MOVWF 32 |
| 041D: MOVF 7A,W |
| 041E: MOVWF 33 |
| .................... Lmotor=speed; |
| 041F: CLRF 7A |
| 0420: MOVF 2D,W |
| 0421: MOVWF 30 |
| 0422: MOVF 7A,W |
| 0423: MOVWF 31 |
| .................... line=R2; |
| 0424: MOVLW FE |
| 0425: MOVWF 2B |
| .................... continue; |
| 0426: GOTO 2FA |
| .................... } |
| .................... |
| .................... if (bit_test(sensors,2)) //..|....// |
| 0427: BTFSS 2A.2 |
| 0428: GOTO 43D |
| .................... { |
| .................... uhel=STRED - BEAR1; |
| 0429: MOVLW 76 |
| 042A: MOVWF 2C |
| .................... Lmotor=speed-20; |
| 042B: MOVLW 14 |
| 042C: SUBWF 2D,W |
| 042D: CLRF 7A |
| 042E: MOVWF 30 |
| 042F: MOVF 7A,W |
| 0430: MOVWF 31 |
| .................... Rmotor=speed; |
| 0431: CLRF 7A |
| 0432: MOVF 2D,W |
| 0433: MOVWF 32 |
| 0434: MOVF 7A,W |
| 0435: MOVWF 33 |
| .................... line=L2; |
| 0436: MOVLW 02 |
| 0437: MOVWF 2B |
| .................... if (rovinka<255) rovinka++; |
| 0438: INCFSZ 2F,W |
| 0439: GOTO 43B |
| 043A: GOTO 43C |
| 043B: INCF 2F,F |
| .................... continue; |
| 043C: GOTO 2FA |
| .................... } |
| .................... |
| .................... if (bit_test(sensors,4)) //....|..// |
| 043D: BTFSS 2A.4 |
| 043E: GOTO 453 |
| .................... { |
| .................... uhel=STRED + BEAR1; |
| 043F: MOVLW 8A |
| 0440: MOVWF 2C |
| .................... Rmotor=speed-20; |
| 0441: MOVLW 14 |
| 0442: SUBWF 2D,W |
| 0443: CLRF 7A |
| 0444: MOVWF 32 |
| 0445: MOVF 7A,W |
| 0446: MOVWF 33 |
| .................... Lmotor=speed; |
| 0447: CLRF 7A |
| 0448: MOVF 2D,W |
| 0449: MOVWF 30 |
| 044A: MOVF 7A,W |
| 044B: MOVWF 31 |
| .................... line=L2; |
| 044C: MOVLW 02 |
| 044D: MOVWF 2B |
| .................... if (rovinka<255) rovinka++; |
| 044E: INCFSZ 2F,W |
| 044F: GOTO 451 |
| 0450: GOTO 452 |
| 0451: INCF 2F,F |
| .................... continue; |
| 0452: GOTO 2FA |
| .................... } |
| .................... |
| .................... |
| .................... if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate |
| 0453: MOVF 2B,W |
| 0454: SUBLW 03 |
| 0455: BTFSC 03.2 |
| 0456: GOTO 45B |
| 0457: MOVF 2B,W |
| 0458: SUBLW FD |
| 0459: BTFSS 03.2 |
| 045A: GOTO 480 |
| .................... { |
| .................... if (rovinka>50) |
| 045B: MOVF 2F,W |
| 045C: SUBLW 32 |
| 045D: BTFSC 03.0 |
| 045E: GOTO 47D |
| .................... { |
| .................... BL; BR; |
| 045F: BSF 03.5 |
| 0460: BCF 05.0 |
| 0461: BCF 03.5 |
| 0462: BCF 05.0 |
| 0463: BSF 03.5 |
| 0464: BCF 05.1 |
| 0465: BCF 03.5 |
| 0466: BSF 05.1 |
| 0467: BSF 03.5 |
| 0468: BCF 05.6 |
| 0469: BCF 03.5 |
| 046A: BCF 05.6 |
| 046B: BSF 03.5 |
| 046C: BCF 05.7 |
| 046D: BCF 03.5 |
| 046E: BSF 05.7 |
| .................... Delay_ms(100); |
| 046F: MOVLW 64 |
| 0470: MOVWF 3A |
| 0471: CALL 065 |
| .................... if (rovinka > 250 || speed > 170) delay_ms(50); |
| 0472: MOVF 2F,W |
| 0473: SUBLW FA |
| 0474: BTFSS 03.0 |
| 0475: GOTO 47A |
| 0476: MOVF 2D,W |
| 0477: SUBLW AA |
| 0478: BTFSC 03.0 |
| 0479: GOTO 47D |
| 047A: MOVLW 32 |
| 047B: MOVWF 3A |
| 047C: CALL 065 |
| .................... }; |
| .................... rovinka=0; |
| 047D: CLRF 2F |
| .................... speed=R17; |
| 047E: MOVLW C8 |
| 047F: MOVWF 2D |
| .................... }; |
| .................... } |
| 0480: GOTO 2FA |
| .................... } |
| 0481: SLEEP |
| Configuration Fuses: |
| Word 1: 3F38 NOWDT NOPUT MCLR NOBROWNOUT NOLVP NOCPD NOWRT NODEBUG CCPB0 NOPROTECT INTRC_IO |
| Word 2: 3FFC NOFCMEN NOIESO |
| /Designs/ROBOTS/3Orbis/SW/main.PJT |
|---|
| 0,0 → 1,41 |
| [PROJECT] |
| Target=main.HEX |
| Development_Mode= |
| Processor=0x688F |
| ToolSuite=CCS |
| [Directories] |
| Include=C:\Program Files\PICC\devices\;C:\Program Files\PICC\dr |
| Library= |
| LinkerScript= |
| [Target Data] |
| FileList=D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.c |
| BuildTool=C-COMPILER |
| OptionString=+FM |
| AdditionalOptionString= |
| BuildRequired=1 |
| [main.c] |
| Type=4 |
| Path= |
| FileList= |
| BuildTool= |
| OptionString= |
| AdditionalOptionString= |
| [mru-list] |
| 1=main.c |
| [Windows] |
| 0=0000 main.c 0 0 796 451 3 0 |
| [Opened Files] |
| 1=D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.c |
| 2=D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.h |
| 3=C:\Program Files\PICC\devices\16F88.h |
| 4=D:\KAKLIK\programy\PIC_C\roboti\3Orbis\objizdka_centrovani_R.c |
| 5= |
| [Units] |
| Count=1 |
| 1=D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.c (main) |
| /Designs/ROBOTS/3Orbis/SW/main.SYM |
|---|
| 0,0 → 1,76 |
| 015-016 CCP_1 |
| 015 CCP_1_LOW |
| 016 CCP_1_HIGH |
| 020 @INTERRUPT_AREA |
| 021 @INTERRUPT_AREA |
| 022 @INTERRUPT_AREA |
| 023 @INTERRUPT_AREA |
| 024 @INTERRUPT_AREA |
| 025 @INTERRUPT_AREA |
| 026 @INTERRUPT_AREA |
| 027 @INTERRUPT_AREA |
| 028 @INTERRUPT_AREA |
| 029 @INTERRUPT_AREA |
| 02A sensors |
| 02B line |
| 02C uhel |
| 02D speed |
| 02E turn |
| 02F rovinka |
| 030-031 Lmotor |
| 032-033 Rmotor |
| 034 main.n |
| 035 main.i |
| 036 main.j |
| 037 main.last_sensors |
| 038 main.RozumnaRychlost |
| 039 cikcak.i |
| 039 objizdka.@SCRATCH |
| 039 main.@SCRATCH |
| 03A @delay_ms1.P1 |
| 03A cikcak.@SCRATCH |
| 03A main.@SCRATCH |
| 03B TIMER2_isr.n |
| 03C TIMER2_isr.@SCRATCH |
| 03D @delay_us1.P1 |
| 077 @SCRATCH |
| 078 @SCRATCH |
| 078 _RETURN_ |
| 079 @SCRATCH |
| 07A @SCRATCH |
| 07B @SCRATCH |
| 09C.6 C1OUT |
| 09C.7 C2OUT |
| 0065 @delay_ms1 |
| 0037 @delay_us1 |
| 0047 TIMER2_isr |
| 007D cikcak |
| 015A objizdka |
| 026C main |
| 026C @cinit |
| Project Files: |
| D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.c |
| D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.h |
| C:\Program Files\PICC\devices\16F88.h |
| D:\KAKLIK\programy\PIC_C\roboti\3Orbis\objizdka_centrovani_R.c |
| Units: |
| D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.c (main) |
| Compiler Settings: |
| Processor: PIC16F88 |
| Pointer Size: 8 |
| ADC Range: 0-255 |
| Opt Level: 9 |
| Short,Int,Long: 1,8,16 |
| Output Files: |
| Errors: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.err |
| INHX8: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.hex |
| Symbols: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.sym |
| List: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.lst |
| Debug/COFF: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.cof |
| Call Tree: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.tre |
| Statistics: D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.sta |
| /Designs/ROBOTS/3Orbis/SW/main.c |
|---|
| 0,0 → 1,288 |
| #include ".\main.h" |
| #define KOLMO1 225 // predni kolecko sroubem dopredu |
| #define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu |
| #define STRED 128 // sredni poloha zataceciho kolecka |
| #define BEAR1 10//10 // 3 stupne zataceni |
| #define BEAR2 25//25 |
| #define BEAR3 45//45 |
| #define R 100 // Rozumna rychlost |
| #define R17 200 // X nasobek rozumne rychlosti |
| #define PRED_CIHLOU 100 // rychlost pri dalkove detekci cihly |
| //#define L1 1 // cara vlevo |
| #define L2 2 // cara vlevo |
| #define L3 3 // cara vlevo |
| #define S 0 // cara mezi sensory |
| //#define R1 -1 // cara vpravo |
| #define R2 -2 // cara vpravo |
| #define R3 -3 // cara vpravo |
| // servo |
| #define SERVO PIN_B5 |
| // kroutitka |
| #define CERVENA 4 // AN4 |
| //#define CERNA 5 // AN5 |
| //#define ZELENA 6 // AN6 |
| #define MODRA 2 // AN2 |
| // IR |
| #define IRTX PIN_B2 |
| #define CIHLA PIN_A3 |
| //motory |
| #define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred |
| #define FL output_low(PIN_A1); output_high(PIN_A0) |
| #define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad |
| #define BL output_low(PIN_A0); output_high(PIN_A1) |
| #define STOPR output_low(PIN_A6);output_low(PIN_A7) |
| #define STOPL output_low(PIN_A0);output_low(PIN_A1) |
| //HID |
| #define LED1 PIN_B1 //oranzova |
| #define LED2 PIN_B2 //zluta |
| #define STROBE PIN_B0 |
| //#define SW1 PIN_A2 // Motory On/off |
| unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
| signed int8 line = S; // na ktere strane byla detekovana cara |
| //unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
| unsigned int8 uhel; // urcuje aktualni uhel zataceni |
| unsigned int8 speed; // maximalni povolena rychlost |
| unsigned int8 turn; // rychlost toceni |
| unsigned int8 rovinka; // pocitadlo na zjisteni rovinky |
| signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
| signed int16 Rmotor; // a pravem motoru |
| // makro pro PWM pro motory |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} else {stop##motor;} |
| //////////////////////////////////////////////////////////////////////////////// |
| #int_TIMER2 |
| TIMER2_isr() // ovladani serva |
| { |
| unsigned int8 n; |
| output_high(SERVO); |
| delay_us(1000); |
| for(n=uhel; n>0; n--) Delay_us(2); |
| output_low(SERVO); |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| int8 IRcheck() // potvrdi detekci cihly |
| { |
| output_high(IRTX); // vypne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| if(bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
| { |
| output_low(IRTX); // zapne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| if(!bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
| { |
| output_high(IRTX); // vypne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| output_low(IRTX); // zapne vysilac IR |
| if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla |
| } |
| }; |
| output_low(IRTX); // zapne vysilac IR |
| return 0; // vrat 0, kdyz je detekovano ruseni |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| #include ".\objizdka_centrovani_R.c" |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| unsigned int8 n; |
| unsigned int8 i,j; |
| unsigned int8 last_sensors; |
| unsigned int8 RozumnaRychlost; |
| setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED|T1_DIV_BY_8); |
| setup_timer_2(T2_DIV_BY_16,140,16); |
| setup_oscillator(OSC_8MHZ|OSC_INTRC); |
| STOPR; STOPL; // zastav motory |
| Lmotor=0;Rmotor=0; |
| uhel = STRED; // nastav zadni kolecko na stred |
| rovinka = 0; |
| enable_interrupts(INT_TIMER2); |
| enable_interrupts(GLOBAL); |
| output_low(IRTX); // zapni IR vysilac |
| delay_ms(2000); // musime pockat na diagnostiku slave CPU |
| //nastaveni rychlosti |
| set_adc_channel(CERVENA); |
| Delay_ms(1); |
| RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63 |
| set_adc_channel(MODRA); |
| Delay_ms(1); |
| turn=speed-32+(read_adc()>>2); // rychlost toceni +-32; kroutitko dava 0-63 |
| speed=R17; |
| while(true) |
| { |
| GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor |
| delay_us(2000); // cekani na SLAVE, nez pripravi data od cidel |
| last_sensors=sensors; |
| output_low(STROBE); // vypni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; // neguj prijata data |
| output_high(STROBE); // zobraz data na posuvnem registru |
| i=0; // havarijni kod |
| for (n=0; n<=6; n++) |
| { |
| if(bit_test(sensors,n)) i++; |
| } |
| if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly |
| { |
| BL; BR; |
| delay_ms(300); |
| STOPR; STOPL; |
| While(true); |
| }; |
| /* |
| if (!input(CIHLA)) // dalkova detekce cihly |
| { |
| speed=PRED_CIHLOU; |
| } |
| else |
| { |
| speed=RozumnaRychlost; |
| } |
| */ |
| if (bit_test(sensors,7)) // detekce cihly |
| { |
| BR;BL; |
| Delay_ms(400); |
| STOPR;STOPL; |
| // if (1==IRcheck()) // kontrola, jestli nebylo rusene cidlo |
| { |
| Delay_ms(100); |
| cikcak(); |
| delay_ms(100); |
| objizdka(); // objede cihlu |
| } |
| } |
| if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl. |
| if(bit_test(sensors,3)) //...|...// |
| { |
| uhel=STRED; |
| Lmotor=speed; |
| Rmotor=speed; |
| line=S; |
| if (rovinka < 255) rovinka++; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| uhel=STRED - BEAR3; |
| Lmotor=0; |
| Rmotor=turn; |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| uhel=STRED + BEAR3; |
| Rmotor=0; |
| Lmotor=turn; |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| uhel=STRED - BEAR2; |
| Lmotor=speed-70; |
| Rmotor=speed; |
| line=L2; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| uhel=STRED + BEAR2; |
| Rmotor=speed-70; |
| Lmotor=speed; |
| line=R2; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| uhel=STRED - BEAR1; |
| Lmotor=speed-20; |
| Rmotor=speed; |
| line=L2; |
| if (rovinka<255) rovinka++; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| uhel=STRED + BEAR1; |
| Rmotor=speed-20; |
| Lmotor=speed; |
| line=L2; |
| if (rovinka<255) rovinka++; |
| continue; |
| } |
| if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate |
| { |
| if (rovinka>50) |
| { |
| BL; BR; |
| Delay_ms(100); |
| if (rovinka > 250 || speed > 170) delay_ms(50); |
| }; |
| rovinka=0; |
| speed=R17; |
| }; |
| } |
| } |
| /Designs/ROBOTS/3Orbis/SW/main.cof |
|---|
| Cannot display: file marked as a binary type. |
| svn:mime-type = application/octet-stream |
| Property changes: |
| Added: svn:mime-type |
| +application/octet-stream |
| \ No newline at end of property |
| /Designs/ROBOTS/3Orbis/SW/main.err |
|---|
| 0,0 → 1,2 |
| No Errors |
| 0 Errors, 0 Warnings. |
| /Designs/ROBOTS/3Orbis/SW/main.h |
|---|
| 0,0 → 1,18 |
| #include <16F88.h> |
| #device adc=8 |
| #FUSES NOWDT //No Watch Dog Timer |
| #FUSES INTRC_IO |
| #FUSES NOPUT //No Power Up Timer |
| #FUSES MCLR //Master Clear pin enabled |
| #FUSES NOBROWNOUT //Reset when brownout detected |
| #FUSES NOLVP //Low Voltage Programming on B3(PIC16) or B5(PIC18) |
| #FUSES NOCPD //No EE protection |
| #FUSES NOWRT //Program memory not write protected |
| #FUSES NODEBUG //No Debug mode for ICD |
| #FUSES NOPROTECT //Code not protected from reading |
| #FUSES NOFCMEN //Fail-safe clock monitor enabled |
| #FUSES NOIESO //Internal External Switch Over mode enabled |
| #use delay(clock=8000000,RESTART_WDT) |
| /Designs/ROBOTS/3Orbis/SW/main.sta |
|---|
| 0,0 → 1,35 |
| ROM used: 1154 (28%) |
| 1154 (28%) including unused fragments |
| 2 Average locations per line |
| 3 Average locations per statement |
| RAM used: 32 (18%) at main() level |
| 35 (20%) worst case |
| Lines Stmts % Files |
| ----- ----- --- ----- |
| 289 190 49 D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.c |
| 19 0 0 D:\KAKLIK\programy\PIC_C\roboti\3Orbis\main.h |
| 279 0 0 C:\Program Files\PICC\devices\16F88.h |
| 140 200 34 D:\KAKLIK\programy\PIC_C\roboti\3Orbis\objizdka_centrovani_R.c |
| ----- ----- |
| 1454 780 Total |
| Page ROM % RAM Functions: |
| ---- --- --- --- ---------- |
| 0 24 2 1 @delay_ms1 |
| 0 16 1 1 @delay_us1 |
| 0 30 3 2 TIMER2_isr |
| 0 221 19 2 cikcak |
| 0 274 24 1 objizdka |
| 0 534 46 7 main |
| Segment Used Free |
| --------- ---- ---- |
| 00000-00003 4 0 |
| 00004-00036 51 0 |
| 00037-007FF 1099 894 |
| 00800-00FFF 0 2048 |
| /Designs/ROBOTS/3Orbis/SW/main.tre |
|---|
| 0,0 → 1,36 |
| ÀÄmain |
| ÃÄmain 0/534 Ram=7 |
| ³ ÃÄ??0?? |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_us1 0/16 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄcikcak 0/221 Ram=2 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÀÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄobjizdka 0/274 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÀÄcikcak 0/221 Ram=2 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ³ ÀÄ@delay_ms1 0/24 Ram=1 |
| ³ ÃÄ@delay_ms1 0/24 Ram=1 |
| ³ ÀÄ@delay_ms1 0/24 Ram=1 |
| ÀÄTIMER2_isr 0/30 Ram=2 |
| ÀÄ@delay_us1 0/16 Ram=1 |
| /Designs/ROBOTS/3Orbis/SW/objizdka_centrovani.BAK |
|---|
| 0,0 → 1,131 |
| #define DOLEVA 0 |
| #define DOPRAVA 1 |
| void cikcak() |
| { |
| unsigned int8 i=0; |
| uhel=KOLMO1; // aby se dalo tocit na miste |
| Delay_ms(100); |
| if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| if (line==S) line=L3; |
| if (line==R2) line=R3; |
| Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| while(true) |
| { |
| Delay_ms(3); |
| if (line==L3) |
| { |
| GO(L,B,160);GO(R,F,160); |
| }; |
| if (line==R3) |
| { |
| GO(R,B,160);GO(L,F,160); |
| }; |
| if (line==S) {STOPL;STOPR; i++;} else i=0; |
| if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| if(bit_test(sensors,3)) //...|...// |
| { |
| line=S; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| line=R3; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| line=L3; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| line=R3; |
| continue; |
| } |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| BL;FR; |
| Delay_ms(300); |
| FL;BR; |
| Delay_ms(100); |
| STOPL;STOPR; |
| uhel=STRED; |
| FL;FR; |
| Delay_ms(360); // rovne |
| uhel=STRED+55; |
| STOPR;FL; |
| Delay_ms(190); // doprava |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(300); // rovne |
| uhel=STRED+55; |
| FL;STOPR; |
| Delay_ms(190); // doprava |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(60); // rovne |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL;BR; |
| Delay_ms(300); |
| uhel=STRED-55; |
| FR;STOPL; // doleva |
| delay_ms(250); |
| line=L3; |
| cikcak(); |
| } |
| /Designs/ROBOTS/3Orbis/SW/objizdka_centrovani.c |
|---|
| 0,0 → 1,131 |
| #define DOLEVA 0 |
| #define DOPRAVA 1 |
| void cikcak() |
| { |
| unsigned int8 i=0; |
| uhel=KOLMO1; // aby se dalo tocit na miste |
| Delay_ms(100); |
| if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| if (line==S) line=L3; |
| if (line==R2) line=R3; |
| Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| while(true) |
| { |
| Delay_ms(3); |
| if (line==L3) |
| { |
| GO(L,B,160);GO(R,F,160); |
| }; |
| if (line==R3) |
| { |
| GO(R,B,160);GO(L,F,160); |
| }; |
| if (line==S) {STOPL;STOPR; i++;} else i=0; |
| if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| if(bit_test(sensors,3)) //...|...// |
| { |
| line=S; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| line=R3; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| line=L3; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| line=R3; |
| continue; |
| } |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| BL;FR; |
| Delay_ms(300); |
| FL;BR; |
| Delay_ms(100); |
| STOPL;STOPR; |
| uhel=STRED; |
| FL;FR; |
| Delay_ms(360); // rovne |
| uhel=STRED+55; |
| STOPR;FL; |
| Delay_ms(190); // doprava |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(300); // rovne |
| uhel=STRED+55; |
| FL;STOPR; |
| Delay_ms(190); // doprava |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(200); // rovne |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL;BR; |
| Delay_ms(300); |
| uhel=STRED-55; |
| FR;STOPL; // doleva |
| delay_ms(250); |
| line=L3; |
| cikcak(); |
| } |
| /Designs/ROBOTS/3Orbis/SW/objizdka_centrovani_R.BAK |
|---|
| 0,0 → 1,139 |
| #define DOLEVA 0 |
| #define DOPRAVA 1 |
| void cikcak() |
| { |
| unsigned int8 i=0; |
| uhel=KOLMO1; // aby se dalo tocit na miste |
| Delay_ms(100); |
| if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| if (line==S) line=L3; |
| if (line==R2) line=R3; |
| Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| while(true) |
| { |
| Delay_ms(3); |
| if (line==L3) |
| { |
| GO(L,B,160);GO(R,F,160); |
| }; |
| if (line==R3) |
| { |
| GO(R,B,160);GO(L,F,160); |
| }; |
| if (line==S) {STOPL;STOPR; i++;} else i=0; |
| if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| if(bit_test(sensors,3)) //...|...// |
| { |
| line=S; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| line=R3; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| line=L3; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| line=R3; |
| continue; |
| } |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| FL;BR; // doprava 90 |
| Delay_ms(300); |
| BL;FR; // brzdi z toceni |
| Delay_ms(100); |
| STOPL;STOPR; |
| uhel=STRED; |
| FL;FR; |
| Delay_ms(370); // rovne |
| uhel=STRED-55; |
| STOPL;FR; |
| Delay_ms(190); // doleva |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(300); // rovne |
| uhel=STRED-55; |
| FR;STOPL; |
| Delay_ms(190); // doleva |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(100); // rovne |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL;BR; // zabrzdi |
| Delay_ms(250); |
| uhel=KOLMO1; |
| FL;BR; // doprava |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| STOPL;STOPR; // zabrzdi |
| Delay_ms(300); |
| // delay_ms(250); |
| line=R3; |
| cikcak(); |
| } |
| /Designs/ROBOTS/3Orbis/SW/objizdka_centrovani_R.c |
|---|
| 0,0 → 1,139 |
| #define DOLEVA 0 |
| #define DOPRAVA 1 |
| void cikcak() |
| { |
| unsigned int8 i=0; |
| uhel=KOLMO1; // aby se dalo tocit na miste |
| Delay_ms(100); |
| if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| if (line==S) line=L3; |
| if (line==R2) line=R3; |
| Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| while(true) |
| { |
| Delay_ms(3); |
| if (line==L3) |
| { |
| GO(L,B,160);GO(R,F,160); |
| }; |
| if (line==R3) |
| { |
| GO(R,B,160);GO(L,F,160); |
| }; |
| if (line==S) {STOPL;STOPR; i++;} else i=0; |
| if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| if(bit_test(sensors,3)) //...|...// |
| { |
| line=S; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| line=R3; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| line=L3; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| line=R3; |
| continue; |
| } |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| FL;BR; // doprava 90 |
| Delay_ms(300); |
| BL;FR; // brzdi z toceni |
| Delay_ms(100); |
| STOPL;STOPR; |
| uhel=STRED; |
| FL;FR; |
| Delay_ms(380); // rovne |
| uhel=STRED-55; |
| STOPL;FR; |
| Delay_ms(190); // doleva |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(300); // rovne |
| uhel=STRED-55; |
| FR;STOPL; |
| Delay_ms(190); // doleva |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(100); // rovne |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL;BR; // zabrzdi |
| Delay_ms(250); |
| uhel=KOLMO1; |
| FL;BR; // doprava |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| STOPL;STOPR; // zabrzdi |
| Delay_ms(300); |
| // delay_ms(250); |
| line=R3; |
| cikcak(); |
| } |
| /Designs/ROBOTS/3Orbis/SW/objizdka_cidla.c |
|---|
| 0,0 → 1,64 |
| void objizdka() |
| { |
| int8 shure=0; |
| unsigned int16 n; |
| // toceni na miste dokud nezmizi cihla |
| //------------------------------------ |
| uhel=KOLMO1; // nastav zataceci kolecko kolmo na osu robota |
| Delay_ms(100); |
| BL;FR; |
| Delay_ms(200); // minimalni toceni, kdyby se zastavilo sikmo k cihle |
| While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel |
| } |
| STOPL; STOPR; |
| for (n=0;n<1000;n++) // vystred se na hranu cihly |
| { |
| if(!input(CIHLA)) |
| { |
| GO(L,B,180);GO(R,F,160); // zapni motory PWM podle promenych Lmotor a Rmotor |
| } else |
| { |
| GO(L,F,180);GO(R,B,160); // zapni motory PWM podle promenych Lmotor a Rmotor |
| }; |
| delay_ms(1); |
| } |
| STOPR;STOPL; |
| uhel=STRED; // dopredu |
| delay_ms(100); |
| FR; FL; |
| delay_ms(500); |
| BL;BR; |
| delay_ms(200); |
| STOPL;STOPR; |
| uhel=STRED+BEAR3; // doprava |
| delay_ms(100); |
| FL; |
| delay_ms(400); |
| uhel=STRED+BEAR2; // min doprava |
| FL;FR; |
| delay_ms(100); |
| uhel=STRED+BEAR1; // jeste min doprava |
| FL;FR; |
| delay_ms(200); |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL; BR; |
| delay_ms(400); |
| uhel=STRED-BEAR3; // doleva |
| } |
| //////////////////////////////////////////////////////////////////////////////// |