// date 21.th august 2016 10:19
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/wdt.h>
#include <avr/io.h>

#include "uart.h" // interrupt driven


#define F_CPU 20000000
#include <util/delay.h>

void delayMs(uint16_t time){
  uint16_t i;
  for (i=1;i<=time;i++){
	_delay_ms(1);
	}
  }





// ADC channels
#define hallX1 0
#define hallX2 1
#define hallY1 2
#define hallY2 3
#define ADCkDpoti 4
#define ADCkPpoti 5


//----------------------------------------------------------------------------------------

void setupTimer0(){
  TCCR0A=0b00100011 ; // fast pwm mode 7, top=ocr0a oc0b=output pwm
  TCCR0B=0b00001001 ; // mode 7 prescaler 1
  OCR0A=80 ;
  OCR0B=40 ;
  }


#define PWMMAX (256 )

void setupTimer1(){
  TCCR1A=0b10100010 ; // fast pwm 
  TCCR1B=0b00011010	; // ck=XTAL/8
  OCR1A=1000 ; // OC1A pin15
  OCR1B=100 ; // OC1B pin 16
  ICR1=PWMMAX-1 ;
  }

// OC1A pin 15 PB1 with PD7 pin 13 magnet X
// OC1B pin 16 PB2 with PD6 pin 12 magnet Y

void setupTimer2(){
  TCCR2A=0b00000011 ; // fast pwm mode 7, top=ocr2a
  TCCR2B=0b00001111 ; // mode 7 prescaler 1024
  OCR2A=20-1 ;         // 20MHz/1024/20=976.56.. Hz
  TIMSK2=0b00000001 ; // overflow interrupt enable
  }

//-------------------------------------------------------------------------


void setCoilX(int v){
  if(v>0){
    if(v>PWMMAX ) { v=PWMMAX ; }
    PORTD &= ~_BV(7) ;
	OCR1A=v ;
	} 
   else {
    if(v<-PWMMAX ) { v=-PWMMAX ; }
    PORTD |= _BV(7) ;
	OCR1A=PWMMAX+v ;
	}
  }

void setCoilY(int v){
  if(v>0){
    if(v>PWMMAX ) { v=PWMMAX ; }
    PORTD &= ~_BV(6) ;
	OCR1B=v ;
	} 
   else {
    if(v<-PWMMAX ) { v=-PWMMAX ; }
    PORTD |= _BV(6) ;
	OCR1B=PWMMAX+v ;
	}
  }

//----------------------------------------------------------------------------------------

void ADCsetup(){
  ADCSRA=0b0111          ; // predivider 128 ADCclk=16000/128=125kHz Conversion time 13/125kHz=104us, 9.6kHz sample rate
  ADCSRA |= ( 1 << ADEN)  ; // enable
  ADMUX =0b01000001 ; // Vref=AVcc , input at ADC1 
  }

int32_t getADCx(uint8_t Channel){
  ADMUX &=0b11110000 ; 
  ADMUX |=Channel ;
  ADCSRA |= ( 1 << ADSC)  ;
    while ( (ADCSRA & (1 << ADIF ))==0 ){ } ;
  ADCSRA |= (1 << ADIF ) ;
  return ADC ;
  }

int32_t getADCmean(uint8_t Channel, int16_t N){
  int32_t mean=0 ;
  for (uint16_t k=0 ; k<N ; k++){
    mean+=getADCx(Channel) ;
	}
  return mean/N ;
  }




void adcShow(){
   while(1){
    int v=getADCmean(4,16) ; 
	uartPutsPgm(PSTR(" poti1(ADC4)=")) ;
	uartDecOut(v) ;
	v=getADCmean(5,16) ; 
	uartPutsPgm(PSTR(" poti2(ADC5)=")) ;
	uartDecOut(v) ;
    v=getADCmean(0,16)-512 ; uartPutsPgm(PSTR("    ADC0=")) ; uartDecOut(v) ; 
    v=getADCmean(1,16)-512 ; uartPutsPgm(PSTR("    ADC1=")) ; uartDecOut(v) ;
    v=getADCmean(2,16)-512 ; uartPutsPgm(PSTR("    ADC2=")) ; uartDecOut(v) ; 
    v=getADCmean(3,16)-512 ; uartPutsPgm(PSTR("    ADC3=")) ; uartDecOut(v) ;
	uartCrlf() ;
	delayMs(250) ;
	}
  }

//-------------------------------------------------------------------------
int32_t HallX ;
int32_t HallY ;
int32_t kDpoti ;
int32_t kPpoti ;

void getADCs(int nMean){ 
  HallX=(getADCmean(hallX2,nMean)-getADCmean(hallX1,nMean)) ;
  HallY=(getADCmean(hallY2,nMean)-getADCmean(hallY1,nMean)) ;
  kDpoti = getADCmean(ADCkDpoti,nMean)*512 ;
  kPpoti = getADCmean(ADCkPpoti,nMean)*64 ;
  }

int32_t Kp ; 
int32_t Kd ;
volatile int refValueX=0 ;
volatile int refValueY=0 ; 


int32_t errorX; // refValue - current X hall sensor reading
int32_t prevErrX=0; // previous error for calculating dirivative
int32_t dErrorX; //derivative of error in X

int32_t errorY; // refValue - current Y hall sensor reading
int32_t prevErrY=0; // previous error for calculating dirivative
int32_t dErrorY; // derivative of error in Y

 
int32_t ctrlX ;
int32_t ctrlY ;
  


int32_t kDpoti ;
int32_t kPpoti ;

#define Sign (-1)

void controller(){
  getADCs(1) ;
  Kd=kDpoti*Sign ;
  Kp=kPpoti*Sign ;

  errorX=refValueX-HallX;
  dErrorX=errorX-prevErrX;
  ctrlX=(errorX*Kp+dErrorX*Kd)/4096  ;
  setCoilX(ctrlX) ;
  prevErrX=errorX;

  errorY=refValueY-HallY;
  dErrorY=errorY-prevErrY;
  ctrlY=(errorY*Kp+dErrorY*Kd)/4096  ;
  setCoilY(ctrlY) ;
  prevErrY=errorY;
  }

int count=0 ;

volatile int printMail=0 ;
int printMailPeriod=200 ;

ISR(TIMER2_OVF_vect) {
  PORTD |=  _BV(2) ; // pin 4
  controller() ; 
  count++ ;
  if(count==printMailPeriod){ printMail=1 ; count=0 ; }
  PORTD &= ~_BV(2) ;	
  }

void ControlAndPrint(){
  printMailPeriod=200 ;
  while(1){
    while(  printMail==0 ){ }
	printMail=0 ;
    uartPutsPgm(PSTR("Kp="))  ; uartFracOut(kPpoti) ;
    uartPutsPgm(PSTR(" Kd=")) ; uartFracOut(kDpoti) ; 
    uartPutsPgm(PSTR(" x="))  ; uartDecOut3(errorX) ; 
    uartPutsPgm(PSTR(" ctrlX=")) ; uartDecOut3(ctrlX) ; 
    uartPutsPgm(PSTR(" y="))  ; uartDecOut3(errorY) ; 
    uartPutsPgm(PSTR(" ctrlY=")) ; uartDecOut3(ctrlY) ; 
	uartCrlf() ;
    }
  }

void ControlAndLogProcessing(){
  printMailPeriod=160 ;
  printMailPeriod=6 ;
  while(1){
    while(  printMail==0 ){ }
	printMail=0 ;
    //  processing display
    uartPutsPgm(PSTR("x=")) ; uartDecOut(errorX) ;
    uartPutsPgm(PSTR(" y=")) ; uartDecOut(errorY) ;
    uartPutsPgm(PSTR(" /")) ; 
	uartPutc(13) ;
	uartPutc(10) ;
    }
  }



int main(){

  DDRD =0b11111110 ; // enable outputs , bit 0=input
  PORTD=0b11111110 ; // pullups on on input pins
  
  DDRB =0b11111111 ; // enable outputs 
  PORTB=0b11111111 ; // pullups on on input pins
  
  DDRC =0b11000000 ; // enable outputs , bit 0,1,2,3,4,5=input
  PORTC=0b11000000 ; // pullups off on ADC pins
 

// 16.00MHz/19200/16=52
// 20MHz/16/115200=11
  UCSR0B |= ( 1 << TXEN0 ) | ( 1 << RXEN0 ) ;					// UART TX einschalten
  // UCSR0C |= ( 1 << URSEL )|( 3<<UCSZ0 );	    // Asynchron 8N1
  UBRR0H  = 0;                               // Highbyte ist 0
  UBRR0L  = 11-1 ;                             // 115200 Bit/sec
  

  setupTimer0() ;
 
  setupRS232(11-1 ) ;  
  onDirect() ; // non interrupt driven RS232

  uartCrlf() ; uartCrlf() ;
  uartPutsPgm(PSTR("ElektorRepMagLevMega88v01.c ")) ;
  uartCrlf() ;
  delayMs(100) ;
  ADCsetup() ;
  setupTimer1() ;
  sei() ;
  offDirect() ;
  if(0){ setupTimer2() ; while(1){} ;}
  if(0){ setupTimer2() ; ControlAndLogProcessing() ; }
  if(1){ setupTimer2() ; ControlAndPrint() ; }
  if(0){ adcShow() ; }
  while(1){} ;
  return 0 ;
  }