#include "dist-nx-lib.nxc"

// Range Finder Device Address
#define   DIST_ADDR 0x02\
 // 0.5 1.5

#define P 96    // useless
#define I 75    // useless
#define D 39    // useless


byte fileHandle1;
short fileSize1;
short bytesWritten1;

byte fileHandle2;
short fileSize2;
short bytesWritten2;

byte fileHandle3;
short fileSize3;
short bytesWritten3;

byte fileHandle4;
short fileSize4;
short bytesWritten4;

byte fileHandle5;
short fileSize5;
short bytesWritten5;


float set_point=0.0;

float K1=459.1388;
float K2=594.0909; 
float K3=5.3849;
float K4=0.4687;


float X;
float curAngleInDegrees;
float curAngleInDegreesOld=0.0;

float CurPos;
float vel;

float thetam;

float X_old=0;

float DerX;
float integral=0;

long angle;

float r = 0.0;

float dT = 0.1;

float t0;
float time=0.0;
float time_old;

int ReadNum = 30;  // number of mesures for sensor calibration
int FiltNum = 3;  // number of mesures read by the sesor during regolation
int i;
int j;

task main() {


DeleteFile("Tempo.txt");
DeleteFile("Data.txt");
DeleteFile("LinVel.txt");
DeleteFile("Theta.txt");
//DeleteFile("AngVel.txt");
CreateFile("Tempo.txt", 1000, fileHandle1);
CreateFile("Data.txt", 1000, fileHandle2);
CreateFile("LinVel.txt", 1000, fileHandle3);
CreateFile("Theta.txt", 1000, fileHandle4);
//CreateFile("AngVel.txt", 5000, fileHandle5);

SetMotorRegulationTime (10);
PosRegEnable(OUT_A,P,I,D);
PosRegSetMax (OUT_A, 0, 0);

SetSensorLowspeed(S1); // empower the port 1 for sensor



DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR); // energize the sensor




for (i = 0; i < ReadNum; i++)  //cycle for calibration
 	{

         	set_point = set_point + DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR);
         	Wait(100);
         	TextOut(10, LCD_LINE3, "Calibrating...");
   }

   ClearScreen();


set_point = ((set_point/ReadNum)*0.001) + 0.2;   // give the set point


float dato = 0.0;
long theta = 0;
float AngVel = 0.0;
float LinVel = 0.0;
float xapCur =  0.0;
float xaCur = 0.0 ;
float u = 0.0;
char Pwr;



float    time_old = 0.0;
float    thetaOld = 0.0;
float    CurPosOld = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)*0.001 - set_point;
float    CurPos = 0.0;
float    xapOld = 0.0;


  t0 = CurrentTick();

	while (1) {    //cycle for regulation





     	  	for (j = 0; j < FiltNum; j++)
     		{

         		CurPos = CurPos + DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)*0.001 - set_point;
         	}


	   	CurPos = CurPos/FiltNum;  // current error




    X = CurPos ;    // current error X       //write on file time and error
    time = (CurrentTick()-t0) * 0.001;

    theta = MotorRotationCount(OUT_A);

    thetam = theta * 0.0175 ;



    AngVel = (thetam - thetaOld)/(time - time_old);

    LinVel = (CurPos - CurPosOld)/(time - time_old);

    u = -1 * (K1*CurPos + K2*LinVel + K3*thetam + K4*AngVel);


    ClearScreen();
    NumOut(0, LCD_LINE1, u, false);
    NumOut(0, LCD_LINE2, CurPos, false);
    NumOut(0, LCD_LINE3, thetam, false);
    
     PosRegSetAngle(OUT_A,u);

    Pwr =  MotorActualSpeed (OUT_A);

    string tmp1 = NumToStr(time);
    WriteLnString(fileHandle1,tmp1, bytesWritten1);
    string tmp2 = NumToStr(X);
    WriteLnString(fileHandle2,tmp2, bytesWritten2);
    string tmp3 = NumToStr(u);
    WriteLnString(fileHandle3,tmp3, bytesWritten3);
    string tmp4 = NumToStr(thetam);
    WriteLnString(fileHandle4,tmp4, bytesWritten4);
    string tmp5 = NumToStr(Pwr);
    WriteLnString(fileHandle5,tmp5, bytesWritten5);


    time_old = time;
    thetaOld = thetam;
    CurPosOld = CurPos;
    xapOld = xapCur;
    CurPos = 0.0;
    
    //Wait(20);
		}

	DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); // deenegize the sensor
}