/*
 * File:     serwomechanizm-main-90-do-90-bez-przerwy.c
 * Author:   elektronikawsrode.pl
 * Program:  uruchamia serwomechanizm od -90 do 90.
 */

// Bity konfiguracyjne 
// CONFIG1
#pragma config FOSC = INTOSC     // (INTOSC oscillator; I/O function on CLKIN pin)
#pragma config WDTE = OFF        // Watchdog Timer Enable (WDT disabled)
#pragma config PWRTE = OFF       // Power-up Timer Enable (PWRT disabled)
#pragma config MCLRE = ON        // MCLR Pin Function Select (MCLR/VPP pin function is MCLR)
#pragma config CP = OFF          // Flash Program Memory Code Protection (Program memory code protection is disabled)
#pragma config BOREN = OFF       // Brown-out Reset Enable (Brown-out Reset disabled)
#pragma config CLKOUTEN = OFF    // Clock Out Enable (CLKOUT function is disabled. I/O or oscillator function on the CLKOUT pin)

// CONFIG2
#pragma config WRT = OFF         // Flash Memory Self-Write Protection (Write protection off)
#pragma config PLLEN = OFF       // PLL Enable (4x PLL disabled)
#pragma config STVREN = ON       // Stack Overflow/Underflow Reset Enable (Stack Overflow or Underflow will cause a Reset)
#pragma config BORV = LO         // Brown-out Reset Voltage Selection (Brown-out Reset Voltage (Vbor), low trip point selected.)
#pragma config LPBOREN = OFF     // Low Power Brown-out Reset enable bit (LPBOR is disabled)
#pragma config DEBUG = OFF       // In-Circuit Debugger Mode (In-Circuit Debugger disabled, ICSPCLK and ICSPDAT are general purpose I/O pins)
#pragma config LVP = OFF         // Low-Voltage Programming Enable (High-voltage on MCLR/VPP must be used for programming)

#define _XTAL_FREQ 500000 // Wewnetrzny oscylator ustawiono na 500 kHz
#include <xc.h>

// Wartosci skrajne (skalibrowane)
#define KAT_MINUS_90 300  // 0.6 ms
#define KAT_ZERO     750  // 1.5 ms
#define KAT_PLUS_90  1225 // 2.45 ms

// --- DEFINICJE DLA PLYNNOSCI RUCHU ---

// Szybkosc ruchu serwa (mniejsza wartosc = szybszy ruch)
// Ten delay wykonuje sie po KAZDYM kroku.
#define SZYBKOSC_RUCHU_MS 10

// Krok (skok) pozycji w kazdej iteracji petli.
// Wieksza wartosc = szybszy, ale mniej plynny ruch.
#define KROK_RUCHU 20 


// Inicjalizuje oscylator systemowy (FOSC) na 500 kHz.
void inicjalizacja_systemu(void) {
    // OSCCON: IRCF<3:0> = 0111 (500 kHz MF) 
    // SCS<1:0> = 10 (Internal oscillator block) 
    OSCCON = 0b00111010; 

    // 1. Konfiguracja pinu RA2 (PWM3) 
    TRISAbits.TRISA2 = 0;   // RA2 jako wyjscie
    ANSELAbits.ANSA2 = 0; // RA2 jako cyfrowe I/O
    
    // 2. Ustawienie zegara PWM3 
    // CS<1:0> = 00 (FOSC), PS<2:0> = 000 (Preskaler 1:1)
    PWM3CLKCON = 0b00000000;
    
    // 3. Ustawienie okresu (Period = 20 ms)
    // PWM3PR = 9999 (binarnie 0b0010011100001111)
    PWM3PRH = 0b00100111;
    PWM3PRL = 0b00001111;
    
    // 4. Ustawienie fazy (Phase = 0)
    // PWM3PH = 0 (binarnie 0b0000000000000000)
    PWM3PHH = 0b00000000;
    PWM3PHL = 0b00000000;

    // 5. Ustawienie poczatkowego wypelnienia (pozycja 0 stopni)
    // PWM3DC = 750 (binarnie 0b0000001011101110)
    PWM3DCH = (KAT_ZERO >> 8); // (0b00000010)
    PWM3DCL = (KAT_ZERO & 0b11111111); // (0b11101110)
    
    // 6. Konfiguracja trybu PWM 
    // EN=1 (Wlaczony), OE=1 (Wyjscie wlaczone)
    // POL=0 (Aktywny-wysoki), MODE=00 (Standard)
    PWM3CON = 0b11000000;
    
    // 7. Zaladowanie wartosci (Immediate Reload) 
    // Ustawienie bitu LDA (Load Buffer Armed) 
    PWM3LDCONbits.LDA = 1;
}

// Ustawia wypelnienie PWM3 (pozycje serwa).
void ustaw_wypelnienie(unsigned int wypelnienie) {
    PWM3DCH = (wypelnienie >> 8);   // Zapisz gorny bajt
    PWM3DCL = (wypelnienie & 0b11111111); // Zapisz dolny bajt (maska binarna)
    
    // Zaladuj nowe wartosci do buforow operacyjnych
    // Uzywamy rejestru PWM3LDCON zgodnie z ustaleniami
    PWM3LDCONbits.LDA = 1; 
}


void main(void) {
    unsigned int pozycja; // Zmienna do przechowywania aktualnej pozycji
    
    inicjalizacja_systemu();
    
    // Ustaw pozycje poczatkowa na -90 stopni
    pozycja = KAT_MINUS_90;
    ustaw_wypelnienie(pozycja);
    __delay_ms(1000); // Poczekaj 1 sekunde na stabilizacje serwa
    
    
    while (1) {
        
        // --- RUCH od -90 do +90 stopni (PLYNNY) ---
        // Petla zwieksza pozycje o KROK_RUCHU
        for (pozycja = KAT_MINUS_90; pozycja < KAT_PLUS_90; pozycja += KROK_RUCHU) {
            ustaw_wypelnienie(pozycja);
            __delay_ms(SZYBKOSC_RUCHU_MS); // Opóznienie miedzy krokami
        }
        
        // Gwarantujemy osiagniecie dokladnej pozycji koncowej
        ustaw_wypelnienie(KAT_PLUS_90);
        // Pauza w skrajnej pozycji (+90)
        __delay_ms(1000); 

        // --- RUCH od +90 do -90 stopni (PLYNNY) ---
        // Petla zmniejsza pozycje o KROK_RUCHU
        for (pozycja = KAT_PLUS_90; pozycja > KAT_MINUS_90; pozycja -= KROK_RUCHU) {
            ustaw_wypelnienie(pozycja);
            __delay_ms(SZYBKOSC_RUCHU_MS); // Opóznienie miedzy krokami
        }
        
        // Gwarantujemy osiagniecie dokladnej pozycji koncowej
        ustaw_wypelnienie(KAT_MINUS_90);
        // Pauza w skrajnej pozycji (-90)
        __delay_ms(1000); 
    }
}