#include "eyebot.h"
#define SAFE 200

static int v_des1,v_des2;

float Kp=0.25;
float Ki=0.06;
float Klink=0.05;

TIMER t1;

void picontroller()
{
    int enc_new1,v_act1,r_mot1,e_func1;
    int enc_new2,v_act2,r_mot2,e_func2;
    static int enc_old1,enc_old2;
    static int r_old1=0,e_old1=0;
    static int r_old2=0,e_old2=0;
    
    enc_new1=ENCODERRead(1);
    enc_new2=ENCODERRead(2);
    
    v_act1=enc_new1-enc_old1;
    v_act2=enc_new2-enc_old2;
    
    e_func1=v_des1-v_act1;
    e_func2=v_des2-v_act2;
    

    r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2));
    r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2));
    
    if(v_des1>0&&v_des2>0)
    {
        if(r_mot1>35)r_mot1=35;
        else if(r_mot1<0) r_mot1=0;
    
        if(r_mot2>35)r_mot2=35;
        else if(r_mot2<0) r_mot2=0;
    }
    else if(v_des1>0&&v_des2<0)
    {
        if(r_mot1>35)r_mot1=35;
        else if(r_mot1<0) r_mot1=0;
        if(r_mot2>0)r_mot2=0;
        else if(r_mot2<-35) r_mot2=-35;
    }
    else if(v_des1<0&&v_des2>0)
    {
        if(r_mot1>0)r_mot1=0;
        else if(r_mot1<-35) r_mot1=-35;
        if(r_mot2>35)r_mot2=35;
        else if(r_mot2<0) r_mot2=0;
    }
    else if(v_des1==0&&v_des2==0)
    {
        r_mot1=0;
        r_mot2=0;
        v_act1=0;
        v_act2=0;
        enc_old1=0;
        r_old1=0;
        enc_old2=0;
        r_old2=0;
        e_func1=0;
        e_func2=0;
        ENCODERReset(1);
        ENCODERReset(2);
    }
    
    MOTORDrive(1,r_mot1);
    MOTORDrive(2,r_mot2);    
    
    enc_old1=enc_new1;
    r_old1=r_mot1;
    e_old1=e_func1;
    
    enc_old2=enc_new2;
    r_old2=r_mot2;
    e_old2=e_func2;
    
    printf("%d    %d    %d     %d\n",r_mot1,r_mot2,enc_old1,enc_old2);
    
}

void VWSpeed(int v1,int v2)
{
    v_des1=v1;
    v_des2=v2;
}


void drive()
{
   do
   {
        if(PSDGetRaw(PSD_LEFT)<SAFE)
            VWSpeed(100,90);
        else VWSpeed(90,100);
        
        OSWait(80);
        
    }while(PSDGetRaw(PSD_FRONT)>SAFE);
    VWSpeed(0,0);
    OSWait(1000);
}

void turn()
{
    VWSpeed(50,-50);
    while(PSDGetRaw(PSD_FRONT)<400) OSWait(80);
    VWSpeed(0,0);
    OSWait(1000);
}

int main() 
{
    LCDMenu("STOP","2","3","END");
    ENCODERReset(1);
    ENCODERReset(2);
    
    PSDGetRaw(PSD_FRONT);
    PSDGetRaw(PSD_LEFT);
    
    
    t1=OSAttachTimer(100,picontroller);

    while(KEYRead()!=KEY1)
    { drive();
      turn();
    }
    
    OSDetachTimer(t1);
    
    AUBeep();

    while(KEYRead()!=KEY4)
    {
    MOTORDrive(1,0);
    MOTORDrive(2,0);
    }

    return(0);
 }