Subversion Repositories svnkaklik

#include ".\main.h"

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

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

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

#define  STRED       128      // sredni poloha zadniho kolecka
#define  BEAR1       20       // 3 stupne zataceni
#define  BEAR2       40
#define  BEAR3       60
// servo
#define  SERVO PIN_A2

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

// 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=16ms,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);
}

////////////////////////////////////////////////////////////////////////////////
#task (rate=2ms, max=1ms)
void rizeni()
{
   GO(L,F,Lmotor);GO(R,F,Rmotor);

   delay_us(500);

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

   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=190;
      return;
   }

   if(bit_test(sensors,6)) //......|//
   {
      Rmotor=0;Lmotor=190;
      uhel=STRED-BEAR3;
      return;

   }

   if(bit_test(sensors,1)) //.|.....//
   {
      uhel=STRED+BEAR2;
      Lmotor=150;Rmotor=190;
      return;
   }

   if(bit_test(sensors,5)) //.....|.//
   {
      uhel=STRED-BEAR2;
      Rmotor=150;Lmotor=190;
      return;
   }

   if (bit_test(sensors,2)) //..|....//
   {
      uhel=STRED+BEAR1;
      Lmotor=150;Rmotor=190;
      return;
   }

   if (bit_test(sensors,4)) //....|..//
   {
      uhel=STRED-BEAR1;
      Rmotor=150;Lmotor=190;
      return;
   }

   if(bit_test(sensors,3)) //...|...//
   {
      uhel=STRED;
      Lmotor=190;Rmotor=190;
      return;
   }

/*STOPL;STOPR;
      if(bit_test(sensors,7))
      {

      }*/
}


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

   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;
   uhel=STRED;

   for(i=0;i<100;i++)
   {
      diag();
      delay_ms(16);
   }
//      diagnostika();
Lmotor=0;
Rmotor=0;
   rtos_run();

}