| 0,0 → 1,168 |
|---|
| #include ".\main.h" |
| |
| #use rtos (timer=2, minor_cycle=2ms) |
| |
| unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
| unsigned int8 line; // na ktere strane byla detekovana cara |
| unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
| unsigned int8 uhel; // urcuje aktualni uhel zataceni |
| |
| signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
| signed int16 Rmotor; // a pravem motoru |
| |
| #define STRED 128 // sredni poloha zadniho kolecka |
| #define BEAR1 20 // 3 stupne zataceni |
| #define BEAR2 40 |
| #define BEAR3 60 |
| // servo |
| #define SERVO PIN_A2 |
| |
| //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 |
| |
| // makro pro PWM |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} else {stop##motor;} |
| |
| |
| //////////////////////////////////////////////////////////////////////////////// |
| //////////////////////////////////////////////////////////////////////////////// |
| /* |
| void diagnostika() |
| { |
| if(input(SW1))STOPR;STOPL;While(TRUE); |
| // if(LSENSOR==true) output_high(LED2); else output_low(LED2); |
| // if(RSENSOR==true) output_high(LED1); else output_low(LED1); |
| } |
| */ |
| //////////////////////////////////////////////////////////////////////////////// |
| #task (rate=16ms,max=2ms) |
| void zatoc() // ridi servo ktere otaci kolem |
| { |
| unsigned int8 n; |
| |
| output_high(SERVO); |
| delay_us(1100); |
| for(n=uhel; n>0; n--); |
| output_low(SERVO); |
| } |
| |
| |
| void diag() // ridi servo ktere otaci kolem |
| { |
| unsigned int8 n; |
| |
| output_high(SERVO); |
| delay_us(1100); |
| for(n=uhel; n>0; n--); |
| output_low(SERVO); |
| } |
| |
| //////////////////////////////////////////////////////////////////////////////// |
| #task (rate=2ms, max=1ms) |
| void rizeni() |
| { |
| GO(L,F,Lmotor);GO(R,F,Rmotor); |
| |
| delay_us(500); |
| |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru na caru |
| output_high(STROBE); |
| |
| 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=190; |
| return; |
| } |
| |
| if(bit_test(sensors,6)) //......|// |
| { |
| Rmotor=0;Lmotor=190; |
| uhel=STRED-BEAR3; |
| return; |
| |
| } |
| |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| uhel=STRED+BEAR2; |
| Lmotor=150;Rmotor=190; |
| return; |
| } |
| |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| uhel=STRED-BEAR2; |
| Rmotor=150;Lmotor=190; |
| return; |
| } |
| |
| if (bit_test(sensors,2)) //..|....// |
| { |
| uhel=STRED+BEAR1; |
| Lmotor=150;Rmotor=190; |
| return; |
| } |
| |
| if (bit_test(sensors,4)) //....|..// |
| { |
| uhel=STRED-BEAR1; |
| Rmotor=150;Lmotor=190; |
| return; |
| } |
| |
| if(bit_test(sensors,3)) //...|...// |
| { |
| uhel=STRED; |
| Lmotor=190;Rmotor=190; |
| return; |
| } |
| |
| /*STOPL;STOPR; |
| if(bit_test(sensors,7)) |
| { |
| |
| }*/ |
| } |
| |
| |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| unsigned int16 i; |
| |
| setup_adc_ports(NO_ANALOGS); |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED); |
| setup_oscillator(OSC_8MHZ|OSC_INTRC); |
| |
| STOPR; STOPL; |
| uhel=STRED; |
| |
| for(i=0;i<100;i++) |
| { |
| diag(); |
| delay_ms(16); |
| } |
| // diagnostika(); |
| Lmotor=0; |
| Rmotor=0; |
| rtos_run(); |
| |
| } |