Subversion Repositories svnkaklik

#include "main.h"

int movement;     // smer minuleho pohybu

int line;         // na ktere strane byla detekovana cara

unsigned int8 dira;     // pocitadlo pro nalezeni preruseni cary

unsigned int8 speed;    // rychlost zataceni

unsigned int8 straight; // pocitadlo pro zjisteni rovneho useku

// Konstanty pro dynamiku pohybu

#define T_DIRA       16       // po jakem case zataceni se detekuje dira

#define INC_SPEED    5        // prirustek rychlosti v jednom kroku

#define RIGHT_ANGLE  1000     // 90 stupnu

#define CIKCAK       20000    // 45 stupnu

#define BW_PO_DIRE   200      // zpetny chod po dire

#define FW_RYCHLE    200      // cara primo rovne

#define FW_POMALU    100      // trochu mimo caru vnitrni pas

#define FW_STREDNE   150      // trochu mimo caru vnejsi pas

#define TURN_MIN     90       // minimalni rychlost pri zataceni

#define TURN_MAX     150      // miximalni rychlost pri zataceni

#define BRZDENI      40       // doba zpetneho chodu v ms, aby pas stal

#define ROVINKA      8        // doba po kterou se musi jet rovne, aby se brzdilo

//motory            //Napred vypnout potom zapnout!

#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred

#define FL         output_low(PIN_B7); output_high(PIN_B6)

#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad

#define BL         output_low(PIN_B6); output_high(PIN_B7)

#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav

#define STOPL      output_low(PIN_B6);output_low(PIN_B7)

#define L 0b10  // left

#define R 0b01  // right

#define S 0b11  // straight

//cidla

#define RSENSOR    1       // Senzory na caru

#define LSENSOR    0

#define BUMPER     PIN_C4   // Senzor na cihlu

#define DIAG_SERVO      PIN_B0   // Propojka pro diagnosticky mod

#define DIAG_SENSORS    PIN_B1   // Propojka pro diagnosticky mod

#DEFINE SOUND_HI   PIN_B3     // komplementarni vystupy pro piezo pipak

#DEFINE SOUND_LO   PIN_B2

char AXstring[40];   // Buffer pro prenos telemetrie

// makro pro PWM

#define GO(motor, direction, power) if(get_timer0()<=power) \

                                          {direction##motor;} \

                                          else \

                                          {stop##motor;}

#int_TIMER1                        // This function is called every time

void TIMER1_isr() {                // the RTCC (timer0) overflows (255->0).

                                   // For this program this is apx 76 times

                                   // per second.

   if (speed<TURN_MAX) speed+=INC_SPEED;

   if (dira<255) dira++;

   if (straight<255) straight++;

}

// Primitivni Pipani

void beep(unsigned int16 period, unsigned int16 length)

{

   unsigned int16 nn;

   for(nn=length; nn>0; nn--)

   {

     output_high(SOUND_HI);output_low(SOUND_LO);

     delay_us(period);

     output_high(SOUND_LO);output_low(SOUND_HI);

     delay_us(period);

   }

}

void diagnostika()

{

   unsigned int16 n;

   while (input(DIAG_SERVO))   // Propojka, ktera spousti diagnostiku

   {

      for (n=500; n<800; n+=100)

      {

         beep(n,n); //beep UP

      };

      Delay_ms(1000);

      //zastav vse

      STOPL; STOPR;

      //pravy pas

      FR; Delay_ms(1000); STOPR; Delay_ms(1000);

      BR; Delay_ms(1000); STOPR; Delay_ms(1000);

      Beep(880,100); Delay_ms(1000);

      //levy pas

      FL; Delay_ms(1000); STOPL; Delay_ms(1000);

      BL; Delay_ms(1000); STOPL; Delay_ms(1000);

      Beep(880,100); Delay_ms(1000);

      //oba pasy

      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);

      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);

   };

   while (input(DIAG_SENSORS))

   {

      int ls, rs;

		while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}

      set_adc_channel(RSENSOR);

      Delay_us(20);

      rs=read_adc();

      set_adc_channel(LSENSOR);

      Delay_us(20);

      ls=read_adc();

   };

}

void main()

{

   STOPL; STOPR;     // prepne vystupy na ovladani motoru na output a zastavi

   setup_adc_ports(RA0_RA1_RA3_ANALOG);

   setup_adc(ADC_CLOCK_DIV_2);

   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM

   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro regulaci

   setup_timer_2(T2_DISABLED,0,1);

   setup_ccp1(CCP_OFF);

   diagnostika();

   Beep(1000,200);     //double beep

   Delay_ms(50);

   Beep(1000,200);

   Delay_ms(1000);      // 1s

   speed=TURN_MIN;      // povoleni rizeni rychlosti zataceni pres preruseni

   enable_interrupts(INT_TIMER1);

   enable_interrupts(GLOBAL);

// Cik-Cak -------------------------------------------------------------

   line=S;

   movement=R;

 //  cik_cak();     // toc se, abys nasel caru

   Delay_ms(500);

   Beep(1000,200);

   Delay_ms(500);

   dira=0;           // inicializace globalnich promennych

   straight=255;

   while(true)       // hlavni smycka (jizda podle cary)

   {

      set_adc_channel(LSENSOR);								// kdyz cara nebyla pod pravym cidlem, mozna bude pod levym

      Delay_us(10);

      if(tresholdL > read_adc())

		{

            movement = L;

            if (straight>ROVINKA)

             {GO(R, F, FW_STREDNE+40); GO(L, F, FW_POMALU+40)} // pridej

            else

             {GO(R, F, FW_STREDNE); GO(L, F, FW_POMALU)};

            speed=TURN_MIN;

            dira=0;

				line=L;

            continue;

		}		

      set_adc_channel(RSENSOR);								// podivej se jestli neni cara pod pravym cidlem

      Delay_us(10);

      if(tresholdR > read_adc())

		{

            movement = R;

            if (straight>ROVINKA)

             {GO(L, F, FW_STREDNE+40); GO(R, F, FW_POMALU+40)} // pridej

            else

             {GO(L, F, FW_STREDNE); GO(R, F, FW_POMALU)};

            speed=TURN_MIN;

            dira=0;

				line=R;

            continue;

		}

		if(line==S)

            movement = S;

            if (straight>ROVINKA)

             {FL; FR;}  // pokud se jede dlouho rovne, tak pridej

            else

             {GO(R, F, FW_RYCHLE); GO(L, F, FW_RYCHLE)};

            speed=TURN_MIN;   // nastav minimalni rychlost pro zataceni

            dira=0;           // protoze byla cara, tak nuluj pocitadlo diry

            continue;

      if (straight>ROVINKA)     // pokud byla dlouha rovinka, tak zabrzdi

      {

            BL; BR;

            Delay_ms(BRZDENI);

            STOPL;

            STOPR;

            dira=0;

      };

      straight=0;    // nuluj pocitadlo rovinky

      if (L==movement)  // kdyz jsou obe cidla mimo caru, zatoc na caru

      {

         GO(R, F, speed);

         STOPL;

      }

      else

      {

         GO(L, F, speed);

         STOPR;

      }

      if (dira>T_DIRA)  // pokud se moc dlouho zataci bez detekce cary, vrat se

      {

         STOPL;

         STOPR;

         Beep(1000,200);     //double beep

         Delay_ms(30);

         Beep(2000,200);

         Delay_ms(30);

         if (L==movement)  // zpet, podle toho kam se jelo

         {

            STOPL;

            BR;

         }

         else

         {

            STOPR;

            BL;

         };

         Delay_ms(BW_PO_DIRE);

         STOPL;

         STOPR;

         cik_cak();        // najdi caru

         dira=0;

      }

   } // while(true)

}