/*
 * File:   LCD_Display.c
 * Author: Sascha
 *
 * Created on 25. Februar 2015, 15:11
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <xc.h>
#include "config.h"

//Variable definieren
unsigned char SDatLCD; //8 Bit Daten für Schreiboperation
unsigned char SDat4Bit; //4 Bit Daten für Schreiboperation
unsigned char LDatLCD; //8 Bit Daten für Leseoperation
unsigned char BOD; //0 bedeutet Befehl, 1 Daten


#define KP 1 //1
#define KD 5// 3
//#define Ta 0.05 // 125n
#define KI 0
#define phix0 180// 200
#define phiy0 180// 200
#define xsoll 80
#define ysoll 60

void initRS232() //Serielle Datenübertragung
{

    TXSTA = 0b00100000;
    RCSTA = 0b10010000;
    SPBRG = 51;
    SPEN = 1;

}

int LCDWrite4Bit() {
    extern unsigned char BOD, SDat4Bit;
    //Schreibt die unteren 4 Bit der Variablen SDat4Bit
    //auf das Display hinaus, die Variable BOD
    //steuert den Pin RS
    extern unsigned char BOD, SDat4Bit;
    //Ports initialisieren
    RA4 = 0; //DB4
    RA5 = 0; //DB5
    RA6 = 0; //DB6
    RA7 = 0; //DB7
    RC3 = 0; //RS *****************+
    asm ("nop");
    if (BOD != 0) RC3 = 1; //RS, Daten setzen
    asm ("nop");
    RC4 = 0; //RW, Write aktiv *******************+
    asm ("nop");
    asm ("nop"); //kurze Wartezeit für Ports
    //Ausgeben der 4 Bit Daten
    RC5 = 1; //Enable gesetzt *************** ??????????????????????
    if ((SDat4Bit & 0x08) != 0) RA7 = 1; //DB7
    if ((SDat4Bit & 0x04) != 0) RA6 = 1; //DB6
    if ((SDat4Bit & 0x02) != 0) RA5 = 1; //DB5
    if ((SDat4Bit & 0x01) != 0) RA4 = 1; //DB4
    asm ("nop"); //kurze Verzögerung
    asm ("nop");
    //Display Übernimmt mit fallender Flanke von E
    RC5 = 0;
}

int LCDWriteByte(void) {
    //Schreibt ein Byte Daten auf das Display, zuerst die
    //oberen 4 Bit und dann die unteren.
    extern unsigned char SDatLCD, SDat4Bit, BOD;
    unsigned char Oben, Unten;
    Unten = SDatLCD & 0x0F; //untere 4 Bit
    Oben = SDatLCD / 16; //obere 4 Bit
    SDat4Bit = Oben;
    LCDWrite4Bit(); //schreibe die oberen 4 Bit

    asm ("nop"); //kurze Verzögerung
    asm ("nop");
    asm ("nop");
    SDat4Bit = Unten;
    LCDWrite4Bit(); //schreibe die unteren 4 Bit
    //Warten
    asm ("nop"); //kurze Verzögerung
    asm ("nop");
    asm ("nop");
}

void main() {
    extern unsigned char SDatLCD, SDat4Bit, LDatLCD, BOD;
    unsigned char i;
    unsigned char posx, posy;
    unsigned char zeile1[16], zeile2[16];

    unsigned char x, y; 
    float ex, ey, exsum=0, eysum=0;//    <=-
    int phix, phiy;
    float exalt = 0;
    float eyalt = 0;

    //Ports D4-D7 auf Ausgang setzen
    ANSELA = ANSELB = 0;

    TRISA4 = 0; //DB4
    TRISA5 = 0; //DB5
    TRISA6 = 0; //DB6
    TRISA7 = 0; //DB7
    TRISC3 = 0; //RS
    TRISC4 = 0; //R/W
    TRISC5 = 0; //E
    TRISB2 = 0; // Backlight

    OSCCON = 0b01110000;

    PR2 = 255;
    T2CON = 0b00000110;

    CCP1CON = 0b00001111;
    CCP2CON = 0b00001111;

    TRISC1 = 0; //PWM output
    TRISC2 = 0; //PWM output

    //Init LCD

    __delay_ms(41);
    BOD = 0; //Befehle
    SDat4Bit = 0x03; //#1 Control Sequence
    LCDWrite4Bit();
    //Wartezeit mindestens 4.1 ms
    __delay_ms(5);
    BOD = 0; //Befehle
    SDat4Bit = 0x03; //#2 Control Sequence
    LCDWrite4Bit();
    //Wartezeit mindestens 100 us
    __delay_us(100);
    BOD = 0; //Befehle
    SDat4Bit = 0x03; //#3 Control Sequence
    LCDWrite4Bit();
    //Wartezeit mindestens 100 us
    __delay_us(100);
    BOD = 0; //Befehle
    SDat4Bit = 0x02; //#4 set 4 Bit
    LCDWrite4Bit();
    __delay_us(100);
    BOD = 0;
    SDatLCD = 0x28; //#5 Function Set
    LCDWriteByte();
    BOD = 0;
    SDatLCD = 0x0D; //#6 Display = ON
    LCDWriteByte();
    BOD = 0;
    SDatLCD = 0x01; //#7 Display Clear
    LCDWriteByte();
    BOD = 0;
    SDatLCD = 0x06; //#8 Entry Mode
    LCDWriteByte();
    BOD = 0;
    SDatLCD = 0x80; //#9 DDRAM address 00
    LCDWriteByte();

    LATB2 = 1; // Backlight on

    BOD = 0;
    SDatLCD = 0x80; //DDRAM address 00
    LCDWriteByte(); //1. Zeichen 1. Zeile
    BOD = 1;

    sprintf(zeile1, "SX=%3d          ", ex);
    for (i = 0; i < 16; i++) {
        SDatLCD = zeile1[i];
        LCDWriteByte();
    };


    BOD = 0;
    SDatLCD = 0xC0;
    LCDWriteByte(); //1. Zeichen 2. Zeile
    BOD = 1;

    sprintf(zeile2, "SY=%3d          ", ey);
    for (i = 0; i < 16; i++) {
        SDatLCD = zeile2[i];
        LCDWriteByte();
    };

    initRS232();

    while (1) {

        while (RCIF == 0);
        if (RCREG == 204) {

            while (RCIF == 0);
            posx = RCREG;

            // Rgegldifferenz Bereich -80 .. +80
            //
            ex =  (xsoll - posx);

            if (abs(ex)>40)

            {
            //    ex =ex/4;

            }
            phix = phix0 - ( ex*KP + KD * (ex - exalt));// + exsum/1024);
            //   printf(phix);
            exalt = ex;

            if (phix < 125) phix = 125;
            else if (phix > 250) phix = 250;
            else
            {
                exsum = exsum + ex;
            }

            CCPR1L = phix;


            while (RCIF == 0);
            posy = RCREG;

            ey =  (ysoll - posy);

               if (abs(ey)>40)

            {
            //    ey =ey/4;

            }


            phiy = phiy0 - ( ey*KP + KD * (ey - eyalt) );//+ eysum/1024);
            
            eyalt = ey;

            if (phiy < 125) phiy = 125;
            else if (phiy > 250) phiy = 250;
            else
            {
                eysum = eysum + ey;
            }

            CCPR2L = phiy;


            BOD = 0;
            SDatLCD = 0x83; //DDRAM address 00
            LCDWriteByte(); //1. Zeichen 1. Zeile
            BOD = 1;

            sprintf(zeile1, "%3d", phix);
            for (i = 0; i < 6; i++) {
                SDatLCD = zeile1[i];
                LCDWriteByte();
            };



            BOD = 0;
            SDatLCD = 0xC3;
            LCDWriteByte(); //1. Zeichen 2. Zeile
            BOD = 1;

            sprintf(zeile2, "%3d", phiy);
            for (i = 0; i < 6; i++) {
                SDatLCD = zeile2[i];
                LCDWriteByte();
            };


        }
    }

}