#include "dist-nx-lib.nxc"

// Range Finder Device Address
#define   DIST_ADDR 0x02\
 // 0.5 1.5
#define   Kp 0.92  // non superare 1.1   (0.9)  -92
#define   Kd 0.40  // 0.40
#define   Ki 0



byte fileHandle1;
short fileSize1;
short bytesWritten1;


byte fileHandle2;
short fileSize2;
short bytesWritten2;



float set_point=0;



float X;
long curAngleInDegrees;


float Error;



float X_old=0;

float DerX;
float integral=0;

float angle;

float dT = 0.1;

int t0;
int time;


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");
CreateFile("Tempo.txt", 10000, fileHandle1);
CreateFile("Data.txt", 10000, fileHandle2);


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

PosRegEnable(OUT_A);   // empower the port A for motor

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









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

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


	set_point = set_point/ReadNum;   // give the set point


	Error =    DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)- set_point;   // give the error DR_old = current position - set point








  t0 = CurrentTick();

	while (1) {    //cycle for regulation



	  	Error = 0;

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

         		Error += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)- set_point;
         	}


	   	Error = Error/FiltNum;  // current error

	   	


    X = Error ;    // current error X       //write on file time and error
    time = CurrentTick()-t0;
    string tmp1 = NumToStr(time);
    WriteLnString(fileHandle1,tmp1, bytesWritten1);
    string tmp2 = NumToStr(X);
    WriteLnString(fileHandle2,tmp2, bytesWritten2);
    
    
    
    
    



    // write on nxt display
    curAngleInDegrees = MotorRotationCount(OUT_A);  // give the current angle of the motor in degrees (encoder)
    
    
    ClearScreen();
		/*NumOut(0, LCD_LINE1, DR, false);*/
		/*NumOut(0, LCD_LINE2, DR, false); */
		NumOut(0, LCD_LINE4,Kp*X, false);
		/*NumOut(0, LCD_LINE6, dT, false);*/
		NumOut(0, LCD_LINE7, Kd*DerX, false);
		NumOut(0, LCD_LINE8, curAngleInDegrees, false);




    //PD control

    DerX = (X - X_old)/dT;     // derivative error
    integral = integral + 1/2 *(X + X_old)*dT;   // integral error (useless)

    
		angle = (Kp*X) + (Kd*DerX) + (Ki*integral); // command for motor after PD regulation
    PosRegSetAngle (OUT_A, -angle);  // command for the motor



    X_old = X; //save current error

		}

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