/**************************************************************
   Schemat podłączeń programowanego układu

                 +----v----+
           MCLR [| 1     40|] 
                [| 2     39|] 
                [| 3     38|] 
                [| 4     37|] 
                [| 5     36|] 
                [| 6     35|] 
                [| 7     34|] 
                [| 8     33|] 
                [| 9     32|] VDD
                [|10     31|] VSS
            VDD [|11     30|] 
            VSS [|12     29|] 
(20 MHz) / OSC1 [|13     28|]
         \ OSC2 [|14     27|]
                [|15     26|] 
                [|16     25|] 
                [|17     24|]
                [|18     23|] 
  serwo0 <- RD0 [|19     22|] 
  serwo1 <- RD1 [|20     21|] RD2 -> serwo2
                 +---------+
                 PIC16F877A
***************************************************************/

#include <htc.h>
//definiujemy szybkość oscylatora dla funkcji __delay_
#define _XTAL_FREQ 20000000

//oscylator szybszy od 10 MHz (FOSC_HS)
//watchdog wyłączony (WDTE_OFF)
//wyłączone LVP (Low-Voltage ICSP Programming) (LVP_OFF)
__CONFIG(FOSC_HS & WDTE_OFF & LVP_OFF);

//tablica pozycjonowania serwomechanizmów
char serwa[21] = {
0b00000111,	//0 - 0,70 ms
0b00000111, //1 - 0,78 ms
0b00000111, //2 - 0,86 ms
0b00000111, //3 - 0,94 ms
0b00000111, //4 - 1,02 ms
0b00000111, //5 - 1,10 ms
0b00000111, //6 - 1,18 ms
0b00000111, //7 - 1,26 ms
0b00000111, //8 - 1,34 ms
0b00000111, //9 - 1,42 ms
0b00000111, //10 - 1,50 ms
0b00000111, //11 - 1,58 ms
0b00000111, //12 - 1,66 ms
0b00000111, //13 - 1,74 ms
0b00000111, //14 - 1,82 ms
0b00000111, //15 - 1,90 ms
0b00000111, //16 - 1,98 ms
0b00000111, //17 - 2,06 ms
0b00000111, //18 - 2,14 ms
0b00000110, //19 - 2,22 ms
0b00000110  //20 - 2,30 ms
};

//najstarsze ustawione indeksy w tablicy serwa
char indeksy[3] = {18, 20, 20};

//funkcja wysyłająca do serwomechanizmów impulsy
//określonej długości
Sterowanie_serwami()
{
	char i;
	//pierwszy impuls = 0,7 ms
	PORTD = 0x07;
	__delay_us(620);
	//ustawienie ramienia pod właściwym kątem
	for(i=0; i<=20; i++)
	{
		PORTD = serwa[i];
		__delay_us(77);
	}
	//wyłącz impuls na wszystkich serwach
	PORTD = 0x00;
}

//zmiana długości wysyłanego impulsu
//dla serwa = numer_serwa 
//ostatni ustawiony indeks = indeks
Zmiana_tablicy(char numer_serwa, char indeks)
{
	char i;
	//maska dla sumy bitowej
	char maska = (1<<numer_serwa);	
	//ustawienie 1
	for(i=0; i<=indeks; i++) serwa[i] |= maska;
	//maska dla iloczynu bitowego
	maska = ~maska;
	//wyzerowanie
	for(i=indeks+1; i<=21; i++) serwa[i] &= maska;
}

void main()
{
	unsigned char i, j;		//zmienne pomocnicze
	ADCON1 = 0x06;			//wyłączenie linii analogowych 
                    		//(wszystkie linie cyfrowe)
	TRISDbits.TRISD0 = 0;	//linia RD0 wyjściowa
	TRISDbits.TRISD1 = 0;	//linia RD1 wyjściowa
	TRISDbits.TRISD2 = 0;	//linia RD2 wyjściowa

	for(;;)					//pętla nieskończona
	{
		//1. pozycja początkowa
		for(i=0; i<160; i++)
		{
			Sterowanie_serwami();
			//zaczekaj 15 ms
			__delay_ms(15);
		}
		//2. wyprostowanie ramienia
		for(i=0; i<12; i++)
		{
			for(j=0; j<3; j++)
			{
				Sterowanie_serwami();
				//zaczekaj 14 ms
				__delay_ms(14);
				indeksy[j]--;
				Zmiana_tablicy(j, indeksy[j]);
			}	
		}
		//3. ramię wyprostowane
		for(i=0; i<160; i++)
		{
			Sterowanie_serwami();
			//zaczekaj 15 ms
			__delay_ms(15);
		}
		//4. schowanie ramienia
		for(i=0; i<12; i++)
		{
			for(j=0; j<3; j++)
			{
				Sterowanie_serwami();
				//zaczekaj 14 ms
				__delay_ms(14);
				indeksy[j]++;
				Zmiana_tablicy(j, indeksy[j]);
			}	
		}
	}
}
