Subversion Repositories svnkaklik

#include "tank.h"

#define DEBUG

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

unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
unsigned int8 line;           // na ktere strane byla detekovana cara
unsigned int8 speed;          // rychlost zataceni
unsigned int8 rovinka;        // pocitadlo pro zjisteni rovneho useku
unsigned int8 last;           // kde byla cara, kdyz byly minule cidla mimo
unsigned int8 movement;       // obsahuje aktualni smer zataceni
unsigned int8 dira;           // pocita dobu po kterou je ztracena cara

// Konstanty pro dynamiku pohybu
#define T_DIRA       100      // po jakem case zataceni se detekuje dira
#define INC_SPEED    1        // prirustek rychlosti v jednom kroku
#define FW_POMALU    100      // trochu mimo caru vnitrni pas
#define FW_ZATACKA   200      // rychlost vnejsiho kola pri zataceni
#define FW_STREDNE   110      // trochu mimo caru vnejsi pas
#define COUVANI      600      // couvnuti zpet na caru, po detekci diry
#define PRES_DIRU    300
#define MAX_ROVINKA  (255-FW_STREDNE)
#define TRESHOLD     6        // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd

//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    C2OUT      // Senzory na caru
#define LSENSOR    C1OUT
#define BUMPER     PIN_A4     // Senzor na cihlu

#define DIAG_SERVO      PIN_B3   // Propojka pro diagnosticky mod
#define DIAG_SENSORS    PIN_B2   // Propojka pro diagnosticky mod

#DEFINE SOUND_HI   PIN_A6     // komplementarni vystupy pro piezo pipak
#DEFINE SOUND_LO   PIN_A7

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_TIMER2
void TIMER2_isr()
{
   if (speed<255) speed+=INC_SPEED;
   if (rovinka<MAX_ROVINKA) rovinka++;
   if (dira<T_DIRA) dira++;
}

// 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();
      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;
        }
        while (0==(RSENSOR|LSENSOR))
        {
                if (n==50)                                                      //cara 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 prejeddiru()
{
   STOPL;STOPR;
   beep(800,500);
   Delay_ms(50);
   beep(800,500);
   switch (movement)                            //vrat se zpet na caru
   {
   case L:
         BR;Delay_ms(COUVANI);STOPR;
         STOPL;STOPR;
      break;
   case R:

         BL;Delay_ms(COUVANI);STOPL;
         STOPL;STOPR;
      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;
   cikcak();                                    // najdi caru
   dira=0;
}
///////////////////////////////////////////////////////////////////////////////
void main()
{
   unsigned int16 n; // pro FOR

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

   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator

   port_b_pullups(TRUE);      // pullups pro piano na diagnostiku
   setup_spi(FALSE);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM

   setup_timer_2(T2_DIV_BY_16,255,10);    // Casovac pro regulaci
                                         // preruseni kazdych 10ms
   setup_adc_ports(sAN2|VSS_VDD);      // nastaveni A/D prevodniku pro naraznik
   setup_adc(ADC_CLOCK_INTERNAL);
   set_adc_channel(2);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro naraznik
   setup_ccp1(CCP_COMPARE_RESET_TIMER);
   CCP_1=(2^10)-1;                        // prevod kazdou 1ms

   setup_comparator(A0_VR_A1_VR);   // inicializace komparatoru pro cidla cary
   setup_vref(VREF_HIGH|TRESHOLD);        // 32 kroku od 0.25 do 0.75 Vdd

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

         // povoleni rizeni rychlosti zataceni pres preruseni
   enable_interrupts(INT_TIMER2);
   enable_interrupts(GLOBAL);

/*---------------------------------------------------------------------------*/
   sensors=S;
   line=S;
   last=S;
   movement=S;
   speed=FW_POMALU;

   diagnostika();
   cikcak();     // toc se, abys nasel caru
   Delay_ms(500);
   Beep(1000,200);
   Delay_ms(500);

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

      sensors = RSENSOR;         // cteni senzoru na caru
      sensors |= LSENSOR << 1;

      switch (sensors)  // zatacej podle toho, kde vidis caru
      {
         case S:                          // rovne
            GO(L, F, speed); GO(R, F, speed);
//            FL; FR;  // pokud se jede dlouho rovne, tak pridej
            dira=0;
            continue;

         case L:                          // trochu vlevo
            GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
            line=L;
            dira=0;
            continue;

         case R:                          // trochu vpravo
            GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
            line=R;
            dira=0;
            continue;

         default:       // kdyz jsou obe cidla mimo caru, tak pokracuj dal
      }
   rovinka=0;
//      if (dira>=T_DIRA) prejeddiru();
      if (last!=line)     // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
      {
         last=line;
         speed=FW_ZATACKA;
      }
      else speed=255;
      
/*      if (dira==0)
      {
         if (L==line)  // kdyz jsou obe cidla mimo caru, zabrzdi vnitrni kolo
         {
            BL;
            for(n=4000;n>0;n--)           // Delay
            {
               line = RSENSOR;         // precteni cidel
               line |= LSENSOR << 1;      // sestav informaci o care
               if(line!=0) break;
            }
            STOPL;
         }
         else
         {
            BR;
            for(n=4000;n>0;n--)           // Delay
            {
               line = RSENSOR;         // precteni cidel
               line |= LSENSOR << 1;      // sestav informaci o care
               if(line!=0) break;
            }
            STOPR;
        }
      }*/
      
      if (L==line)  // kdyz jsou obe cidla mimo caru, zatoc na caru
      {
         STOPL;
         GO(R, F, speed);
         movement=L;
      }
      else
      {
         STOPR;
         GO(L, F, speed);
         movement=R;
     
      }
   } // while(true)
}