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

 // Program pro MiniSumo na R-Day 2006
-//"$Id: cholerik.c 209 2007-03-22 22:03:46Z kakl $"
+//"$Id: cholerik.c 240 2007-04-19 06:14:36Z kakl $"
 #include "cholerik.h"
-// Konstanty
-#define TRESHOLD     0x90        // rozhodovaci uroven pro okraj areny
+#define THRESHOLD     0x8        // rozhodovaci uroven pro okraj areny 0-15 (od 0.25 do 0.75 Vdd)
-//#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)
+// PIN_A3   Prave cidlo na okraj areny (C1OUT)
+// PIN_A2   Leve cidlo na okraj areny (C2OUT)
 #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_l=FALSE;
-int1  arena_r;
+int1  arena_r=FALSE;
 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 (C1OUT) arena_r=TRUE; else arena_r=FALSE;
-if (input(PIN_A2)) arena_l=TRUE; else arena_l=FALSE;
+   if (C2OUT) 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);
+}
-/******************************************************************************/
+/**** DIAG ********************************************************************/
 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);
+         if((g>100) && arena_l && arena_r) break;  // Preklopen na zada a bily papir na obou cidlech na okraj areny
-      }
+      };
+      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);
+      };
    };
+}
+/**** MAIN ********************************************************************/
 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_ports(NO_ANALOGS);   // komparatory vypnuty
    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_comparator(A3_VR_A2_VR);   // inicializace komparatoru
-   setup_vref(FALSE);
+   setup_vref(VREF_HIGH|THRESHOLD);        // 16 kroku od 0.25 do 0.75 Vdd
-   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);
+   }
+/*----- Main Loop ------------------------------------------------------------*/
    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 – Rev 209 → 240