Subversion Repositories svnkaklik

#include ".\main.h"

#define  KOLMO1      225         // predni kolecko sroubem dopredu
#define  KOLMO2      30          // predni kolecko je hlavou sroubu dozadu
#define  STRED       128         // sredni poloha zataceciho kolecka
#define  BEAR1       10          // 3 stupne zataceni
#define  BEAR2       25
#define  BEAR3       45
#define  R           100         // Rozumna rychlost
#define  R17         200         // X nasobek rozumne rychlosti
#define  PRED_CIHLOU 100         // rychlost pri dalkove detekci cihly
#define  L2            2     // cara vlevo
#define  L3            3     // cara vlevo
#define  S             0     // cara mezi sensory
#define  R2           -2     // cara vpravo
#define  R3           -3     // cara vpravo

// servo
#define  SERVO PIN_B5

// kroutitka
#define  CERVENA  4  // AN4
#define  MODRA    2  // AN2

// IR
#define IRTX      PIN_B2
#define  CIHLA    PIN_A3

//motory
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)

//HID
#define  LED1     PIN_B1      //oranzova
#define  LED2     PIN_B2      //zluta

#define  STROBE   PIN_B0

unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
signed int8 line = S;         // na ktere strane byla detekovana cara
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
unsigned int8 speed;          // maximalni povolena rychlost
unsigned int8 turn;           // rychlost toceni
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky

signed int16  Lmotor;         // promene, ktere urcuji velikost vykonu na levem
signed int16  Rmotor;         // a pravem motoru

// makro pro PWM pro motory
#define GO(motor, direction, power) if(get_timer0()<=power) \
{direction##motor;} else {stop##motor;}
////////////////////////////////////////////////////////////////////////////////
#int_TIMER2
TIMER2_isr()      // ovladani serva
{
   unsigned int8 n;

   output_high(SERVO);
   delay_us(1000);
   for(n=uhel; n>0; n--) Delay_us(2);
   output_low(SERVO);
}

////////////////////////////////////////////////////////////////////////////////
void main()
{

   unsigned int8 n;
   unsigned int8 i,j;
   unsigned int8 last_sensors;
   unsigned int8 RozumnaRychlost;

   setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_DISABLED|T1_DIV_BY_8);
   setup_timer_2(T2_DIV_BY_16,140,16);
   setup_oscillator(OSC_8MHZ|OSC_INTRC);

   STOPR; STOPL;  // zastav motory
   Lmotor=0;Rmotor=0;

   uhel = STRED;    // nastav zadni kolecko na stred
   rovinka = 0;

   enable_interrupts(INT_TIMER2);
   enable_interrupts(GLOBAL);

   output_low(IRTX); // zapni IR vysilac

   delay_ms(2000); // musime pockat na diagnostiku slave CPU

   //nastaveni rychlosti
   set_adc_channel(CERVENA);

   Delay_ms(1);
   RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63
   set_adc_channel(MODRA);
   Delay_ms(1);
   turn=speed-32+(read_adc()>>2);  // rychlost toceni +-32; kroutitko dava 0-63


   while(true)
   {

      GO(L,F,Lmotor);GO(R,F,Rmotor);   // zapni motory PWM podle promenych Lmotor a Rmotor

      delay_us(2000);                  // cekani na SLAVE, nez pripravi data od cidel

      last_sensors=sensors;

      output_low(STROBE);              // vypni zobrazovani na posuvnem registru
      sensors = spi_read(0);           // cteni senzoru
      sensors=~sensors;                // neguj prijata data
      output_high(STROBE);       // zobraz data na posuvnem registru

      i=0;                       // havarijni kod
      for (n=0; n<=6; n++)
      {
         if(bit_test(sensors,n)) i++;
      }
      if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly
      {
         BL; BR;
         delay_ms(300);
         STOPR; STOPL;
         While(true);
      };

      if (bit_test(sensors,7))    // detekce cihly
      {
         BR;BL;
         Delay_ms(100);
         STOPR;STOPL;
         Delay_ms(1000);
      }

      if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
      {
         uhel=STRED - BEAR3;
         Lmotor=0;
         Rmotor=turn;
         line=L3;
         continue;
      }

      if(bit_test(sensors,6)) //......|//
      {
         uhel=STRED + BEAR3;
         Rmotor=0;
         Lmotor=turn;
         line=R3;
         continue;
      }

      if(bit_test(sensors,1)) //.|.....//
      {
         uhel=STRED - BEAR2;
         Lmotor=speed-70;
         Rmotor=speed;
         line=L2;
         continue;
      }

      if(bit_test(sensors,5)) //.....|.//
      {
         uhel=STRED + BEAR2;
         Rmotor=speed-70;
         Lmotor=speed;
         line=R2;
         continue;
      }

      if (bit_test(sensors,2)) //..|....//
      {
         uhel=STRED - BEAR1;
         Lmotor=speed-20;
         Rmotor=speed;
         line=L2;
         if (rovinka<255) rovinka++;
         continue;
      }

      if (bit_test(sensors,4)) //....|..//
      {
         uhel=STRED + BEAR1;
         Rmotor=speed-20;
         Lmotor=speed;
         line=L2;
         if (rovinka<255) rovinka++;
         continue;
      }

      if(bit_test(sensors,3)) //...|...//
      {
         uhel=STRED;
         Lmotor=speed;
         Rmotor=speed;
         line=S;
         if (rovinka < 255) rovinka++;
         continue;
      }



      if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate
      {
         if (rovinka>50)
         {
            BL; BR;
            Delay_ms(100);
            if (rovinka > 250 || speed > 170) delay_ms(50);
         };
         rovinka=0;
      };
   }
}