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