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(); |
|
} |