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  LMAX        190      // maximalni zatoceni doleva
#define  RMAX        60       // maximalni zatoceni doprava
#define  STRED       128      // sredni poloha zadniho kolecka

// 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(1200);
   for(n=uhel; n>0; n--);
   output_low(SERVO);
}

////////////////////////////////////////////////////////////////////////////////
#task (rate=2ms, max=500us)
void motory()        // umoznuje nezavisle rizeni vykonu na obou motorech
{

   if(Rmotor>0)                  //pravy motor
   {
      GO(R, F, Rmotor);
   }

   if(Rmotor==0) STOPR;

   if(Rmotor<0)
   {
      GO(R, B, abs(Rmotor));
   }

   if(Lmotor>0)                  // levy motor
   {
      GO(L, F, Lmotor);
   }

   if(Lmotor==0) STOPL;

   if(Lmotor<0)
   {
      GO(L, B, abs(Lmotor));
   }
}
////////////////////////////////////////////////////////////////////////////////
#task (rate=4ms, max=500us, queue=0)
void rizeni()
{
rizeni:

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

   if(bit_test(sensors,0)) //*......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
   {
      uhel=LMAX;
      Lmotor=60;Rmotor=127;
      goto rizeni;
   }
   if(bit_test(sensors,6)) //......*//
   {
      uhel=RMAX;
      Rmotor=60;Lmotor=127;
      goto rizeni;
   }
   if(bit_test(sensors,1)) //.*.....//
   {
      uhel=STRED+30;
      Lmotor=70;Rmotor=127;
      goto rizeni;
   }
   if(bit_test(sensors,5)) //.....*.//
   {
      uhel=STRED-30;
      Rmotor=70;Lmotor=127;
      goto rizeni;
   }
   if(bit_test(sensors,2)) //..*....//
   {
      uhel=STRED+20;
      Lmotor=90;Rmotor=127;
      goto rizeni;
   }
   if(bit_test(sensors,4)) //....*..//
   {
      uhel=STRED-20;
      Rmotor=90;Lmotor=127;
      goto rizeni;
   }
   if(bit_test(sensors,3)) //...*...//
   {
      uhel=STRED;
      Lmotor=100;Rmotor=100;
      goto rizeni;
   }

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

      }*/
}
////////////////////////////////////////////////////////////////////////////////
void main()
{
   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);
//  set_pwm1_duty(0);       // Spust PWM, ale zatim s trvalou 0 na vystupu
//   setup_ccp1(CCP_PWM);
//   setup_timer_2(T2_DIV_BY_16,200,1); // perioda
//   setup_timer_2(T2_DIV_BY_4,255,10);    // Casovac pro regulaci

   STOPR; STOPL;
   uhel=STRED;
//      diagnostika();
Lmotor=0;
Rmotor=0;
   rtos_run();

}