//by MaPTbIH-inc
//Подбор коэффициентов для ПИД-регулятора
#include "defines.h"

#include <ctype.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include <avr/interrupt.h>

#include <util/delay.h>

#include "uart.c"

#define PAUSEMOTORCHANGE 25

FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);

uint16_t baseA;
uint16_t baseB;

int PowerBase (int Power)
{
int tmp=0;

if (Power<0)
 {
 tmp=260+Power*0.9;
 }
if (Power==0)
 {
 tmp=0;
 }
if (Power>0)
 {
 tmp=260+Power*0.9;
 }
return tmp;

}

void Turn(int DegreeOfTurn, int Power)
{//DegreeOfTurn==0 Прям
 // -100 -> налево вокруг себя, -50 -> налево
 // +100 -> направо вокруг себя, +50 -> направо
 int A,B;
 int Kslow=100/Power;
 B=Power;
 A=0.9*Power;
if (DegreeOfTurn==0) {baseA=PowerBase(A);baseB=PowerBase(B);}
if (DegreeOfTurn<0 && DegreeOfTurn>=-50) {baseA=PowerBase( (DegreeOfTurn+50)*1.8/Kslow );baseB=PowerBase(B);}
if (DegreeOfTurn<-50 && DegreeOfTurn>=-100) {baseA=PowerBase( (DegreeOfTurn+50)*1.8/Kslow );baseB=PowerBase(B);}
if (DegreeOfTurn<50 && DegreeOfTurn>0) {baseA=PowerBase(A);baseB=PowerBase( (DegreeOfTurn-50)*(-2)/Kslow );}
if (DegreeOfTurn<=100 && DegreeOfTurn>=50) {baseA=PowerBase(A);baseB=PowerBase( (DegreeOfTurn-50)*(-2)/Kslow );}
//printf("Turn=%d PowerA=%d PowerB=%d\n",DegreeOfTurn,baseA,baseB);
_delay_ms(PAUSEMOTORCHANGE);
OCR1A=baseA;
OCR1B=baseB;
}

void Stop()
{
baseA=PowerBase(0);
baseB=PowerBase(0);
_delay_ms(PAUSEMOTORCHANGE);
OCR1A=baseA;
OCR1B=baseB;
}

int main(void)
{
  uart_init();
  stdout = stdin = &uart_str;
  
  int mid=4,Error=0,Derivative=0,Integral=0,PrevError=0,Timer=0;
  float Ki=0.3;
  int Kp=15,Kd=35;
  
  printf("Linetracer ready 1!\n");

  DDRC=0x20;
  DDRD=0x30;
  
  Stop();

  _delay_ms(5000);

  //PWM, Phase and frequency correct, TOP=ICR1
  TCCR1A=0xA0; //clear on up-counting, set on down-counting
  TCCR1B=0x12; //Clock prescaler 1/8
  ICR1=9216;  //Set 50Hz frequency	
  //OCR1A=baseA;
  //OCR1B=baseB;
  Stop();
  
  while(1==1){
    DDRA=0xFF;
	PORTA=0xFF;
	DDRC|=0xC0;
	PORTC|=0xC0;
	_delay_us(1000);
	DDRA=0x00;
	PORTA=0x00;
	DDRC&=~0xC0;
	PORTC&=~0xC0;
	PORTC|=0x20;
	_delay_us(500);
	int x=PINA;
	int y=PINC>>6;
	if(y==1){ y=2; }else if (y==2){y=1;};
	x=x+(y<<8);
	int pos1=-1;
	int pos2=10;

	for(int i=0; i<10; i++){
	   int j=x&0x01;
	   if(j==1 && pos1==-1){ pos1=i; };
	   if(j==1){ pos2=i; };
	   //printf("%d",j);
	   x=x>>1;
	};
	//printf("\n");
	
	
	int pos=(pos2+pos1)/2;
	
	//int mid=4,Error=0,Integral=0,Derivative=0,PrevError=0;
    //int Kp=9,Ki=0,Kd=0;
	
	/*switch (pos)
	{
	case 9: Turn(-50,50); break;
	case 8: Turn(-38,50); break;
	case 7: Turn(-25,50); break;
	case 6: Turn(-13,50); break;
	case 5: Turn(0,50); break;
	case 4: Turn(0,50); break;
	case 3: Turn(13,50); break;
	case 2: Turn(25,50); break;
	case 1: Turn(38,50); break;
	case 0: Turn(50,50); break;
	default: break;
	}*/
	
	//PID-controller
	Error=mid-pos;
	Integral+=Error;
	Derivative=Error-PrevError;
	int DOT=Kp*Error+Ki*Integral+Kd*Derivative;//DegreeOfTurn
	PrevError=Error;
	
	if (DOT<-50) DOT=-50;
	if (DOT>50) DOT=50;
	
	Turn(DOT,100);
	_delay_us(5000);
	
	//if (Timer>=1000) 
	//	{Stop();
	//	Ki+=0.1;
		//if (Kp>20) {Kp=10;Kd++;_delay_ms(5000);}
	//	if (Ki>1) {while (1==1) {} }
		
	//	_delay_ms(3000); Timer=0;}
	
	//Timer++;
	
	
	
  	

  };
}
