Subversion Repositories svnkaklik

#include "tank.h"

#define  TXo PIN_B1           // 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       1000                                    // velikost mezery v care
#define BRZDENI         5000                            // doba ptrebna k zastaveni jednoho motoru

//cidla
#define         RSENSOR     C2OUT       // Senzory na caru
#define         LSENSOR     C1OUT
#define BUMPER     PIN_A4               // sensor na cihlu

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

#DEFINE SOUND_HI   PIN_A7
#DEFINE SOUND_LO   PIN_A6

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

#define GO(motor, direction, power) if(get_timer0()<=power) \
                                    {direction##motor;} \
                                    else \
                                    {stop##motor;}

int movement;     // smer minuleho pohybu
int line;         // na ktere strane byla detekovana cara
unsigned int16 dira;                    // pocitadlo pro nalezeni preruseni cary
int speed,speedL,speedR;

// 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);
   }
}
#int_TIMER2
void TIMER2_isr()
{
   switch(line)             // upravime smer
   {
   case S:                  //obe cidla na care
      if(speedL<200)speedL++;
      if(speedR<200)speedR++;
      break;               // vrat se zpet na cteni cidel
   case L:                  // cara je pod levym cidlem, trochu zatocime
      if (speedL>100)speedL -- ;
      if (speedR<200)speedR ++ ;
      break;
   case R:                  // cara pod pravym cidlem
      if (speedR>100)speedR -- ;
      if (speedL<200)speedL ++ ;
      break;
   default:
   }
}
// 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;
        }
        while (0==(RSENSOR|LSENSOR))
        {
                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 pozordira()
{
                        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;
}

void main()
{
   unsigned int16 rovinka;
   int last;

   STOPL; STOPR;

   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator
   port_b_pullups(true);
   setup_adc_ports(NO_ANALOGS|VSS_VDD);
   setup_adc(ADC_OFF);
   setup_spi(FALSE);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM
   setup_timer_1(T1_DISABLED);   // Casovac pro regulaci
   setup_timer_2(T2_DIV_BY_16,50,16);
   setup_ccp1(CCP_OFF);
   setup_comparator(A0_VR_A1_VR);
   setup_vref(VREF_HIGH|15);
   enable_interrupts(INT_TIMER2);
   enable_interrupts(GLOBAL);

        diagnostika();

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

//      FL; FR;
   movement=S;
   line=S;
   dira=0;
   last=0;
   rovinka=0;

   speed=speedL=speedR=200;

   while(true)
   {
      line = RSENSOR;                   // precteni cidel
      line |= LSENSOR << 1;      // sestav informaci o care
      
      if(line!=0)
      {
         GO(L, F, speedL); GO(R,F, speedR);
      }
      else{STOPR; STOPL;}
//sem:
/*              switch(line)             // upravime smer
                {
                case S:                  //obe cidla na care
//         if (speedL<speedR) speedL=speedR;
//         else speedR=speedL;
                        GO(L,F,speedL); GO(R,F,speedR)                // jedeme rovne
//         if(rovinka<BRZDENI) rovinka++; //cara je rovne
//         dira=0;                 // videli jsme caru, proto neni dira
                        continue;               // vrat se zpet na cteni cidel
                case L:                  // cara je pod levym cidlem, trochu zatocime
         GO(L, F, speedL); GO(R,F, speedR);
//         if(rovinka<BRZDENI) rovinka++;   //cara je celkem rovne
//         dira=0;
                        continue;
                case R:                  // cara pod pravym cidlem
         GO(R, F, speedR); GO(L, F, speedL);
//         if(rovinka<BRZDENI) rovinka++;
//         dira=0;
                        continue;
                default:                 // cara neni pod zadnym cidlem
                }*/


      /*switch (last)                          // zatacka
      {
      case L:                                 // do leva
            BL;STOPR;                     //zabrzdeni leveho motoru
            for(;rovinka>0;rovinka--) //chvili pockej
            {
               line = RSENSOR;         // precteni cidel
               line |= LSENSOR << 1;      // sestav informaci o care
               if(line!=0) goto sem;  //kdyz najdes caru, zastav
            }
            STOPL; FR;       // pokracuj v zataceni
            while(line==0)
            {
               line = RSENSOR;         // precteni cidel
               line |= LSENSOR << 1;      // sestav informaci o care
            }
            movement=L;
            rovinka=0;                //zataceli jsme, uz neni rovna cara
           break;
      case R:
            BR; STOPL;                      // zabrzdeni praveho motoru
            for(;rovinka>0;rovinka--)
            {
               line = RSENSOR;         // precteni cidel
               line |= LSENSOR << 1;      // sestav informaci o care
               if(line!=0) goto sem;
            }
            STOPR; FL;
            while(line==0)
            {
               line = RSENSOR;         // precteni cidel
               line |= LSENSOR << 1;      // sestav informaci o care
            }
            movement=R;
            rovinka=0;                //zataceli jsme, uz neni rovna cara
           break;
      }*/
   } // while(true)
}