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//10           // 3 stupne zataceni

#define  BEAR2       25//27

#define  BEAR3       45

#define  R           100         // Rozumna rychlost   

#define  R17         255         // X nasobek rozumne rychlosti

#define  DOZNIVANI   10

#define  L1            1     // cara vlevo

#define  L2            2     // cara vlevo

#define  L3            3     // cara vlevo

#define  S             0     // casa mezi sensory

#define  R1           -1     // cara vpravo

#define  R2           -2     // cara vpravo

#define  R3           -3     // cara vpravo

// servo

#define  SERVO PIN_B5

// kroutitka

#define  CERVENA  5  // AN5

#define  CERNA    2  // AN2

#define  ZELENA   4  // AN4

#define  MODRA    6  // AN6

// 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

//#define  SW1      PIN_A2      // Motory On/off

unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru

signed int8 line = S;         // na ktere strane byla detekovana cara

//unsigned int8 dira;         // pocita dobu po kterou je ztracena 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

short int preteceni;          // flag preteceni timeru1

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_TIMER1

TIMER1_isr()

{

   preteceni = true;

}

////////////////////////////////////////////////////////////////////////////////

#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);

}

////////////////////////////////////////////////////////////////////////////////

short int IRcheck()                 // potvrdi detekci cihly

{

   output_high(IRTX);               // vypne vysilac IR

   delay_ms(100);

   output_low(STROBE);

   sensors = spi_read(0);         // cteni senzoru

   sensors=~sensors;

   output_high(STROBE);

   if(true==bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal

   {

      output_low(IRTX);             // zapne vysilac IR

      delay_ms(100);

      output_low(STROBE);

      sensors = spi_read(0);         // cteni senzoru

      sensors=~sensors;

      output_high(STROBE);

      if(false==bit_test(sensors,7))      // otestuje, jestli je detekovana cihla

      {

         output_high(IRTX);            // vypne vysilac IR

         delay_ms(100);

         output_low(STROBE);

         sensors = spi_read(0);         // cteni senzoru

         sensors=~sensors;

         output_high(STROBE);

         output_low(IRTX);             // zapne vysilac IR

         if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla

      }

   };

   output_low(IRTX);             // zapne vysilac IR

   return 0; // vrat 0, kdyz je detekovano ruseni

}

////////////////////////////////////////////////////////////////////////////////

void objizdka()

{

   int8 shure=0;

   unsigned int16 n;

   BR;BL;

   Delay_ms(400);

   STOPR;STOPL;

// toceni na miste dokud nezmizi cihla

//------------------------------------

   uhel=KOLMO1;      // nastav zataceci kolecko kolmo na osu robota

   Delay_ms(100);

   BL;FR;

   Delay_ms(200);    // minimalni toceni, kdyby se zastavilo sikmo k cihle

   While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru

   {

       sensors = spi_read(0);         // cteni senzoru

       sensors=~sensors;

       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel

   }

   STOPL; STOPR;

   for (n=0;n<1000;n++)    // vystred se na hranu cihly

   {

      if(!input(CIHLA))

      {

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

      } else

      {

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

      };

      delay_ms(1);

   }

   STOPR;STOPL;

   uhel=STRED;          // dopredu

   delay_ms(100);

   FR; FL;

   delay_ms(500);

   BL;BR;

   delay_ms(200);

   STOPL;STOPR;

   uhel=STRED+BEAR3;    // doprava

   delay_ms(100);

   FL;

   delay_ms(400);

   uhel=STRED+BEAR2;    // min doprava

   FL;FR;

   delay_ms(100);

   uhel=STRED+BEAR1;    // jeste min doprava

   FL;FR;

   delay_ms(200);

   While((sensors & 0b01111111)!=0) //dokud neni cara

   {

       sensors = spi_read(0);         // cteni senzoru

       sensors=~sensors;

       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel

   }

   BL; BR;

   delay_ms(400);

   uhel=STRED-BEAR3;    // doleva

}

////////////////////////////////////////////////////////////////////////////////

void main()

{

   unsigned int8 n;

   unsigned int8 i,v;

   unsigned int8 last_sensors;

   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(1500); // musime pockat na diagnostiku slave CPU

   //nastaveni rychlosti

   set_adc_channel(CERVENA);

   Delay_ms(1);

   speed=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

      output_low(STROBE);              // vypni zobrazovani na posuvnem registru

      sensors = spi_read(0);           // cteni senzoru

      sensors=~sensors;

      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>3) // 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

      {

         objizdka();       // objede cihlu

      }

      if(bit_test(sensors,3)) //...|...//

      {

         uhel=STRED;

         if (rovinka<50)

         {

            Lmotor=R17;

            Rmotor=R17;

         }

         else

         {

            Lmotor=speed;

            Rmotor=speed;

         };         

         line=S;

         if (rovinka < 255) rovinka++;

         continue;

      }

      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-50;

         Rmotor=speed;

         line=L2;

         continue;

      }

      if(bit_test(sensors,5)) //.....|.//

      {

         uhel=STRED + BEAR2;

         Rmotor=speed-50;

         Lmotor=speed;

         line=R2;

         continue;

      }

      if (bit_test(sensors,2)) //..|....//

      {

         uhel=STRED - BEAR1;

         Lmotor=speed;

         Rmotor=speed;

         line=L1;

         if (rovinka<255) rovinka++;

         continue;

      }

      if (bit_test(sensors,4)) //....|..//

      {

         uhel=STRED + BEAR1;

         Rmotor=speed;

         Lmotor=speed;

         line=R1;

         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) delay_ms(50);

         };

         rovinka=0;

      };

   }

}