WebSVN – svnkaklik – /roboti/istrobot/3Orbis/bak/06 PD Regulator/master/main.c

#include ".\main.h"

#define  KOLMO1      225         // predni kolecko sroubem dopredu
#define  KOLMO2      30          // predni kolecko je hlavou sroubu dozadu
#define  STRED       128         // sredni poloha zataceciho kolecka
#define  BEAR1       6//10           // 3 stupne zataceni
#define  BEAR2       10//27
#define  BEAR3       40
#define  SPEEDMAX    160         // ANSMANN=140; GP=120; maximalni rozumna rychlost
#define  R17         255         // X nasobek rozumne rychlosti
#define  DOZNIVANI   10
#define  L           1     // cara vlevo
#define  S           2     // casa mezi sensory
#define  R           0     // cara vpravo

// servo
#define  SERVO PIN_A2

// IR
#define IRTX      PIN_B2
#define  CIHLA    PIN_A3

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

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
unsigned int8 speed;        // maximalni povolena rychlost
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky

short int preteceni;          // flag preteceni timeru1

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

// makro pro PWM pro motory
#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);
}
*/
////////////////////////////////////////////////////////////////////////////////
#int_TIMER1
TIMER1_isr()
{
   preteceni = true;
}
////////////////////////////////////////////////////////////////////////////////
#int_TIMER2
TIMER2_isr()      // ovladani serva
{
   unsigned int8 n;

   output_high(SERVO);
   delay_us(1000);
   for(n=uhel; n>0; n--) Delay_us(2);
   output_low(SERVO);
}

////////////////////////////////////////////////////////////////////////////////
short int IRcheck()                 // potvrdi detekci cihly
{
   output_high(IRTX);               // vypne vysilac IR
   delay_ms(100);

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

   if(true==bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal
   {
      output_low(IRTX);             // zapne vysilac IR
      delay_ms(100);

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

      if(false==bit_test(sensors,7))      // otestuje, jestli je detekovana cihla
      {
         output_high(IRTX);            // vypne vysilac IR
         delay_ms(100);

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

         output_low(IRTX);             // zapne vysilac IR
         if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla
      }
   };
   output_low(IRTX);             // zapne vysilac IR
   return 0; // vrat 0, kdyz je detekovano ruseni
}
////////////////////////////////////////////////////////////////////////////////
void objizdka()
{
   int8 shure=0;
   unsigned int16 n;

   BR;BL;
   Delay_ms(400);
   STOPR;STOPL;

   uhel=KOLMO1;      // nastav zataceci kolecko kolmo na osu robota
   Delay_ms(100);
   BL;FR;
   Delay_ms(200);    // minimalni toceni, kdyby se zastavilo sikmo k cihle

   While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru
   {
       sensors = spi_read(0);         // cteni senzoru
       sensors=~sensors;
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
   }
   STOPL; STOPR;

   for (n=0;n<1500;n++)    // vystred se na hranu cihly
   {
      if(!input(CIHLA)) {BL; FR;} else {FL; BR;};
      delay_ms(1);
   }
   STOPR;STOPL;

   uhel=STRED;          // dopredu
   delay_ms(100);
   FR; FL;
   delay_ms(500);
   BL;BR;
   delay_ms(200);
   STOPL;STOPR;

   uhel=STRED+BEAR3;    // doprava
   delay_ms(100);
   FL;
   delay_ms(200);
   uhel=STRED+BEAR2;    // min doprava
   FL;FR;
   delay_ms(200);
   uhel=STRED+BEAR1;    // jeste min doprava
   FL;FR;
   delay_ms(200);
   uhel=STRED;          // rovne
   FL;FR;
   delay_ms(100);
   While((sensors & 0b01111111)!=0) //dokud neni cara
   {
       sensors = spi_read(0);         // cteni senzoru
       sensors=~sensors;
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
   }
   BL; BR;
   delay_ms(400);

   uhel=STRED-BEAR3;    // doleva
}

////////////////////////////////////////////////////////////////////////////////
void main()
{

   unsigned int8 n;
   unsigned int8 i,v;
   unsigned int8 last_sensors;
   unsigned int8 j=0;

   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_16);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);
   setup_timer_2(T2_DIV_BY_16,140,16);
   setup_oscillator(OSC_8MHZ|OSC_INTRC);

   STOPR; STOPL;  // zastav motory
   Lmotor=0;Rmotor=0;

   uhel = STRED;    // nastav zadni kolecko na stred
   rovinka = 0;

   enable_interrupts(INT_TIMER2);
   enable_interrupts(INT_TIMER1);
   enable_interrupts(GLOBAL);

   output_low(IRTX); // zapni IR vysilac

   delay_ms(1000);

//!!!!
speed=SPEEDMAX;

   while(true)
   {

      GO(L,F,Lmotor);GO(R,F,Rmotor);   // zapni motory PWM podle promenych Lmotor a Rmotor

      delay_us(1500);                  // cekani na SLAVE, nez pripravi data od cidel

      last_sensors = sensors;
      j++;

      output_low(STROBE);              // vypni zobrazovani na posuvnem registru
      sensors = spi_read(0);           // cteni senzoru
      sensors=~sensors;
      output_high(STROBE);       // zobraz data na posuvnem registru




      if(false == preteceni)
      {
         v = 255 - (get_timer1() >> 8);
         v >>= 3;
      }
      else
      {
         v=0;
         preteceni=false;
      }



      i=0;                       // havarijni kod
      for (n=0; n<=6; n++)
      {
         if(bit_test(sensors,n)) i++;
      }
      if (i>3) While(true){STOPR; STOPL;}; // zastavi, kdyz je cerno pod vice nez tremi cidly



      if(bit_test(sensors,7))    // detekce cihly
      {
//!!!         objizdka();       // objede cihlu
      }



      if(bit_test(sensors,3)) //...|...//
      {
         uhel=STRED;
         Lmotor=speed;
         Rmotor=speed;
         line=S;
         set_timer1(0);
         if (rovinka < 255) rovinka++;
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
      {
         if(sensors != last_sensors) uhel=STRED - BEAR3 - v;
         if(j>=DOZNIVANI)
         {
            j=0;
            if(uhel<STRED - BEAR3) uhel++;
         }
         Lmotor=0;
         Rmotor=speed;
         line=L;
         set_timer1(0);
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if(bit_test(sensors,6)) //......|//
      {
         if(sensors != last_sensors) uhel=STRED + BEAR3 + v;
         if(j>=DOZNIVANI)
         {
            j=0;
            if(uhel>STRED + BEAR3) uhel--;
         }
         Rmotor=0;
         Lmotor=speed;
         line=R;
         set_timer1(0);
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if(bit_test(sensors,1)) //.|.....//
      {
         if(sensors != last_sensors) uhel=STRED - BEAR2 - v;
         if(j>=DOZNIVANI)
         {
            j=0;
            if(uhel<STRED - BEAR2) uhel++;
         }
         Lmotor=speed-50;
         Rmotor=speed;
         line=S;
         set_timer1(0);
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if(bit_test(sensors,5)) //.....|.//
      {
         if(sensors != last_sensors) uhel=STRED + BEAR2 + v;
         if(j>=DOZNIVANI)
         {
            j=0;
            if(uhel>STRED + BEAR2) uhel--;
         }
         Rmotor=speed-50;
         Lmotor=speed;
         line=S;
         set_timer1(0);
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if (bit_test(sensors,2)) //..|....//
      {
         if(sensors != last_sensors) uhel=STRED - BEAR1 - v;
         if(j>=DOZNIVANI)
         {
            if(uhel<STRED - BEAR1)uhel++;
            j=0;
         }
         Lmotor=speed;
         Rmotor=speed;
         line=S;
         set_timer1(0);
         if (rovinka<255) rovinka++;
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if (bit_test(sensors,4)) //....|..//
      {
         if(sensors != last_sensors) uhel=STRED + BEAR1 + v;
         if(j>=DOZNIVANI)
         {
            j=0;
            if(uhel>STRED + BEAR1) uhel--;
         }
         Rmotor=speed;
         Lmotor=speed;
         line=S;
         set_timer1(0);
         if (rovinka<255) rovinka++;
//         if (speed > SPEEDMAX) speed--;
         continue;
      }

      if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
      {
         if (rovinka>250)
         {
            BL; BR;
            Delay_ms(100);
         };
         rovinka=0;
         speed=SPEEDMAX;
      };
   }
}
Subversion Repositories svnkaklik

(root)/roboti/istrobot/3Orbis/bak/06 PD Regulator/master/main.c – Rev 381
