Subversion Repositories svnkaklik

#include "main.h"

#define  TXo PIN_C3                 // To the transmitter modulator
#include "AX25.c"             // podprogram pro prenos telemetrie

//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)
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)

#define         L 0b10  // left
#define         R 0b01  // right
#define         S 0b11  // straight

#define         COUVANI         1600                                    // couvnuti po zjisteni diry
#define         MEZERA          5400                                    // za jak dlouho bude ztracena cara
#define         PRES_DIRU       400                                     // velikost mezery v care
#define         ODEZVA          1                                               // za jak dlouho po opusteni cary se ma zacit zatacet
#define BRZDENI         100                                     // doba (v ms) ptrebna k zastaveni jednoho motoru

//cidla
#define         RSENSOR    1       // Senzory na caru
#define         LSENSOR    0
#define BUMPER  PIN_C4          // sensor 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
#DEFINE SOUND_LO   PIN_B2

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

int tresholdL;          // rozhodovaci uroven pro prave cidlo
int tresholdR;          // rozhodovaci uroven pro prave cidlo
int movement;     // smer minuleho pohybu
int line;         // na ktere strane byla detekovana cara
unsigned int16 dira;                    // pocitadlo pro nalezeni preruseni cary

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

// Diagnostika pohonu, hejbne vsema motorama ve vsech smerech
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();
      sprintf(AXstring,"L: %U  R: %U\0", ls, rs);  // Convert DATA to String.
      SendPacket(&AXstring[0]);
      delay_ms(1000);
   };
}

void cikcak()
{
int n;
        switch(movement)                                                                                                // podivej se na jednu stranu
        {
        case L:
                                FL;BR;
                                movement=R;
                        break;
        case R:
                                FR;BL;
                                movement=L;
                        break;
        case S:
                                FR;BL;
                                movement=L;
                        break;
        }
        set_adc_channel(LSENSOR);
        Delay_us(10);
        while (tresholdL < read_adc())                                          // je tam cara?
        {
                if (n==50)                                                                                              // asi bude na druhe strane
                {
                        STOPR;STOPL;
                        n=0;
                        switch(movement)
                        {
                        case L:
                                                FL;BR;
                                                movement=R;
                                        break;
                        case R:
                                                FR;BL;
                                                movement=L;
                                        break;
                        }
                }
                Delay_ms(5);
                n++;
        }
        STOPL;STOPR;                                                                                            // nasli jsme caru
        line=S;
}
void objizdka()
{
        BL;BR;Delay_ms(300);
        STOPR;STOPL;
        beep(1000,1000);
        Delay_ms(500);
        beep(1000,1000);
        Delay_ms(1000);

}
void kalibrace()
{
unsigned int16 i;
int min;
int max;
int current;
int treshold;

        FL; BR; Delay_ms(130);
chyba1:
        FR; BL;                                                                                                 //kalibrace leveho cidla
   set_adc_channel(LSENSOR);
        Delay_us(20);
        min=max=read_adc();
        for (i=1;i<=500;i++)
        {
                current=read_adc();
                if (max < current) max=current;
                if (min > current) min=current;
                Delay_us(500);
        }
        FL; BR;
        for (i=1;i<=500;i++)
        {
                current=read_adc();
                if (max < current) max=current;
                if (min > current) min=current;
                Delay_us(500);
        }
        STOPL; STOPR; Delay_ms(200);
        if((max-min)<50) {Beep(1000,300); GOTO chyba1;}
        treshold=(max-min)>>1;
        tresholdL=treshold+min;

chyba2:
        FR; BL;
   set_adc_channel(RSENSOR);
        Delay_us(20);
        min=max=read_adc();                                                                     //naplneni min a max nejakou rozumnou hodnotou
        for (i=1;i<=500 ;i++)
        {
                current=read_adc();
                if (max < current) max=current;                         //zmereni minima a maxima
                if (min > current) min=current;
                Delay_us(500);
        }
        FL; BR;
        for (i=1;i<=500 ;i++)
        {
                current=read_adc();
                if (max < current) max=current;                         //zmereni minima a maxima
                if (min > current) min=current;
                Delay_us(500);
        }
        STOPL; STOPR; Delay_ms(200);
        if((max-min)<50) {Beep(1000,300); GOTO chyba2;}
        treshold=(max-min)>>1;
        tresholdR=treshold+min;

        FR; BL;
        movement=L;
        set_adc_channel(LSENSOR);
        Delay_us(20);
        while (tresholdL < read_adc()) Delay_us(100);
        FL; BR; Delay_ms(50);
        STOPL; STOPR; Delay_ms(500);
        Beep(780,200);
}

void main()
{
unsigned int16 rovne;                                                                                           // pocita delku rovne cary

   STOPL; STOPR;

   setup_adc_ports(RA0_RA1_RA3_ANALOG);
   setup_adc(ADC_CLOCK_DIV_2);

   port_b_pullups(false);

        diagnostika();

   Beep(1000,200);     //double beep
   Delay_ms(50);
   Beep(1000,200);
   Delay_ms(1000); // 1s

//      kalibrace();
        tresholdl=tresholdr=80;
//      FL; FR;
   movement=S;
   line=S;
   dira=0;
        rovne=0;

   while(true)
   {
                if(!input(BUMPER)) objizdka();
                line=0;
      set_adc_channel(RSENSOR);                                                         // podivej se jestli neni cara pod pravym cidlem
      Delay_us(10);
      if(tresholdR > read_adc())
                {
                        dira=0;
                        line=R;
                }
      set_adc_channel(LSENSOR);                                                         // kdyz cara nebyla pod pravym cidlem, mozna bude pod levym
      Delay_us(10);
      if(tresholdL > read_adc())
                {
                        dira=0;
                        line=line | L;
                }

                switch(line)
                {
                case S:
                        FR;FL;
                        movement=S;
                        continue;
                case L:
                        STOPL;
                        FR;movement=L;
                        continue;
                case R:
                        STOPR;
                        FL;movement=R;
                        continue;
                default:
                }

                if (dira==ODEZVA)                                                                       // kdyz uz chvili jedeme po bile plose
                {
                        //BR;BL;Delay_us(rovne >>= 5);
                        rovne=0;                                                                                        //kdyz sme museli zatocit, uz neni rovna cara

                        switch (line)                                                                   // musime zatocit
                        {
                        case L:
                                        BL;Delay_ms(BRZDENI);STOPL;
                                                FR;
                                                movement=L;
                                        break;
                case R:
                                        BR;Delay_ms(BRZDENI);STOPR;
                                                FL;
                                                movement=R;
                                        break;
                        }
                }
                if (dira==MEZERA)                                                                                       // kdyz zkoncila cara
                {
                        beep(800,500);
                        Delay_ms(50);
                        beep(800,500);
                        switch (movement)                                                                               //vrat se zpet na caru
                        {
                        case L:
                                                STOPL;STOPR;
                                        BR;Delay_ms(COUVANI);STOPR;
                                        break;
                case R:
                                                STOPL;STOPR;
                                        BL;Delay_ms(COUVANI);STOPL;
                                        break;
                        case S:
                                                BL; BR; Delay_ms(COUVANI);
                                                STOPL; STOPR;
                                        break;
                        }

                        FR;FL; Delay_ms(PRES_DIRU);                                             // popojedem dopredu mozna tam bude cara
                        STOPL; STOPR; movement=S;
                        cikcak();                                                                                               // najdi caru
                        dira=0;
                }
                dira++;
   } // while(true)
}