| 0,0 → 1,170 |
|---|
| #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 LMAX 190 // maximalni zatoceni doleva |
| #define RMAX 60 // maximalni zatoceni doprava |
| #define STRED 128 // sredni poloha zadniho kolecka |
| |
| // 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(1200); |
| for(n=uhel; n>0; n--); |
| output_low(SERVO); |
| } |
| |
| //////////////////////////////////////////////////////////////////////////////// |
| #task (rate=2ms, max=500us) |
| void motory() // umoznuje nezavisle rizeni vykonu na obou motorech |
| { |
| |
| if(Rmotor>0) //pravy motor |
| { |
| GO(R, F, Rmotor); |
| } |
| |
| if(Rmotor==0) STOPR; |
| |
| if(Rmotor<0) |
| { |
| GO(R, B, abs(Rmotor)); |
| } |
| |
| if(Lmotor>0) // levy motor |
| { |
| GO(L, F, Lmotor); |
| } |
| |
| if(Lmotor==0) STOPL; |
| |
| if(Lmotor<0) |
| { |
| GO(L, B, abs(Lmotor)); |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| #task (rate=4ms, max=500us, queue=0) |
| void rizeni() |
| { |
| rizeni: |
| |
| sensors = spi_read(0); // cteni senzoru na caru |
| output_high(STROBE); |
| output_low(STROBE); |
| |
| if(bit_test(sensors,0)) //*......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| uhel=LMAX; |
| Lmotor=0;Rmotor=127; |
| goto rizeni; |
| } |
| if(bit_test(sensors,6)) //......*// |
| { |
| uhel=RMAX; |
| Rmotor=0;Lmotor=127; |
| goto rizeni; |
| } |
| if(bit_test(sensors,1)) //.*.....// |
| { |
| uhel=STRED+30; |
| Lmotor=70;Rmotor=127; |
| goto rizeni; |
| } |
| if(bit_test(sensors,5)) //.....*.// |
| { |
| uhel=STRED-30; |
| Rmotor=70;Lmotor=127; |
| goto rizeni; |
| } |
| if(bit_test(sensors,2)) //..*....// |
| { |
| uhel=STRED+20; |
| Lmotor=90;Rmotor=127; |
| goto rizeni; |
| } |
| if(bit_test(sensors,4)) //....*..// |
| { |
| uhel=STRED-20; |
| Rmotor=90;Lmotor=127; |
| goto rizeni; |
| } |
| if(bit_test(sensors,3)) //...*...// |
| { |
| uhel=STRED; |
| Lmotor=100;Rmotor=100; |
| goto rizeni; |
| } |
| |
| /*STOPL;STOPR; |
| if(bit_test(sensors,7)) |
| { |
| |
| }*/ |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| 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); |
| // set_pwm1_duty(0); // Spust PWM, ale zatim s trvalou 0 na vystupu |
| // setup_ccp1(CCP_PWM); |
| // setup_timer_2(T2_DIV_BY_16,200,1); // perioda |
| // setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
| |
| STOPR; STOPL; |
| uhel=STRED; |
| // diagnostika(); |
| Lmotor=0; |
| Rmotor=0; |
| rtos_run(); |
| |
| } |