WebSVN – svnkaklik – /roboti/istrobot/cholerik/cholerik.c

// Program pro MiniSumo na R-Day 2006
//"$Id: cholerik.c 209 2007-03-22 22:03:46Z kakl $"

#include "cholerik.h"

// Konstanty
#define TRESHOLD     0x90        // rozhodovaci uroven pro okraj areny
//#define DEBUG1 1   // Diagnostika pohonu

//motory            //Napred vypnout potom zapnout!
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
#define FL         output_low(PIN_B7); output_high(PIN_B6)
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
#define BL         output_low(PIN_B6); output_high(PIN_B7)
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)

//cidla
#define L         2           // Senzory na okraj areny
#define R         3
#define SIDE_R    !input(PIN_A7)     // Sensory na soupere
#define SIDE_L    !input(PIN_A4)
#define FRONT     !input(PIN_A6)
#define BACK      !input(PIN_B3)
#define GRAVITY   !input(PIN_A1)

#DEFINE SOUND_HI   PIN_B1     // komplementarni vystupy pro piezo pipak
#DEFINE SOUND_LO   PIN_B2

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

unsigned int8 majak=0;
unsigned int8 sl=0;
unsigned int8 sr=0;
unsigned int8 b=0;
unsigned int8 f=0;
unsigned int8 g=0;
int1  arena_l;
int1  arena_r;
int1 diag=FALSE;

#int_TIMER0
TIMER0_isr()
{
   int1 stav;

   stav = ((majak & 0b1) == 0b1);
   if (((SIDE_R && stav) || (!SIDE_R && !stav))) {if (sr<255) sr++;} else {sr=0;};
   if (((SIDE_L && stav) || (!SIDE_L && !stav))) {if (sl<255) sl++;} else {sl=0;};
   if (((BACK && stav) || (!BACK && !stav))) {if (b<255) b++;} else {b=0;};
   if (((FRONT && stav) || (!FRONT && !stav))) {if (f<255) f++;} else {f=0;};
   majak++;
   stav = ((majak & 0b1) == 0b1);
if (input(PIN_A3)) arena_r=TRUE; else arena_r=FALSE;
if (input(PIN_A2)) arena_l=TRUE; else arena_l=FALSE;
   if (stav)
   {
//      if (read_adc(ADC_READ_ONLY) > TRESHOLD) arena_l=TRUE; else arena_l=FALSE;
//      set_adc_channel(R); // prepnuti kanalu ADC, je treba min 10us na ustaleni
//      delay_us(10);
//      read_adc(ADC_START_ONLY);
      set_pwm1_duty(27);      // 1:1
   }
   else
   {
//      if (read_adc(ADC_READ_ONLY) > TRESHOLD) arena_r=TRUE; else arena_r=FALSE;
//      set_adc_channel(L); // prepnuti kanalu ADC, je treba min 10us na ustaleni
//      delay_us(10);
//      read_adc(ADC_START_ONLY);
      set_pwm1_duty(55);      // 1:0
   };
   if (GRAVITY) {if (g<255) g++;} else g=0;
   if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se
}

// Primitivni Pipani
void beep(unsigned int16 period, unsigned int16 length)
{
   unsigned int16 nn;

   disable_interrupts(GLOBAL);
   for(nn=length; nn>0; nn--)
   {
     output_high(SOUND_HI);output_low(SOUND_LO);
     delay_us(period);
     output_high(SOUND_LO);output_low(SOUND_HI);
     delay_us(period);
   }
   enable_interrupts(GLOBAL);
}

/******************************************************************************/
inline void diagnostika()
{
   unsigned int16 n;

#ifdef DEBUG1
   while (true)   // Diagnostika podvozku
   {
      for (n=500; n<800; n+=100)
      {
         beep(n,n); //beep UP
      };
      Delay_ms(1000);
      //zastav vse
      STOPL; STOPR;
      //pravy pas
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
      Beep(880,100); Delay_ms(1000);
      //levy pas
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
      Beep(880,100); Delay_ms(1000);
      //oba pasy
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
   };
#endif

//!!!!!!!!!!!!!!!
/*
while(true)
{
      set_adc_channel(R); // prepnuti kanalu ADC, je treba min 10us na ustaleni
      delay_us(10);
      read_adc(ADC_START_ONLY);
      delay_ms(1);
      delay_ms(read_adc(ADC_READ_ONLY));
      beep(1000,200);
}
*/
   if (GRAVITY)
   {
      diag=TRUE;
      enable_interrupts(INT_TIMER0);
      enable_interrupts(GLOBAL);
      while (true)         // Diagnostika cidel
      {
         if (g>100) beep(800,100);
         Delay_ms(50);
         if (arena_l) {beep(1000,200); delay_ms(10);beep(1000,200);};
         Delay_ms(50);
         if (arena_r) {beep(2000,300); delay_ms(10);beep(2000,300);};
         Delay_ms(50);

         if (sr>10) beep(3000,400);
         Delay_ms(50);
         if (f>10) beep(4000,500);
         Delay_ms(50);
         if (sl>10) beep(5000,500);
         Delay_ms(50);
         if (b>10) beep(6000,600);
         Delay_ms(50);
      }
   };
}

void main()
{
   unsigned int16 n; // for FOR

   STOPL; STOPR;     // zastavi motory

   setup_oscillator(OSC_8MHZ|OSC_INTRC);     // CPU clock 8MHz
//   setup_adc_ports(sAN2|sAN3|VSS_VDD);   // prevodniky na cidla na okraj areny
//   setup_adc(ADC_CLOCK_INTERNAL);
//!!!!!!!!!!!!!!!!!!
   setup_adc_ports(NO_ANALOGS);   // prevodniky na cidla na okraj areny
   setup_adc(ADC_OFF);
   setup_spi(FALSE);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4);  // Casovac pro SW PWM a cteni cidel 
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz
   setup_ccp1(CCP_PWM);    // HW PWM ON
   set_pwm1_duty(27);      // 1:1
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);

   set_adc_channel(R);

   Beep(1000,200);     //double beep
   Delay_ms(50);
   Beep(1000,200);
   diagnostika();

   enable_interrupts(INT_TIMER0);
   enable_interrupts(GLOBAL);
/*---------------------------------------------------------------------------*/
   for (n=1;n<=5;n++)   // 5s do zacatku souboje
   {
      Delay_ms(720);
      Beep(1000,200);
   }

   while(true)       // hlavni smycka
   {
LOOP:

      GO(L, F, 150); GO(R, F, 150);

      if (arena_r)
      {
         BL; BR;
         delay_ms(100);
         STOPL; BR;
         for(n=0; n<5000; n++)
         {
            if (!arena_r || arena_l) {BL; BR;};
         };
         FL; BR;
         delay_ms(100);
         STOPL; STOPR;
      }

      if (arena_l)
      {
         BL; BR;
         delay_ms(100);
         BL; STOPR;
         for(n=0; n<5000; n++)
         {
            if (!arena_l || arena_r) {BL; BR;};
         };
         BL; FR;
         delay_ms(100);
         STOPL; STOPR;
      }

      if (sr>10)     // Nepritel vpravo
      {
         FL; FR;                 // popojed rovne
         for(n=0; n<5000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
         };
         FL; BR;                     // otoc se na nej
         for(n=0; n<10000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
            if (f>5)
            {
               FL; FR;               // vytlac ho
            };
            if (sl>5) {BL; FR;};
            if (sr>5) {FL; BR;};
         };
      }

      if (sl>10)     // Nepritel vlevo
      {
         FL; FR;                 // popojed rovne
         for(n=0; n<5000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
         };
         BL; FR;                    // otoc se na nej
         for(n=0; n<10000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
            if (f>5)
            {
               FL; FR;              // vytlac ho
            };
            if (sl>5) {BL; FR;};
            if (sr>5) {FL; BR;};
         };
      }

      if (f>10)      // Nepritel vpredu
      {
         BL; FR;
         delay_ms(110);
         FL; BR;
         delay_ms(50);
         STOPL; STOPR;
      }

      if (b>10)      // Nepritel vzadu
      {
         BL; FR;
         delay_ms(110);
         FL; BR;
         delay_ms(50);
         STOPL; STOPR;
      }

   } // while(true)
}
Subversion Repositories svnkaklik

(root)/roboti/istrobot/cholerik/cholerik.c @ 218 – Rev
