// Program: BallAndBeam.nxc
// Author:  Alex Alspach (alexalspach@gmai.com)
// Date:    March 2009
// Status:  Working but needs Tuning and redesign of the syste

#include "dist-nx-lib.nxc"

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

#define   Kp 0.15
#define   Kd 0.04

float DL_offset;
float DR_offset;

float DL;
float DR;
float X;
long curAngleInDegrees;

float DL_old;
float DR_old;

float errorX;

float errorX_old;

float DerX;

float angle;

float dT = 0.1;


float alpha = 0.5;

int calibNum = 30;
int inLoopAvg = 3;
int i;
int j;

task main() {
	SetSensorLowspeed(S1);
	SetSensorLowspeed(S2);
	
	PosRegEnable(OUT_A);
  	PosRegSetMax (OUT_A, 80, 50);

  	DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR);
  	DISTNxSendCommand(S2, DIST_CMD_ENERGIZED, DIST_ADDR);
  
  	DL_old =    DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR);
	DR_old =    DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR);
  
	int count = 0;
	
	
	for (i = 0; i < calibNum; i++)
 	{
        	DL_offset += DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR);
         	DR_offset += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR);
         	Wait(100);
         	TextOut(10, LCD_LINE3, "Calibrating...");
     	}
	
	DL_offset = DL_offset/calibNum;
	DR_offset = DR_offset/calibNum;
	
	DL_old =    DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DL_offset;
	DR_old =    DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DR_offset;

	
	while (1) {
	
		DL = 0;
	  	DR = 0;
	  
     	  	for (j = 0; j < inLoopAvg; j++)
     		{
         		DL += DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DL_offset;
         		DR += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DR_offset;
         	}
     
	   	DL = DL/inLoopAvg;
	   	DR = DR/inLoopAvg;
		X = (DR-DL);
    
		curAngleInDegrees = MotorRotationCount(OUT_A);

		ClearScreen();
		NumOut(0, LCD_LINE1, DL, false);
		NumOut(0, LCD_LINE2, DR, false);
		NumOut(0, LCD_LINE4, X, false);
		NumOut(0, LCD_LINE6, dT, false);
		NumOut(0, LCD_LINE7, DerX, false);
		NumOut(0, LCD_LINE8, curAngleInDegrees, false);
		
    		errorX = X;

    		DerX = (errorX-errorX_old)/dT;

		//PD control
		angle = (Kp*X) + (Kd*DerX);

  		// Limit the beam angle range
  		if (angle > 200)  angle = 200;
  		if (angle < -200) angle = -200;

    		PosRegSetAngle (OUT_A, angle);
		
	  	DR_old = DR;
    		DL_old = DL;
    
    		errorX_old = errorX;
    
    		count = count + 1;
		}
	
	DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR);
}