WIRTUALNY KLUB MODELARZY RC

// zegar 1
   #define CO_ILE_PRZERWANIE1   40000 //50Hz na początek
   TIMSK |= _BV(TOIE1); //wlaczenie przerowania T1
   TCNT1 = 0xffff - CO_ILE_PRZERWANIE1;
   TCCR1A = 0x00;        // włącz tryb czasomierza T/C1
   TCCR1B = _BV(CS11);    // preskaler ck/8

SIGNAL (SIG_OVERFLOW1)
{
  if (servo_state ==0)
  {
//    wysyłamy jedynke na bit 0 portu c
   servo_state = 1;
   PORTC = _BV(0);
//   utawiamy wywołanie przerwania na czas wynikający z połozenia serwa
   TCNT1 = 0xFFFF - 2*servo_level; //2 bo czestotliwosc zegara jest 2Mhz
  }else
  {
   servo_state = 0;
   PORTC = 0x00;
   //   utawiamy wywołanie przerwania na czas wpozostały do 20ms
   TCNT1 = 0xFFFF - (CO_ILE_PRZERWANIE1 - 2*servo_level); // tyle zostąło do pełnego okresu...

  }
}

   while(1)
   {

    _delay_ms(50);
    lcd_clrscr();
    memset(&e1,0, sizeof(e1));

    // zablokowanie przerwania T0
    TIMSK &= ~_BV(TOIE0); //wlaczenie przerowania T0
    int ret = qqueue_remove(&e1);
    // odblokowanie przerwania T0
    TIMSK |= _BV(TOIE0); //wlaczenie przerowania T0

    #define DUZY_SKOK      100
    #define MIN_SKOK      600
    #define MAX_SKOK      2300

    if ( ret != RET_EMPTY_QUEUE )
    {
       if ( e1.msg_type == QMSG_TYPE_BTN_DOWN )
       {
         switch (e1.param1)
         {
            case QID_BTN_LEFT:
               if ( servo_level > MIN_SKOK + DUZY_SKOK )
               {
                  servo_level -= DUZY_SKOK;   
               }
               break;
            case QID_BTN_RIGHT:
               if ( servo_level < MAX_SKOK + DUZY_SKOK )
               {
                  servo_level += DUZY_SKOK;   
               }
                 break;
            case QID_BTN_UP:
               if ( servo_level < MAX_SKOK)
                  {
                  servo_level++;
                  }
               break;
            case QID_BTN_DOWN:
               if ( servo_level > MIN_SKOK)
                  {
                  servo_level--;
                  }
               break;

            case QID_BTN_BACK:
               servo_level = MIN_SKOK;
               break;
            case QID_BTN_OK:
               servo_level = MAX_SKOK;
               break;
   
         }
       }

    }

   
    sprintf( str, "Qsize=%u \nServo Level=%u", qqueue_get_queue_lenght(), servo_level);   
    lcd_puts_p2(str);
   

   }

#include "qservo_control.h"


// zmienne
volatile unsigned char qservo_control_step;                     //aktualny krod kontroli serwa
volatile unsigned char qservo_max_step;                        //maksymalny krok
volatile unsigned int qservo_servo_levels[QSERVO_NO];            //poziomy serwa, tutaj wpisujemy jak chcemy wystareowac serwa
volatile unsigned int qservo_servo_levels_tmp[QSERVO_NO];         //kopia poziomów serwa
//tablica z kolejnymi krokami serwa (krok 0 inicjalizacja i wstawienie wygenerowanie zbocza narastającego,
//krok 1 do max N+1 generowanie zbocza opadającego dla poszczególnych serw, krok ostatni generowanie przerwy do 20ms
volatile struct qservo_control_str qservo_steps[QSERVO_NO+2];      

// inicjalizacja
int qservo_initialize(void)
{
   qservo_control_step = 0;                     // pierwszy krok kontroli serw

   // inicjalizacja sterowania
   for (int i=0; i<QSERVO_NO; i++)
   {
      qservo_servo_levels[i] = QSERVO_INITIAL_STATE;
   }

   
   // ustawienia portu na wyjscie
   DDR(QSERVO_PORT) = 0xFF & QSERVO_MASK;

}

int qservo_do_control(void)
{
   unsigned int min_level;
   signed char min_level_index;
   unsigned char find_next_min;
   signed int i;

   // krok 0 inicjalny, przygotowanie tabelki dla następnych kroków żeby było szybciej
   if (qservo_control_step == 0)
   {
      // rozpisujemy co i jak mamy włączyć
      
      
      // na początek zrobimy sobie kopie levelów serw
      memcpy(qservo_servo_levels_tmp, qservo_servo_levels, sizeof(qservo_servo_levels_tmp));
      
      // teraz musimy sobie posortować od najmniejszego do najwiekszego elementu
      memset(qservo_steps,0,sizeof(qservo_steps));      // kasujemy całą tabelke
      qservo_steps[0].servos_bits |= 0xFF & QSERVO_MASK;  // krod 0 ma zapalić wszystkie bity odpowiedzialne za serwa
      qservo_max_step = 0;

      // dopuki musimy jest jeszcze do znalezienia cokolwiek to robimy pętelke
      do
      {
         min_level = 0xFFFE; // ustawiamy sobie na dużą wartość, wszystko bedzie poniżej
         min_level_index = -1;

         // znajdujemy najmniejszy element
         for (i=0; i<QSERVO_NO; i++)
         {
            if (qservo_servo_levels_tmp[i] <= min_level )
            {
               min_level_index = i;
               min_level = qservo_servo_levels_tmp[i];   
            }

         }
         // sprawdzamy czy cos się znalazło
         if (min_level_index >= 0)
         {
            // zaznaczamay wartosc w tabeli ze juz została wybrana
            qservo_servo_levels_tmp[min_level_index] = 0xFFFF; // bedzie zawsze większe i nigy nie zostanie wybrane
            // dodajemy nasze serwo do tabeli wylaczenia qservo_steps
            // wyszukujemy czy jest już taka pozycja w naszej tabeli.
            char val_found = 0;
            for (i=0; i<=qservo_max_step; i++)
            {
               // znajdujemy zawartośc z taką sama zawartością serwa
               if (qservo_steps[i].us_to_next_step == min_level)
               {   
                  val_found = 1;
                  break;
               }   
            }
            if ( val_found == 1)
            {
               // w i mamy index z takim samym opużnieniem wiec gasimy tam bit dla odpowiedniego serwa
               qservo_steps[i].servos_bits &= ~_BV(min_level_index);
            }else
            {
               // nie ma takiej wartości więc ją dostawiamy i zwiększamy max_krok
               qservo_max_step++;
               qservo_steps[qservo_max_step].servos_bits = qservo_steps[qservo_max_step-1].servos_bits & ~_BV(min_level_index);
               qservo_steps[qservo_max_step].us_to_next_step = min_level;
            }
         }
      }while( min_level_index >= 0);

      // dodajemy krok który bedzie stanowił przerwe do 20ms
      qservo_steps[qservo_max_step+1] = qservo_steps[qservo_max_step];
      qservo_steps[qservo_max_step+1].us_to_next_step = 20000 - qservo_steps[qservo_max_step].us_to_next_step;
      //qservo_max_step++; krok potrzebny tylko do obliczeń

      // musimy sobie jeszcze prawidłowo przeliczyć różnice wywołania
      for(i=0; i<=qservo_max_step; i++)
      {
         qservo_steps[i].us_to_next_step = qservo_steps[i+1].us_to_next_step - qservo_steps[i].us_to_next_step;
      };
      
   }

//  ten fragment wywołuje się co każde przerwania, dla każdego kroku
   //(zapamietujemy co wstawić)wstawiamy do portu odpowiednią wartosc
   unsigned char servos_bits = qservo_steps[qservo_control_step].servos_bits & QSERVO_MASK;
   
   
   unsigned int us_to_next_step;
   us_to_next_step = qservo_steps[qservo_control_step].us_to_next_step;
   if ( qservo_control_step < qservo_max_step)
   {
      qservo_control_step++;
   }else
   {
      qservo_control_step = 0;
   }

   //zapisujemy do portu
   QSERVO_PORT = servos_bits;
   // ustawiamy nastepne przerwanie w zależności od zegara (tutaj zegar T1, z freq 2MHz)
   TCNT1 = 0xFFFF - 2*us_to_next_step; //2 bo czestotliwosc zegara jest 2Mhz

   return RET_OK;
}