Subversion Repositories svnkaklik

#include ".\main.h"

#use rtos (timer=2, minor_cycle=2ms)

#define  STRED       128      // sredni poloha zadniho kolecka
#define  BEAR1       12       // 3 stupne zataceni
#define  BEAR2       34
#define  BEAR3       70
#define  SPEEDMAX    120      // maximalni rychlost

#define  L           1
#define  S           2
#define  R           0

// servo
#define  SERVO PIN_A2

// IR
#define IRTX      PIN_B2

//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
unsigned int8 line;           // 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 rovinka;        // pocitadlo na zjisteni rovinky

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

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

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/*
void diagnostika()
{
   if(input(SW1))STOPR;STOPL;While(TRUE);
//   if(LSENSOR==true) output_high(LED2); else output_low(LED2);
//   if(RSENSOR==true) output_high(LED1); else output_low(LED1);
}
*/
////////////////////////////////////////////////////////////////////////////////
#task (rate=18ms,max=2ms)
void zatoc()   // ridi servo ktere otaci kolem
{
   unsigned int8 n;

   output_high(SERVO);
   delay_us(1100);
   for(n=uhel; n>0; n--);
   output_low(SERVO);
}

////////////////////////////////////////////////////////////////////////////////
void diag()   // ridi servo ktere otaci kolem
{
   unsigned int8 n;

   output_high(SERVO);
   delay_us(1100);
   for(n=uhel; n>0; n--);
   output_low(SERVO);
}

////////////////////////////////////////////////////////////////////////////////
short int IRcheck()                 // potvrdi detekci cihly
{
   output_high(IRTX);               // vypne vysilac IR

   output_low(STROBE);
   sensors = spi_read(0);         // cteni senzoru
   output_high(STROBE);

   if(true==bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal
   {
      output_low(IRTX);             // zapne vysilac IR

      output_low(STROBE);
      sensors = spi_read(0);         // cteni senzoru
      output_high(STROBE);

      if(false==bit_test(sensors,7))      // otestuje, jestli je detekovana cihla
      {
         output_high(IRTX);            // vypne vysilac IR

         output_low(STROBE);
         sensors = spi_read(0);         // cteni senzoru
         output_high(STROBE);

         if(bit_test(sensors,7)) return 1; //

         else return 0; // vrat 0 kdyz je ruseni
      }
      else return 0; // vrat 0 kdyz to nebyla cihla
   }
   else return 0; // vrat 0 kdyz je detekovano ruseni
}
////////////////////////////////////////////////////////////////////////////////
/*
#task (rate=4ms, max=20us)
void rychlost()
{
   if(speed<255) speed++;
}
*/
////////////////////////////////////////////////////////////////////////////////
#task (rate=2ms, max=1ms)
void rizeni()
{
   unsigned int16 n;
   unsigned int8 i;

   GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory

   delay_us(500);

   output_low(STROBE);
   sensors = spi_read(0);         // cteni senzoru
   sensors=~sensors;
   output_high(STROBE); 
   
   i=0;                    // havarijni kod   
   for (n=0; n<=6; n++)
   {
      if(bit_test(sensors,n)) i++;
   }
   if (i>3) rtos_terminate(); // zastavi, kdyz je cerno pod vice nez tremi cidly
   

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

   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=SPEEDMAX;
      line=L;
      return;
   }

   if(bit_test(sensors,6)) //......|//
   {
      uhel=STRED+BEAR3;
      Rmotor=0;
      Lmotor=SPEEDMAX;
      line=R;
      return;

   }

   if(bit_test(sensors,1)) //.|.....//
   {
      uhel=STRED-BEAR2;
      Lmotor=SPEEDMAX-50;
      Rmotor=SPEEDMAX;
      line=S;
      return;
   }

   if(bit_test(sensors,5)) //.....|.//
   {
      uhel=STRED+BEAR2;
      Rmotor=SPEEDMAX-50;
      Lmotor=SPEEDMAX;
      line=S;
      return;
   }

   if (bit_test(sensors,2)) //..|....//
   {
      uhel=STRED-BEAR1;
      Lmotor=SPEEDMAX;
      Rmotor=SPEEDMAX;
      line=S;
      if (rovinka<255) rovinka++;
      return;
   }

   if (bit_test(sensors,4)) //....|..//
   {
      uhel=STRED+BEAR1;
      Rmotor=SPEEDMAX;
      Lmotor=SPEEDMAX;
      line=S;
      if (rovinka<255) rovinka++;
      return;
   }

   if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
   {
      if (rovinka>250)
      {
         BL; BR;
         for(n=1; n<(60); n++) {rtos_yield(); delay_us(500);};
      };
      rovinka=0;
   }
   
   if(bit_test(sensors,7))       // zjisti jestli neni cihla
   {
      rtos_terminate();
   }
}



////////////////////////////////////////////////////////////////////////////////
void main()
{
   unsigned int16 i;
   unsigned int8 last;

   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_CLOCK_INTERNAL);
    setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_DISABLED);
   setup_oscillator(OSC_8MHZ|OSC_INTRC);

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

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

   output_low(IRTX); // zapni IR vysilac

   for(i=0;i<100;i++)   // pockej, nez se zadni kolecko vystredi
   {
      diag();
      delay_ms(16);
   }
//      diagnostika();

   while(true)
   {
      rtos_run();
      STOPR;
      STOPL;
      while(true);
   }
}