Subversion Repositories svnkaklik

#include "D:\KAKLIK\programy\PIC_C\roboti\tank\main.h"

//motory
#define FR         output_high(PIN_B4);output_low(PIN_B5)  // Vpred
#define FL         output_high(PIN_B6);output_low(PIN_B7)
#define BR         output_high(PIN_B5);output_low(PIN_B4)  // Vzad
#define BL         output_high(PIN_B7);output_low(PIN_B6)
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)

//cidla
#define RSENSOR    1   // Senzory na caru
#define LSENSOR    0
//#define BUMPER     SENSOR_2   // Senzor na cihlu

//human interface
#DEFINE LED        PIN_A2

#DEFINE SOUND_HI   PIN_B2
#DEFINE SOUND_LO   PIN_B3
#include "sound.c"

#DEFINE TXo        PIN_C0     // Vysilac
#include "C:\library\kaklik\CCS\AX25.c"             // Podprogram pro prenos telemetrie

//program
#define MEZERA     38         // jak se muze jet dlouho bez cary
#define COUVEJ     700         // kolik se ma couvat po detekci diry
#define CIKCAK     40         // kolik se ma jet cik/cak
#define PRES_DIRU  350         // jak predpokladame velkou diru
#define TRESHOLD   50

#define L 0  // left
#define R 1  // right
#define S 2  // straight

#use    fixed_io(b_outputs=PIN_B2, PIN_B3, PIN_B4, PIN_B5, PIN_B6, PIN_B7)
#use    fixed_io(a_outputs=PIN_A2)
#use    fixed_io(c_outputs=PIN_C0)

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

int movement;     // smer minuleho pohybu
int line;         // na ktere strane byla detekovana cara
int dira;         // pocitadlo pro nalezeni preruseni cary
int n;            // pomocna promena pro cyklus FOR

void cik_cak()   // Hledani cary
{
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
  switch(movement)  // zmenime smer zataceni
  {
   case L:
        FL;STOPR;          // doprava
        movement=R;        // poznamenej kam jedem
        line=L;            // kdyz prejedem, tak bude cara vlevo
     break;
   case R:
        FR;STOPL;           // doleva
        movement=L;        // poznamenej kam jedem
        line=R;            // kdyz prejedem, tak bude cara vpravo
     break;
  }

  while(true)   // jed cik-cak, dokud nenajdes caru
  {
    set_adc_channel(RSENSOR);
    if (TRESHOLD > read_adc())   // je cara ?
    {
         STOPL;
         STOPR;
         break;
    };
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
    {
       n=0;
       STOPL;
       STOPR;
       switch(movement)     // zmenime smer zataceni
       {
         case L:
              FL;          // doprava
              movement=R;        // poznamenej kam jedem
              line=L;            // kdyz prejedem, tak bude cara vlevo
           break;
         case R:
              FR;           // doleva
              movement=L;        // poznamenej kam jedem
              line=R;            // kdyz prejedem, tak bude cara vpravo
           break;
       }
    }
  }
}


void diagnostika()
{
   int n;
   while (input(PIN_C5))
   {
      set_adc_channel(RSENSOR);
      Delay_us(2);

      set_adc_channel(LSENSOR);
      Delay_us(2);

      sprintf(AXstring,"Counter: %U\0", n++);  // Convert DATA to String.
      SendPacket(&AXstring[0]);
      delay_ms(1000);
   }

   while (input(PIN_C4))
   {

      for (n=500; n<5000; n+=100)
      {
         SoundBeep(n,50);                        //beep
      };
      Delay_ms(1000);

      STOPL;                     //zastav vse
      STOPR;

      FR;                        //pravy pas
      Delay_ms(1000);
      STOPR;
      Delay_ms(1000);
      BR;
      Delay_ms(1000);
      STOPR;
      Delay_ms(1000);

      FL;                        //levy pas
      Delay_ms(1000);
      STOPL;
      Delay_ms(1000);
      BL;
      Delay_ms(1000);
      STOPL;
      Delay_ms(1000);

      FL;                        //oba pasy
      FR;
      Delay_ms(1000);
      STOPL;
      STOPR;
      Delay_ms(1000);
      BL;
      BR;
      Delay_ms(1000);
      STOPL;
      STOPR;
      Delay_ms(1000);
   }
}

void main()
{
  setup_adc_ports(RA0_RA1_RA3_ANALOG);
  setup_adc(ADC_CLOCK_DIV_2);
  diagnostika();

  SoundBeep(640,200);                        //beep
  Delay_ms(50);
  SoundBeep(640,200);
  Delay_ms(1000); // 1s

  movement=R;
  line=S;
  dira=0;

  //cik_cak();    // toc se, abys nasel caru
  FL; FR;         // vpred

                                                   // Sledovani cary
  while(true)
  {
snimani:
    set_adc_channel(RSENSOR);
    Delay_us(2);
    if (TRESHOLD > read_adc())    // Cara pod pravym senzorem
    {
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
      line=R;    // zaznamenej, kdes videl caru
      FL;FR;
      goto snimani;
    }

    set_adc_channel(LSENSOR);
    Delay_us(2);
    if (TRESHOLD > read_adc())    // Cara pod levym senzorem
    {
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
      line=L;    // zaznamenej, kdes videl caru
      FL;FR;
      goto snimani;
    }

    // oba senzory mimo caru
    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
    {
                           // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
      if(line==L)
      {
        // doleva
//      STOPL;
      FR;
        movement=L;    // poznamenej kam jedes
      }
      if(line==R)
      {
        // doprava
//     STOPR;
        FL;
        movement=R;    // poznamenej kam jedes
      }
      dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
      goto snimani; // co nejrychleji se vrat na snimani cary
    }

    /*if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
    {
      dira=0;
      switch(movement)  // kam se jelo ?
      {
       case L:
            BR;      // couvej doprava
            Delay_ms(COUVEJ);
            movement=R;
         break;
       case R:
            BL;       // couvej doleva
            Delay_ms(COUVEJ);
            movement=L;
         break;
      }
      STOP();                    // konec couvani
      FL; FR;                  // rovne pres diru
      Delay_ms(PRES_DIRU);
      STOP();
      cik_cak();                // najdi caru
      FR; FL;                  // vpred
    } // dira*/

  } // while(true)

}

/*void objizdka()
{
  BL; BR;                 // zacouvej
  Delay_ms(150);
  STOP();

  cik_cak();               // vyrovnej se na caru
  cik_cak();               // radeji 3x, aby se vyrovnaly setrvacne sily
  cik_cak();

  BL; FR; Delay_ms(220);               // vlevo
  STOP();
  FL; FR; Delay_ms(650);               // rovne
  STOP();
  FL; BR; Delay_ms(220);               // vpravo
  STOP();
  FL; FR; Delay_ms(770);               // rovne
  STOP();
  FL; BR; Delay_ms(210);               // vpravo
  STOP();
  FL; FR;
  Delay_ms(200);                       // kousek rovne
  // rovne, dokud nenarazis na caru
  while(input(RSENSOR));     // pravej senzor, protoze cihla je vpravo
  STOP();
  BL; FR;                         // toc se doleva, dokud nenarazis na caru
  while(input(LSENSOR));     // levej senzor, protoze cara je urcite vlevo
  STOP();
  line=R;      // caru predpokladame vpravo, kdybysme ji prejeli
  dira=0;
  FL; FR;                         // vpred
}*/