Subversion Repositories svnkaklik

#include "tank.h"

#define DEBUG

#define  TXo PIN_A3           // To the transmitter modulator

#include "AX25.c"             // podprogram pro prenos telemetrie

unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru

unsigned int8 line;           // na ktere strane byla detekovana cara

unsigned int8 speed;          // rychlost zataceni

unsigned int8 rovinka;        // pocitadlo pro zjisteni rovneho useku

unsigned int8 last;           // kde byla cara, kdyz byly minule cidla mimo

unsigned int8 movement;       // obsahuje aktualni smer zataceni

unsigned int8 dira;           // pocita dobu po kterou je ztracena cara

unsigned int8 cihla;          // urcuje za jak dlouho muze byt znova detekovana cihla

// Konstanty pro dynamiku pohybu

#define T_DIRA       87      // po jakem case zataceni se detekuje dira

#define INC_SPEED    1        // prirustek rychlosti v jednom kroku

#define FW_POMALU    230      // trochu mimo caru vnitrni pas

#define FW_ZATACKA   200      // rychlost vnejsiho kola pri zataceni

#define FW_STREDNE   240      // trochu mimo caru vnejsi pas

#define COUVANI      600      // couvnuti zpet na caru, po detekci diry

#define PRES_DIRU    250

#define MAX_ROVINKA  (255-FW_STREDNE)

#define TRESHOLD     10        // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd

#define BUMPER_TRESHOLD 128

#define CIK_CAK      30000

#define T_CIHLA      100      // perioda detekce cihly

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

#define L 0b10  // left

#define R 0b01  // right

#define S 0b11  // straight

//cidla

#define RSENSOR    C2OUT      // Senzory na caru

#define LSENSOR    C1OUT

#define BUMPER     PIN_A4     // Senzor na cihlu

#define DIAG_SERVO      PIN_B3   // Propojka pro diagnosticky mod

#define DIAG_SENSORS    PIN_B2   // Propojka pro diagnosticky mod

#DEFINE SOUND_HI   PIN_A6     // komplementarni vystupy pro piezo pipak

#DEFINE SOUND_LO   PIN_A7

char AXstring[40];   // Buffer pro prenos telemetrie

// makro pro PWM

#define GO(motor, direction, power) if(get_timer0()<=power) \

{direction##motor;} else {stop##motor;}

#int_TIMER2

void TIMER2_isr()

{

   if (speed<255) speed+=INC_SPEED;

   if (rovinka<MAX_ROVINKA) rovinka++;

   if (dira<=T_DIRA) dira++;

   if (cihla>0) cihla--;

}

// Primitivni Pipani

void beep(unsigned int16 period, unsigned int16 length)

{

   unsigned int16 nn;

   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);

   }

}

/******************************************************************************/

void diagnostika()

{

   unsigned int16 n;

   while (input(DIAG_SERVO))   // Propojka, ktera spousti diagnostiku

   {

      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);

   };

   while (input(DIAG_SENSORS))

   {

      int ls, rs;

		while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}

      set_adc_channel(RSENSOR);

      Delay_us(20);

      rs=read_adc();

      set_adc_channel(LSENSOR);

      Delay_us(20);

      ls=read_adc();

      sprintf(AXstring,"L: %U  R: %U\0", ls, rs);  // Convert DATA to String.

      SendPacket(&AXstring[0]);

      delay_ms(1000);

   };

}

///////////////////////////////////////////////////////////////////////////////

void cikcak()

{

unsigned int16 n;

sem1:

   n=CIK_CAK;

	while (0==RSENSOR)       // zkontroluj caru

	{

		if (n==CIK_CAK)							// zmen smer zataceni

		{

			n=0;

			switch(movement)

			{

			case L:

						FL;BR;

						movement=R;

					break;

			case R:

						FR;BL;

						movement=L;

					break;

         case S:

				     FL;BR;

				     movement=R;

                 n=CIK_CAK/2;

			      break;

			}

		}

		n++;

	}

	STOPL;STOPR;

   line = RSENSOR;         // cteni senzoru na caru

   line |= LSENSOR << 1;

   if (line==0) goto sem1;

   												// nasli jsme caru

	line=S;

}

///////////////////////////////////////////////////////////////////////////////

void objizdka()                     // objede cihlu

{

unsigned int16 n;

	BL;BR;Delay_ms(200);

	STOPR;STOPL;

	beep(900,1000);

  // movement=S;

  // cikcak();

   BR; FL; Delay_ms(215);           // otoc se 70° do prava

   FR; FL; Delay_ms(600);           // popojed rovne

   BL; Delay_ms(30);               // otoc se 90° do leva

   STOPL; FR; Delay_ms(600);

   FR; FL; Delay_ms(100);           // popojed rovne na slepo

   for(n=600;n>0;n--)               // popojed rovne ale kontroluj caru

   {

      line = RSENSOR;         // cteni senzoru na caru

      line |= LSENSOR << 1;

      if (line!=0) {Delay_ms(50); break;}

      Delay_ms(1);

   }

   BR;                              // otoc se 60° do prava

   for(n=600;n>0;n--)

   {

      line = RSENSOR;         // cteni senzoru na caru

      line |= LSENSOR << 1;

      if (line!=0) break;

      Delay_ms(1);

   }

   STOPR; STOPL;

   movement=R;

   cikcak();

   cihla=T_CIHLA;

   dira=0;

}

///////////////////////////////////////////////////////////////////////////////

void prejeddiru()                    // vyresi diru

{

unsigned int16 n;

unsigned int8 speed_dira;

   STOPL;STOPR;

   speed_dira=speed;

   beep(1000,500);

   switch (movement)                            //vrat se zpet na caru

   {

   case L:

         for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);}

         STOPL;STOPR;

      break;

   case R:

         for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);}

         STOPL;STOPR;

      break;

   case S:

         goto sem;

      break;

   }

   beep(800,500);

   line=0;

   FR; BL; Delay_ms(300);                 // otoc se na caru

   while(line==0)

   {

      line = RSENSOR;         // cteni senzoru na caru

      line |= LSENSOR << 1;

   }

   FL;BR; Delay_ms(60);

   STOPL; STOPR;

   FL; BR; Delay_ms(500);

   STOPL; STOPR;

   Delay_ms(1000);

   FR;FL;                     //popojed rovne

   for(n=PRES_DIRU;n>0;n--)

   {

      line = RSENSOR;         // cteni senzoru na caru

      line |= LSENSOR << 1;

      if (line!=0) break;

      Delay_ms(1);

   }

sem:

   STOPL; STOPR;

   movement=S;

   cikcak();                                    // najdi caru

   cihla=T_CIHLA;

   dira=0;

}

///////////////////////////////////////////////////////////////////////////////

void main()

{

   unsigned int16 n; // pro FOR

   STOPL; STOPR;     // prepne vystupy na ovladani motoru na output a zastavi

   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator

   port_b_pullups(TRUE);      // pullups pro piano na diagnostiku

   setup_spi(FALSE);

   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM

   setup_timer_2(T2_DIV_BY_4,255,10);    // Casovac pro regulaci

                                         // preruseni kazdych 10ms

   setup_adc_ports(sAN2|VSS_VDD);      // nastaveni A/D prevodniku pro naraznik

   setup_adc(ADC_CLOCK_INTERNAL);

   set_adc_channel(2);

   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro naraznik

   setup_ccp1(CCP_COMPARE_RESET_TIMER);

   CCP_1=(2^10)-1;                        // prevod kazdou 1ms

   setup_comparator(A0_VR_A1_VR);   // inicializace komparatoru pro cidla cary

   setup_vref(VREF_HIGH|TRESHOLD);        // 32 kroku od 0.25 do 0.75 Vdd

   Beep(1000,200);     //double beep

   Delay_ms(50);

   Beep(1000,200);

   Delay_ms(1000);      // 1s

         // povoleni rizeni rychlosti zataceni pres preruseni

   enable_interrupts(INT_TIMER2);

   enable_interrupts(GLOBAL);

/*---------------------------------------------------------------------------*/

   sensors=S;

   line=S;

   last=S;

   movement=S;

   speed=FW_POMALU;

   diagnostika();

 //  cikcak();     // toc se, abys nasel caru

   Delay_ms(500);

   Beep(1000,200);

   Delay_ms(500);

   while(true)       // hlavni smycka (jizda podle cary)

   {

      sensors = RSENSOR;         // cteni senzoru na caru

      sensors |= LSENSOR << 1;

      if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (cihla==0)) objizdka();

      switch (sensors)  // zatacej podle toho, kde vidis caru

      {

         case S:                          // rovne

            FL; FR;  // pokud se jede dlouho rovne, tak pridej

            dira=0;

            movement=S;

            continue;

         case L:                          // trochu vlevo

            GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);

            line=L;

            dira=0;

            movement=L;

            continue;

         case R:                          // trochu vpravo

            GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);

            line=R;

            dira=0;

            movement=R;

            continue;

         default:       // kdyz jsou obe cidla mimo caru, tak pokracuj dal

      }

   rovinka=0;

      if (dira>=T_DIRA) prejeddiru();

      if (last!=line)     // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi

      {

         last=line;

         speed=FW_ZATACKA;

      }

      if (L==line)  // kdyz jsou obe cidla mimo caru, zatoc na caru

      {

         STOPL;

         GO(R, F, speed);

         movement=L;

      }

      else

      {

         STOPR;

         GO(L, F, speed);

         movement=R;

      }

   } // while(true)

}