/*
 * File:   main.c
 * Author: Martin
 *
 * Created on 21. únor 2016, 22:04
 */

//#include <p18lf46k22.h>
#include <xc.h>
#include <stdio.h>
#include "stdint.h"
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <plib/delays.h>
#include <plib/pwm.h>
#include <plib/timers.h> 
#include <plib/usart.h>
#include <plib/adc.h>

//#include <usart.h>
#include "GLCD.h"
#include "convert.h"


//-----------------------------------------------------------------------------
//INIT SETTINGS
//-----------------------------------------------------------------------------
#pragma config FOSC = INTIO67
#pragma config PLLCFG = ON
#pragma config PRICLKEN = ON
#pragma config PWRTEN = ON
#pragma config IESO = OFF 
#pragma config WDTEN = OFF, LVP = OFF
#pragma config WDTPS = 1024 
#pragma config BOREN = SBORDIS 
#pragma config PBADEN = OFF 
#pragma config HFOFST=OFF
#pragma config MCLRE=EXTMCLR

#define apply_Q(x) ((x) >> 8) //exponencialni kumulace

#define _USE_64MHZ_
#define _XTAL_FREQ SYS_FREQ 
#define FOSC 64000000
#define BUFF 20
#define FPWM 40000 //frekvence PWM
#define SQRT2 1.4142135624 //odmocnina ze 2
#define DegRad 0.0174532925 //prevod stupne na radiany
#define UART_SPEED 19200 // zvolena init rychlost uartu
#define VOUT_LIMIT 4.16 //maximalni vystupni proud
#define VDD 340 //napeti na vstupu PWM 350v
#define VREF 2.048 //interni reference
#define VDD_ADC 420 //napeti pro prevod ADC (napocitane vstupni max napeti)
#define PID_INTEG 0.7 //pocatecni integrace (takova pomoc regulaci, asi vyhodit v budoucnu..)


#define VOUT_MAX 230
#define VOUT_MIN 20

#define FREQ_MAX 60
#define FREQ_MIN 50

#define TMR1_CORRECT 1780//korekce frekvence vystupu
#define SPWM_STEPS 50//50 //50 kroku na pul-periodu
#define PHASE_STEP 45 //krok nastavovani faze

#define INIT_VOLTAGE 110 // RMS vystupni napeti
#define INIT_FREQ 50 //vystupni frekvence
#define INIT_DIP_TIME_REP 10 //perioda opakovani v sekundach
#define INIT_DIP 0.4 //pokles na procent
#define INIT_DIP_COUNT 600//0.5 //pul_period poklesnutych
#define INIT_PHASE 45 //faze ve ktere sepnout

#define KPROP 0.02; //04 zesileni proporcionalni casti
#define KINTEG 1; // zesileni integracni casti

//-----------------------------------------------------------------------------
//PROTOTYPES
//-----------------------------------------------------------------------------
//void OpenTimer2( unsigned char config );
//void OpenPWM1(char period);                 // Tpwm =[(period) + 1] x 4 x Tosc x TMR2 prescaler = [] x 4 x 62.5ns x 1
//void SetDCPWM1( unsigned int dutycycle );
//void SetOutputPWM1 (unsigned char outputconfig, unsigned char outputmode);
void InitPWM(void); 
void InitializeClock(void);
void InitTimers(void);
void hi_interrupt(void);
void lo_interrupt(void);
void GenLookup(void);
void SetDipStart(void);
void PID(void);
void InitADC(void);
int LoadADC(void);
void InitUSART(void);
//char RecvUSART(void);
//void SendUSART(char s_byte);
void GetVolCur(void);
char my_strcmp(unsigned char *string1,unsigned char *string2,unsigned char begin,unsigned char end);
unsigned char calculate_crc(unsigned char *string);
unsigned char check_crc(unsigned char *string);
void send_str(unsigned char *string);
char my_strcpy(unsigned char *string1,unsigned char *string2,unsigned char begin);

//-----------------------------------------------------------------------------
//PORT SETUP
//-----------------------------------------------------------------------------
#define P_NEG LATEbits.LATE0
#define V1_NEG LATEbits.LATE1
#define V2_NEG LATEbits.LATE2

//#define OUTPUT_EN LATCbits.LATC0        // output enable
#define OUTPUT_EN LATCbits.LATC1        // output enable

#define ENCODER_A PORTCbits.RC3        // Encoder A Pin
#define ENCODER_B PORTCbits.RC4        // Encoder B Pin
#define ENCODER_TL PORTCbits.RC5        // Encoder Button Pin

#define LED_O LATAbits.LATA3        // Ouput LED
#define POS LATAbits.LATA4        // Positive output
#define NEG LATAbits.LATA5        // Negative output
#define TL_1 PORTAbits.RA6        // Button 1
#define TL_2 PORTAbits.RA7        // Button 2

//-----------------------------------------------------------------------------
//GLOBAL VARIABLES
//-----------------------------------------------------------------------------
//sine cyccle
volatile unsigned char position=0;
volatile unsigned char deg=0;

//sine tab gneration
volatile unsigned int max_duty = FOSC/(FPWM*4);
volatile float SinTab[SPWM_STEPS+1];

//timers setting
volatile unsigned int tmr0_reg = 65535-(FOSC/(200*8)); //200 Hz
volatile unsigned int tmr1_reg = 65535-(FOSC/((unsigned int)INIT_FREQ*SPWM_STEPS*8))+TMR1_CORRECT; //40 KHz
volatile unsigned int tmr3_reg = 65535-(FOSC/((unsigned int)1000*4));//63936;//256-(FOSC/(10000*32)); //10 KHz

//encoder and buttoms controll
volatile unsigned char menu_refresh_flag=1;
volatile unsigned char tl1_flag=0;
volatile unsigned char tl2_flag=0;
volatile unsigned char encoder_tl_flag=0;
volatile unsigned char encoder_flag=0; 
volatile float encoder_counter=0;
volatile float encoder_max=100;
volatile float encoder_min=0;
volatile float encoder_step=1;

//communication
volatile unsigned char remote_ctr = 0;
volatile unsigned char recieved_flag = 0;
volatile unsigned char buff_i=0;
volatile unsigned char rcv_buf[BUFF];

//output setting
volatile float voltage=INIT_VOLTAGE;//*SQRT2; //nastavene vystupni napeti
volatile unsigned int vol_show = 0; //napeti ktere se bude zobrazovat uzivateli
volatile float vout=0; //zmerene napeti
volatile float iout=0; //zmereny proud
volatile unsigned char freq=INIT_FREQ; //output frequency
volatile unsigned char acdc=1; //AC = 1, DC = 0
volatile unsigned char iout_flag = 0;
volatile unsigned char output = 0;
//volatile unsigned int peakup=0;

//dip
volatile unsigned char dip_enable=0;
volatile float dip=INIT_DIP; //pokles na procent
volatile float dip_count=INIT_DIP_COUNT; //pul_period k poklesu
volatile float dip_count_done = INIT_DIP_COUNT;//kolik dipu probehlo

//phase
volatile unsigned int dip_phase_set=INIT_PHASE; //0-360 krok 45
volatile unsigned char dip_phase=0; // normovana faze na pocet kroku PWM (0-SPWM_STEPS)
volatile unsigned char dip_phase_polarity=1; // 1/0 kladna/zaporna perioda
volatile unsigned char dip_phase_is = 0; // spousteni: 0 ceka na fazi, 1 faze byla tak muze

//repeating
volatile unsigned char rep_n = 0; //pocet opakovani
volatile unsigned char rep_set_n = 2; //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
volatile unsigned int dip_time_rep = INIT_DIP_TIME_REP;
volatile unsigned long rep_set_time = (unsigned long)2*INIT_FREQ*SPWM_STEPS*INIT_DIP_TIME_REP; //zvladne 16,777,215 / 5min pri 60hz a 50 krocich = 1,800,000
volatile unsigned long rep_time = (unsigned long)2*INIT_FREQ*SPWM_STEPS*INIT_DIP_TIME_REP;

//pid
volatile float pid_reg=0.1;
volatile float integ = 0.7;
volatile unsigned char pid_en = 1;/////////
//-----------------------------------------------------------------------------


void interrupt high_isr (void)
{
    // -----------------------
    // High Priority Interrupts
    // ----------------------- 
    static unsigned char posflag=1;
    static unsigned char negflag=0;
    //static unsigned char stoptime=50;
    //static unsigned char stoptime_act=0;
    if (PIR1bits.TMR1IF)
    {           
        //WriteTimer1(tmr1_reg);
        //PIR1bits.TMR1IF = 0;          // Clear interrupt flag
        
        if(deg>SPWM_STEPS)deg=0;

        //repeat counting
        if(rep_time == rep_set_time)
        {
             rep_time=0;
             rep_n++;
             dip_phase_is=0;
             dip_count_done=0;
        }
        if((rep_n < rep_set_n)&&(dip_enable==1))
        {
            rep_time++;
        }
        else 
        {           
            dip_enable=0;
        }
        
        //dips application
        if((deg==(char)dip_phase)&&(dip_phase_polarity == POS)&&(dip_enable==1)) dip_phase_is=1;
        
        if((dip_count_done < dip_count) && (dip_phase_is==1))
        {
            if (deg==(char)dip_phase) dip_count_done++;
            SetDCEPWM1((int)(SinTab[deg]*dip*pid_reg));
        }
        else
        {
            SetDCEPWM1((int)(SinTab[deg]*pid_reg));   
            //SetDCEPWM1(SinTab[deg]);               
        }
        
        //regulation
        if(deg==(int)(SPWM_STEPS/2)-1)
        {
            P_NEG = 1;
            V1_NEG = 1;
            V2_NEG = 1;
            Delay10TCYx(120);
            P_NEG = 0;
            V1_NEG = 0;
            V2_NEG = 0;   
        }        
        if(deg==(int)(SPWM_STEPS/2))
        {
            GetVolCur();
            PID();
            if((iout > VOUT_LIMIT) && (output==1))
            {
                iout_flag = 1;
                tl2_flag = 1;
            } 
        }

/*        if(stoptime_act==stoptime)
        {
            if(acdc)
            {            
                POS = negflag;
                NEG = posflag;
            }
            else
            {
                POS = 1;
                NEG = 0;            
            }                
        }
        else stoptime_act++;
*/        
        //degrees counting
        if(deg==(SPWM_STEPS))
        {
            deg=0;
            //stoptime_act=0;
            if(acdc)
            {
                /*
                posflag = POS;
                negflag = NEG;            
                //POS = 0; ///////ochranny interval zajisten HW
                //NEG = 0;
                //Delay10TCYx(1); //sepnuti a rozepnuti tranzistoru
                POS = negflag;
                NEG = posflag;
                 */
                POS = !POS;
                NEG = !NEG;
            }
            else
            {
                POS = 1;
                NEG = 0;            
            }
        } 
        //else if(stoptime_act==stoptime) deg++;
        else deg++;
   
        WriteTimer1(tmr1_reg);
        PIR1bits.TMR1IF = 0;          // Clear interrupt flag        
    }
    return;
}

void interrupt low_priority low_isr (void)
{ 
    // -----------------------
    // Low Priority Interruptsff
    // -----------------------     
    if (PIR1bits.RCIF)
    {    
        if(recieved_flag = 0)
        {
            rcv_buf[buff_i] = Read1USART();//getcUSART();
            if(buff_i<BUFF) buff_i++;
            else rcv_buf[buff_i-1]='\r';
        }
        else Read1USART();
        if(rcv_buf[buff_i-1]=='\r')
        {
            rcv_buf[buff_i]=NULL;    
            buff_i=0;
            recieved_flag = 1;
        }
        PIR1bits.RCIF = 0;
    }     
    if (INTCONbits.TMR0IF)
    {  
        static unsigned char menu_refresh=0;    
    
        static unsigned char    encoder_A;
        static unsigned char    encoder_B;
        static unsigned char    encoder_A_prev=0;       
        static unsigned char    encoder_tl_prev=0;       
        static unsigned char    tl1_prev=0;       
        static unsigned char    tl2_prev=0;         
        //WriteTimer0(tmr0_reg);   // 200 Hz
        //INTCONbits.TMR0IF = 0;          // Clear interrupt flag
        
        encoder_A = ENCODER_A;
        encoder_B = ENCODER_B;
        
        
        if(ENCODER_A == 0) encoder_flag = 1;

        if((!encoder_A) && (encoder_A_prev)) {

            // A has gone from high to low
            if(encoder_B) {
                // B is high so clockwise
                if(encoder_counter<encoder_max) encoder_counter=encoder_counter+encoder_step;
            }    
            else {
                // B is low so counter-clockwise            
                if(encoder_counter>encoder_min) encoder_counter=encoder_counter-encoder_step;
            }    
            encoder_flag = 1;           // Set flag to indicate change to PWM needed
        }    
        encoder_A_prev = encoder_A;     // Store value for next time

        //buttons        
        if((ENCODER_TL == 1)&&(encoder_tl_prev==1))
        {
            encoder_tl_prev = 0;
            encoder_tl_flag = 1;
        }
        else encoder_tl_prev=0;        
        if(ENCODER_TL == 0) encoder_tl_prev = 1; 
        
        
        if((TL_1 == 1)&&(tl1_prev==1))
        {
            tl1_prev = 0;
            tl1_flag = 1;
        }
        else tl1_prev=0;
        if(TL_1 == 0) tl1_prev=1;
        
        if((TL_2 == 1)&&(tl2_prev==1))
        {
            tl2_prev = 0;        
            tl2_flag = 1;
        }
        else tl2_prev=0;
        if(TL_2 == 0) tl2_prev = 1;       
      
        if(menu_refresh==60)
        {
            menu_refresh=0;
            menu_refresh_flag=1;         
        }
        else menu_refresh++;
    
        WriteTimer0(tmr0_reg);   // 200 Hz
        INTCONbits.TMR0IF = 0;          // Clear interrupt flag
    }
    /*
    if (PIR2bits.TMR3IF)
    {
        unsigned int on_len = 1;
        static unsigned int on_act = 0;
        if(output==1)
        {
            if(peakup<200)
            {
                OUTPUT_EN=1;
                peakup++;
            }
            else
            {
              if(!OUTPUT_EN)OUTPUT_EN=1;
              else
              {
                if(on_act>=on_len)
                {                  
                    OUTPUT_EN=0;
                    on_act=0;
                }
                else on_act++;
              }
            }                
        }
        else OUTPUT_EN=0;
        WriteTimer3(tmr3_reg);
        PIR2bits.TMR3IF = 0;
    }  
     */     
    return;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void main(void) {
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------    
    unsigned char init_disp=1;
    unsigned char menu_max=3;
    unsigned char menu_act=0;
    unsigned char set_menu=0;
    unsigned char set_act=0;     
    const unsigned char ok[3]={'O','K',NULL};
      
    ANSELA = 0b00000111; //define analog inputs
    ANSELB = 0b00000000; 
    ANSELC = 0b00000000; 
    ANSELD = 0b00000000; 
    ANSELE = 0b00000000; 
    TRISA = 0b11000111;
    TRISC = 0b10111000;    
    TRISE = 0b00000000;

    LATCbits.LATC2=0;
    OUTPUT_EN = 0;
    P_NEG = 0;
    V1_NEG = 0;
    V2_NEG = 0;
    LED_O = 0;
    POS = 1;
    NEG = 0;

    Delay10KTCYx(16);
       
    
    InitializeClock();  
    SetDipStart();
    InitTimers();
    Init_GLCD();
    InitUSART();
    InitADC();
    GenLookup();
    InitPWM();
    LATCbits.LATC2=0;
    CloseEPWM1();
    
    RCONbits.IPEN = 1;            // Enable interrupt priority 
    INTCONbits.PEIE = 1;          //enable peripheral interrupts
    INTCONbits.GIE = 1;             
    INTCONbits.GIEH = 1;
    INTCONbits.GIEL = 1;
    
    while (1)
    {
        // menu button
        if(tl2_flag)
        {
            remote_ctr=0;
            
            if(output==1)output=0;
            else output=1;
            
            if(output==1)
            {
                set_act=0;
                OUTPUT_EN=1;
                LED_O=1;
                Delay10KTCYx(0);
                //peakup=0;
                iout_flag = 0;            
                GenLookup();
                InitPWM();
            }
            else 
            {
                CloseEPWM1();
                LED_O=0;
                LATCbits.LATC2=0;
                Delay10KTCYx(0);
                OUTPUT_EN=0;                
            }
            tl2_flag=0;
        }        
        
        // UART           
        if(recieved_flag == 1)
        {                    
            if(strlen(rcv_buf)<3);           
            else if(check_crc(rcv_buf)==1)
            {
                my_strcpy(rcv_buf,"WRONG_CRC",0);
                send_str(rcv_buf);
                remote_ctr=0;
            }
            else
            {
                if(remote_ctr==0)
                {
                    remote_ctr=1;
                    ClearScreen();
                    SetPos(0,0);
                    PutMessage("REMOTE CONTROLL");            
                }                    
                
                if(my_strcmp(rcv_buf,"END",0,2)==0)
                {
                    remote_ctr=0; 
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"PING",0,3)==0)
                {                     
                    my_strcpy(rcv_buf,"PONG",0);
                    send_str(rcv_buf);
                }
                //OUTPUT MENU
                if(my_strcmp(rcv_buf,"VOLT ",0,4)==0)
                {
                    if(output==1)
                    {
                        tl2_flag=1;
                        CloseEPWM1();
                        LED_O=0;
                        LATCbits.LATC2=0;
                    }                    
                    voltage=str_num(rcv_buf, 5);
                    GenLookup();
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"VOLT?",0,4)==0)
                {
                    my_ftoa(vout, rcv_buf); 
                    send_str(rcv_buf);                        
                }                
                if(my_strcmp(rcv_buf,"CURR?",0,4)==0)
                {
                    my_ftoa(iout, rcv_buf); 
                    send_str(rcv_buf);
                }                         
                if(my_strcmp(rcv_buf,"FREQ ",0,4)==0)
                {
                    freq=str_num(rcv_buf, 5);
                    tmr1_reg = 65535-(FOSC/((float)freq*SPWM_STEPS*8))+TMR1_CORRECT;
                    rep_set_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep; //zvladne 16,777,215 / 5min pri 60hz a 50 krocich = 1,800,000
                    rep_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep;  
                    send_str(ok);
                }
                if(my_strcmp(rcv_buf,"FREQ?",0,4)==0)
                {
                    itoa(rcv_buf,freq, 10); 
                    send_str(rcv_buf);                        
                }
                if(my_strcmp(rcv_buf,"ACDC ",0,4)==0)
                {
                    if(output==1)
                    {
                        tl2_flag=1;
                        CloseEPWM1();
                        LED_O=0;
                        LATCbits.LATC2=0;
                    }                    
                    acdc=str_num(rcv_buf, 5);
                    GenLookup();
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"ACDC?",0,4)==0)
                {
                    if(acdc==1)my_strcpy(rcv_buf,"1",0);
                    else my_strcpy(rcv_buf,"0",0);
                    send_str(rcv_buf);                        
                }
                if(my_strcmp(rcv_buf,"OUTP ",0,4)==0)
                {
                    if(output!=str_num(rcv_buf, 5)) tl2_flag=1;
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"OUTP?",0,4)==0)
                {
                    if(output==1)my_strcpy(rcv_buf,"1",0);
                    else my_strcpy(rcv_buf,"0",0);
                    send_str(rcv_buf);                        
                }  
                //DIP MENU            
                if(my_strcmp(rcv_buf,"DIPE ",0,4)==0)
                {
                    dip_enable=str_num(rcv_buf, 5);
                    rep_time=0;
                    rep_n=0;
                    dip_phase_is=0;
                    dip_count_done=0;                     
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"DIPE?",0,4)==0)
                {
                    if( dip_enable==1)my_strcpy(rcv_buf,"1",0);
                    else my_strcpy(rcv_buf,"0",0);
                    send_str(rcv_buf);                        
                }                         
                //DIP level
                if(my_strcmp(rcv_buf,"DIPL ",0,4)==0)
                {
                    dip=(float)str_num(rcv_buf, 5)/100;
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"DIPL?",0,4)==0)
                {
                    my_ftoa(dip, rcv_buf); 
                    send_str(rcv_buf);                        
                }  
                //DIP count periods
                if(my_strcmp(rcv_buf,"DIPC ",0,4)==0)
                {
                    dip_count=str_num(rcv_buf, 5);
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"DIPC?",0,4)==0)
                { 
                    my_ftoa(dip_count, rcv_buf); 
                    send_str(rcv_buf);                        
                }
                //DIP phase set
                if(my_strcmp(rcv_buf,"DIPP ",0,4)==0)
                {
                    dip_phase_set=str_num(rcv_buf, 5);
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"DIPP?",0,4)==0)
                {
                    itoa(rcv_buf,dip_phase_set, 10); 
                    send_str(rcv_buf);                        
                }
                //DIP repeat cycles
                if(my_strcmp(rcv_buf,"REPN ",0,4)==0)
                {
                    rep_set_n=str_num(rcv_buf, 5);
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"REPN?",0,4)==0)
                {
                    itoa(rcv_buf,rep_set_n, 10); 
                    send_str(rcv_buf);                        
                }
                //DIP repeat time
                if(my_strcmp(rcv_buf,"REPT ",0,4)==0)
                {
                    dip_time_rep = str_num(rcv_buf, 5);
                    rep_set_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep; //zvladne 16,777,215 / 5min pri 60hz a 50 krocich = 1,800,000
                    rep_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep;
                    send_str(ok);
                }
                if(my_strcmp(rcv_buf,"REPT?",0,4)==0)
                {
                    itoa(rcv_buf,dip_time_rep,10); 
                    send_str(rcv_buf);                        
                }
                //Time done
                if(my_strcmp(rcv_buf,"REPD?",0,4)==0)
                {
                    itoa(rcv_buf,(rep_n*dip_time_rep+rep_time/((int)freq*2*SPWM_STEPS)),10); 
                    send_str(rcv_buf);                        
                } 
                //Actual repeat
                if(my_strcmp(rcv_buf,"REPA?",0,4)==0)
                {
                    itoa(rcv_buf,rep_n, 10); 
                    send_str(rcv_buf);                        
                }                 
                //SETTINGS MENU
                if(my_strcmp(rcv_buf,"PIDE ",0,4)==0)
                {
                    pid_en=str_num(rcv_buf, 5);
                    if(acdc==1)pid_reg=(float)(voltage*SQRT2)/VDD;
                    else pid_reg=(float)(voltage/VDD);
                    integ=PID_INTEG;   
                    send_str(ok);                    
                }
                if(my_strcmp(rcv_buf,"PIDE?",0,4)==0)
                {
                    if(pid_en)my_strcpy(rcv_buf,"1",0);
                    else my_strcpy(rcv_buf,"0",0);
                    send_str(rcv_buf);                          
                }                                  
            }
            recieved_flag = 0;
        }     
        // MENU
        if(menu_refresh_flag && !remote_ctr) 
        {
            if((tl1_flag)||(init_disp))
            {
                init_disp=0;
                menu_act++;
                set_act=0;
                if(menu_act > menu_max)menu_act=1;
                ClearScreen();
                tl1_flag=0;
            }        
            switch(menu_act)
            {
            case 0:        
            case 1:
                if(encoder_tl_flag==1)
                {
                    set_menu=2;
                    set_act++;
                    if(set_act>set_menu)set_act=0;          
                }
                //if(output)set_act=0;          tohle vypne nastavovani kdyz neni zaplej vystup

                SetPos(0,0);
                PutMessage("1/3 OUTPUT       ");   

                //frequency
                SetPos(0,16);
                PutMessage("Frequency:      ");        
                SetPos(70,16);
                write_num(freq,2,0);
                PutMessage("Hz ");
                if(set_act==1)
                {
                    PutChar('<');
                    if(encoder_tl_flag==1)encoder_counter=freq;
                    encoder_max=FREQ_MAX;
                    encoder_min=FREQ_MIN;
                    encoder_step=1;
                }
                PutMessage("   ");
                //voltage        
                SetPos(0,24);
                PutMessage("Voltage:        ");
                SetPos(70,24);
                write_num(voltage,3,0);
                PutMessage("V ");
                if(set_act==2)
                {
                    PutChar('<');
                    if(encoder_tl_flag==1)encoder_counter=voltage;
                    encoder_max=VOUT_MAX;
                    encoder_min=VOUT_MIN;
                    encoder_step=10;
                }
                PutMessage("    ");
                //read voltage
                SetPos(0,32);
                PutMessage("Vol. read:        ");
                SetPos(70,32);
                write_num(apply_Q(vol_show),3,0);
                //write_num(vout,3,0);
                PutMessage("V    ");                 
                //current
                SetPos(0,40);
                PutMessage("Current:        ");
                SetPos(70,40);
                write_num(iout,3,1);
                PutMessage("A   ");      
                //power
                SetPos(0,48);
                PutMessage("Power:          ");                
                SetPos(70,48);
                write_num((vout/SQRT2)*(iout/SQRT2),3,1);
                PutMessage("W   ");
/*              
                //acdc        
                SetPos(0,48);
                PutMessage("AC/DC:      ");
                SetPos(70,48);
                if(acdc)PutMessage("AC");
                else PutMessage("DC");
                PutMessage(" ");
                if(set_act==3)
                {
                    PutChar('<');
                    if(encoder_tl_flag==1)encoder_counter=acdc;
                    encoder_max=1;
                    encoder_min=0;
                    encoder_step=1;
                }
                PutMessage("   "); 

 */ 
                SetPos(0,56);
                if(iout_flag)PutMessage("Output current exceded!!");
                else         PutMessage("                               ");

                encoder_tl_flag=0;
                break;
            case 2:
                if(dip_enable==0)        
                {            
                    if(encoder_tl_flag==1)
                    {
                        set_menu=6;
                        set_act++;
                        if(set_act>set_menu)set_act=0;          
                    }
                    SetPos(0,0);
                    PutMessage("2/3 DIP             ");
                    //level
                    SetPos(0,16);
                    PutMessage("Level:          ");            
                    SetPos(70,16);
                    write_num(dip*100,3,0);
                    PutMessage("%      ");
                    if(set_act==1)
                    {
                        PutChar('<');
                        if(encoder_tl_flag==1)encoder_counter=dip;
                        encoder_max=1;
                        encoder_min=0.2;
                        encoder_step=0.1;
                    }
                    PutMessage("   ");   
                    //periods
                    SetPos(0,24);
                    PutMessage("Periods:        ");        
                    SetPos(70,24);
                    write_num(dip_count/2,5,1);
                    PutMessage(" ");
                    if(set_act==2)
                    {
                        PutChar('<');
                        if(encoder_tl_flag==1)encoder_counter=dip_count;
                        encoder_max=600;
                        encoder_min=1;
                        encoder_step=1;
                    }
                    PutMessage("   ");
                    //phase start
                    SetPos(0,32);
                    PutMessage("Phase start:    ");
                    SetPos(70,32);
                    write_num(dip_phase_set,3,0);
                    PutMessage("dg ");                
                    if(set_act==3)
                    {
                        PutChar('<');
                        if(encoder_tl_flag==1)encoder_counter=dip_phase_set;
                        encoder_max=315;
                        encoder_min=0;
                        encoder_step=45;
                    }
                    PutMessage("   ");
                    //repeat
                    SetPos(0,40);
                    PutMessage("Repeat:         ");                    
                    SetPos(70,40);
                    write_num(rep_set_n,2,0);
                    PutMessage(" ");     
                    if(set_act==4)
                    {
                        PutChar('<');
                        if(encoder_tl_flag==1)encoder_counter=rep_set_n;
                        encoder_max=10;
                        encoder_min=1;
                        encoder_step=1;
                    }
                    PutMessage("   ");
                    //repeat delay          
                    SetPos(0,48);
                    PutMessage("Repeat time:   ");                    
                    SetPos(70,48);
                    write_num(dip_time_rep,3,0);
                    PutMessage("s ");             
                    if(set_act==5)
                    {
                        PutChar('<');
                        if(encoder_tl_flag==1)encoder_counter=dip_time_rep;
                        encoder_max=3000;
                        encoder_min=dip_count/freq;//6;///dopocitavat
                        encoder_step=1;
                        encoder_flag=0;
                    }
                    PutMessage("   ");

                    SetPos(0,56);
                    if(output==1)PutMessage("                     RUN?   ");
                    else PutMessage("            OUTPUT IS OFF  ");

                    if((set_act==6)&&(output))
                    {        
                        SetPos(110,56);
                        PutChar('<');
                        if(encoder_flag==1)
                        { 
                            dip_enable=1;
                            rep_time=0;
                            rep_n=0;
                            dip_phase_is=0;
                            dip_count_done=0;              

                            init_disp=1;
                            menu_act=1;
                            encoder_flag=0;
                            set_act=0;
                        }
                    }                
                }
                else
                {
                    SetPos(0,0);
                    PutMessage("2/3 DIP             ");            
                    SetPos(0,16);
                    PutMessage("Time left:      ");                
                    SetPos(70,16);
                    write_num(rep_n*dip_time_rep+rep_time/((unsigned int)freq*2*SPWM_STEPS),3,0);
                    PutChar('\\');
                    write_num(dip_time_rep*rep_set_n,5,0);
                    PutMessage("s     ");
                    SetPos(0,24);
                    PutMessage("Repeat:         ");
                    SetPos(70,24);
                    write_num(rep_n+1,2,0); 
                    PutChar('\\');
                    write_num(rep_set_n,2,0);

                    SetPos(0,56);
                    PutMessage("          TEST RUNNING    ");                    
                }
                encoder_tl_flag=0;
                break;
            case 3:
                if(encoder_tl_flag==1)
                {
                    set_menu=1;
                    set_act++;
                    if(set_act>set_menu)set_act=0;          
                }        
                SetPos(0,0);
                PutMessage("3/3 SETTING         ");          
                
                SetPos(0,16);
                PutMessage("Volt. feedback:   ");
                SetPos(70,16);
                write_num(pid_en,1,0); 
                if(set_act==1)
                {
                    PutChar('<');
                    if(encoder_tl_flag==1)encoder_counter=pid_en;
                    encoder_max=1;
                    encoder_min=0;
                    encoder_step=1;
                }
                PutMessage("   ");        

                encoder_tl_flag=0;
                break;            
            }  

            //values change
            if(encoder_flag==1)
            {
                switch(menu_act)
                {
                case 1:

                    if(set_act==1)
                    {
                        freq=encoder_counter;
                        tmr1_reg = 65535-(FOSC/((float)freq*SPWM_STEPS*8))+TMR1_CORRECT;
                        rep_set_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep; //zvladne 16,777,215 / 5min pri 60hz a 50 krocich = 1,800,000
                        rep_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep;            
                    }
                    if(set_act==2)                
                    {
                        if(output==1)
                        {
                            tl2_flag=1;
                            CloseEPWM1();
                            LED_O=0;
                            LATCbits.LATC2=0;
                        }
                        voltage=encoder_counter;
                        GenLookup();
                    }
                    if(set_act==3)                
                    {
                        if(output==1)
                        {
                            tl2_flag=1;
                            CloseEPWM1();
                            LED_O=0;
                            LATCbits.LATC2=0;
                        }                        
                        acdc=encoder_counter;
                        GenLookup();
                    }

                    break;
                case 2:
                    if(set_act==1)
                    {
                        dip=encoder_counter;
                    }
                    if(set_act==2)
                    {
                        dip_count=encoder_counter;
                    }
                    if(set_act==3)
                    {
                        dip_phase_set=encoder_counter;
                    }
                    if(set_act==4)
                    {
                        rep_set_n=encoder_counter;
                    }
                    if(set_act==5)
                    {
                        dip_time_rep=encoder_counter;
                        rep_set_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep; //zvladne 16,777,215 / 5min pri 60hz a 50 krocich = 1,800,000
                        rep_time = (unsigned long)2*freq*SPWM_STEPS*dip_time_rep;                      
                    }


                    break;
                case 3:
                    if(set_act==1)
                    {
                        pid_en=encoder_counter;
                        if(acdc==1)pid_reg=(float)(voltage*SQRT2)/VDD;
                        else pid_reg=(float)(voltage/VDD);
                        integ=PID_INTEG;            
                    }        


                    break;            
                }      
                encoder_flag=0;
            }
          menu_refresh_flag=0;
        }
    ClrWdt();    
    }

    while (1);
    return;
}
//-----------------------------------------------------------------------------
 void InitializeClock(void)
//----------------------------------------------------------------------------- 
{
    // Adjust frequency of MPU
    #ifdef _USE_8MHZ_
    // 8MHz
    OSCCON = 0b01100000;
    OSCTUNEbits.PLLEN = 0;                     // turn off the PLL
    #elif defined _USE_16MHZ_
    // 16MHz
    OSCCON = 0b01110000;
    OSCTUNEbits.PLLEN = 0;                     // turn off the PLL
    #elif defined _USE_32MHZ_
    // 32 MHz
    OSCCON = 0b01100000;
    OSCTUNEbits.PLLEN = 1;                     // turn on the PLL 8*4
    #elif defined _USE_64MHZ_
    // 64 MHz
    OSCCON = 0b01110100;
    OSCCON2bits.MFIOFS=1;
    OSCCON2bits.LFIOFS=1;    
    OSCTUNEbits.PLLEN = 1;                     // turn on the PLL 16*4
    #endif
    return;
}
//-----------------------------------------------------------------------------
void InitPWM(void)
//-----------------------------------------------------------------------------
{
     // Init Timer2
     OpenTimer2( TIMER_INT_OFF &
                 T2_PS_1_4 &
                 T2_POST_1_1 );
     SetDCEPWM1(0);
     OpenEPWM1(FOSC/(FPWM*4*4)-1,ECCP_1_SEL_TMR12); 
     SetOutputEPWM1(SINGLE_OUT, PWM_MODE_2);
     return;
}
//-----------------------------------------------------------------------------
void InitTimers(void)
//-----------------------------------------------------------------------------
{
    //buttons
    OpenTimer0( TIMER_INT_ON &
        T0_16BIT &
        T0_SOURCE_INT &
        T0_PS_1_2 );
    INTCONbits.TMR0IE = 1;
    INTCON2bits.TMR0IP = 0;       // interrupt priority 0  
    WriteTimer0(tmr0_reg);
   
    //SPWM
    OpenTimer1( TIMER_INT_ON &
        T1_16BIT_RW &
        T1_SOURCE_FOSC_4 &
        T1_PS_1_1 &
        T1_OSC1EN_OFF,TIMER_GATE_OFF & 
        TIMER_GATE_INT_OFF );
    PIE1bits.TMR1IE = 1;
    IPR1bits.TMR1IP = 1;       // interrupt priority 1  
    WriteTimer1(tmr1_reg);  
    
    /*
    //relay PWM
    OpenTimer3( TIMER_INT_ON &
        T3_16BIT_RW &
        T0_SOURCE_INT &
        T3_PS_1_1 &
        T3_SOURCE_FOSC  &
        T3_SYNC_EXT_OFF);
    PIE2bits.TMR3IE = 1;
    IPR2bits.TMR3IP = 0;       // interrupt priority 0  
    WriteTimer3(tmr3_reg);
    */
    
    return;
}
//-----------------------------------------------------------------------------
void GenLookup(void)
//-----------------------------------------------------------------------------
{
    unsigned char i;

    //CloseEPWM1();
    //LATCbits.LATC2=0;
    if(acdc==1)
    {
        for(i=0;i<=SPWM_STEPS;i++)
        {
            SinTab[i]=max_duty*sin(DegRad*180*((float)i/(SPWM_STEPS)));
            SinTab[i]=(SinTab[i]*((voltage*SQRT2)/VDD));
            pid_reg=(float)(voltage*SQRT2)/VDD;
        }  
    }
    else
     {
        for(i=0;i<=SPWM_STEPS;i++)
        {
            SinTab[i]=max_duty*(voltage/VDD);
            pid_reg=(float)(voltage/VDD);
        }  
    }     
    integ=PID_INTEG;
    
    //InitPWM();
    
    return;
}
//-----------------------------------------------------------------------------
void SetDipStart(void)
//-----------------------------------------------------------------------------
{
    if(dip_phase_set > 180)
    {
        dip_phase_polarity=0;
        dip_phase=dip_phase_set-180;
    }
    else
    {
        dip_phase_polarity=1;
        dip_phase=dip_phase_set;
    }

    dip_phase=(float)((dip_phase*SPWM_STEPS)/180);

    return;
}
//-----------------------------------------------------------------------------
void PID(void)
//-----------------------------------------------------------------------------
{
    // KONSTANTY REGULATORU
    float error;
    float V_out_new;
    float dt = (float)1/(freq*2); // krok regulace       
    //static unsigned float integ = 0;
    const float Kp = KPROP;
    const float Ki = KINTEG;
    if((dip_count_done < dip_count) && (dip_phase_is==1)) error = voltage*dip - (float)(vout/(acdc==1?SQRT2:1)); // P
    else error = voltage - (float)(vout/(acdc==1?SQRT2:1)); // P
    integ = integ + error*dt; // S
    V_out_new = Kp*abs(error)+Ki*integ;
    if(pid_en) pid_reg = V_out_new/VDD; // hloubka modulace
    else
    { 
        pid_reg=1;
        Delay10TCYx(120);
    }
    if (pid_reg<=0)pid_reg=0.1; //zavedeni saturace
    else if (pid_reg*voltage>VDD/SQRT2)pid_reg=1; //zavedeni saturace
    return;
}

//-----------------------------------------------------------------------------
void InitADC(void)
//-----------------------------------------------------------------------------
{
    ADCON2 = 0b10010001; // right just, 4TAD, Fosc/8    
    VREFCON0 = 0b10100000;//internal reference 2.048V
    ADCON0bits.ADON=1;
    SetChanADC(ADC_CH1);
    Delay10TCYx(4);
    while(!VREFCON0bits.FVRST)
    Delay10TCYx(4);    
    ADCON1 = 0b00001000; //Set reference to FVR
    
    return;
}

//-----------------------------------------------------------------------------
int LoadADC(void)
//-----------------------------------------------------------------------------
{
    unsigned int in;
    //Delay10TCYx(2);
    ConvertADC();
    while (BusyADC());
    in = ReadADC();
    return in;
}
//-----------------------------------------------------------------------------
void InitUSART(void)
//-----------------------------------------------------------------------------
{

    //TRISCbits.TRISC6 = 0;    //TX set to output
    //TRISCbits.TRISC7 = 1;   //RX set to 1, but will be set to 0 by PIC for input(as specified in datasheet)

    //!! make zero result
    Open1USART(
        USART_TX_INT_OFF &
        USART_RX_INT_ON &
        USART_ASYNCH_MODE &
        USART_EIGHT_BIT &
        USART_CONT_RX &
        USART_BRGH_HIGH,
        ((FOSC/(UART_SPEED*16))-1)
    );
    //[20000000/(9600*16)] - 1 = 129
     IPR1bits.RCIP = 0;  /* Make receive interrupt low priority */
     
     return;
}
//-----------------------------------------------------------------------------
void send_str(unsigned char *string)
//-----------------------------------------------------------------------------
{
    unsigned char i=0;
    unsigned char crc=0;
    unsigned char crc_str[4];
    
    crc = calculate_crc(string);
    sprintf(crc_str,"%02x\r", crc & 0xff);     
            
    for(i=0;i<=strlen(string);i++)
    {
        while( Busy1USART() );
        Delay100TCYx(10);
        Write1USART(string[i]);
    }
    for(i=0;i<=strlen(crc_str);i++)
    {
        while( Busy1USART() );
        Delay100TCYx(10);      
        Write1USART(crc_str[i]);
    }    
    return;
}
  //-----------------------------------------------------------------------------
void GetVolCur(void)
//-----------------------------------------------------------------------------
 {
    int vol1;
    int vol2;
    int curr;
    
    SetChanADC(ADC_CH1);
    vol1 = LoadADC();
    SetChanADC(ADC_CH2);
    vol2 = LoadADC();
    
    SetChanADC(ADC_CH0);
    curr = LoadADC(); 
    
    if(vol1 > vol2) vout = (((float)(vol1-vol2)*VDD_ADC)/1024);//(acdc==1?SQRT2:1);
    else if(vol1 < vol2) vout = (((float)(vol2-vol1)*VDD_ADC)/1024);///(acdc==1?SQRT2:1);    
    else vout = 0;//vol1*VDD_ADC/1024;
    iout = ((float)(curr*2.048)/1024);///(acdc==1?SQRT2:1);   
    iout = 0.1; //disabled current sense
    
    vol_show -= apply_Q(vol_show);
    vol_show += vout/SQRT2;
    
    return;
 }
 
char my_strcmp(unsigned char *string1,unsigned char *string2,unsigned char begin,unsigned char end)
{
    unsigned int i=begin;
    char equal=0;
    for(i;i<=(end-begin);i++)
    {
        if(string1[i]!=string2[i])equal=1;        
    }
    if(begin>end)equal=-1;
    
    return equal;
}

char my_strcpy(unsigned char *string1,unsigned char *string2,unsigned char begin)
{
    unsigned int i=0;     
    
    for(i;i<strlen(string2);i++)
    {
        string1[i+begin]=string2[i];        
    }
    string1[i]=NULL;
            
    return 0;
}


unsigned char calculate_crc(unsigned char *string)
{
    unsigned char crc=0;
    unsigned char i=0;
   
    for(i;i<strlen(string);i++)
    {
        crc ^= string[i];
    }
    
    return crc;
}

unsigned char check_crc(unsigned char *string)
{
    unsigned char new_str[BUFF-3];
    unsigned char i=0;
    unsigned char first=0;
    unsigned char second=0;
    unsigned char crc_1=0;
    unsigned char crc_2=0;

    //crc1
    for(i;i<(strlen(string)-3);i++)
    {
        new_str[i] = string[i];
    }
    new_str[i] = NULL;
    crc_1 = calculate_crc(new_str);  

    //crc2
    first=string[strlen(string)-3];
    second=string[strlen(string)-2];
    if((first >= 48)&&(first <= 57))crc_2 += (first-48)*16;// is *hexString Between 0-9
    else if((first >= 65)&&(first <= 70))crc_2 += (first-55)*16;// is *hexString Between A-F
    else if((first >= 97)&&(first <= 102))crc_2 += (first-87)*16;// is *hexString Between a-f
    else crc_2 = 0;
    if((second >= 48)&&(second <= 57))crc_2 += (second-48);// is *hexString Between 0-9
    else if((second >= 65)&&(second <= 70))crc_2 += (second-55);// is *hexString Between A-F
    else if((second >= 97)&&(second <= 102))crc_2 += (second-87);// is *hexString Between a-f
    else crc_2 = 0;

    if(crc_1==crc_2)return 0;
    
    return 1;
}

