/*
 * File:     serwomechanizm-main-3-stany-co-90-st.c
 * Author:   elektronikawsrode.pl
 * Program:  uruchamia serwomechanizm od 0 do 90 do 0 do -90 do 0.
 */


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

// Zmieniaj te wartosci, aby znalezc idealne punkty +/- 90 stopni
#define KAT_MINUS_90 300 // Zacznij od 0.6 ms
#define KAT_ZERO     750 // Bez zmian (1.5 ms)
#define KAT_PLUS_90  1225 // Zacznij od 2.4 ms

// 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 (np. 0 stopni)
    // PWM3DC = 750 (binarnie 0b0000001011101110) 
    PWM3DCH = 0b00000010;
    PWM3DCL = 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) {
    inicjalizacja_systemu();
    
    while (1) {
          

        // Ustaw pozycje -90 stopni
        ustaw_wypelnienie(KAT_MINUS_90);
        __delay_ms(2000); // Zaczekaj 2 sekundy

        // Ustaw pozycje 0 stopni
        ustaw_wypelnienie(KAT_ZERO);
        __delay_ms(2000); // Zaczekaj 2 sekundy

        // Ustaw pozycje +90 stopni
        ustaw_wypelnienie(KAT_PLUS_90);
        __delay_ms(2000); // Zaczekaj 2 sekundy

        // Ustaw pozycje 0 stopni
        ustaw_wypelnienie(KAT_ZERO);
        __delay_ms(2000); // Zaczekaj 2 sekundy
    }
}