//---------- Rock steady calibrating programm, A.Pickel------------
//----------------last update  13.02.03-------------------
#include "RScali.h"

void CaliFeet()
{
	BYTE
	input_l = 0,   //input byte left  leg
	input_r = 0;   //input byte right leg

	int
	done = false,
	p_l,
	p_r;
			
	LCDClear();
	LCDPrintf("Calibrating legs");
	LCDPrintf("Lift Robot!");
	LCDPrintf("\nPress Play");
	LCDSetPos(7,11);
	LCDPrintf("Play");
	while ((KEYRead() != KEY4) && (IRTVPressed() != RC_PLAY));
	
	LCDClear();
	LCDPrintf("Calibrating...");
	LCDPrintf("\nPlease wait!");
	LCDSetPos(7,11);
	LCDPrintf("Stop");
	
	while (done == false)
	
	{	//calibrate an reset left leg
		while (input_l != 1)
		{	
			input_l = (OSReadInLatch(0) & 0x01);
			if ((OSReadInLatch(0) & 0x01) ==1)
				QUADReset(qH[0]);	
			p_l = QUADRead(qH[0]);
			LCDSetPos(2,0);
			LCDPrintf ("Leg_l: %4.0d\n",input_l);
			MOTORDrive(mH[0],18);
		}
		MOTORDrive(mH[0],0);

		//calibrate an reset right leg		
		while (input_r != 4)
		{
			input_r = (OSReadInLatch(0) & 0x04);
			if ((OSReadInLatch(0) & 0x04) ==4)
				QUADReset(qH[1]);	
			p_r = QUADRead(qH[1]);
			LCDSetPos(3,0);
			LCDPrintf ("Leg_r: %4.0d\n",input_r/4);
			MOTORDrive(mH[1], 16);
		}
		MOTORDrive(mH[1],0);
						
		done = true;	  	 //exit calibrating loop
			}
	//display values of quadencoders to check reset point	
	while ((KEYRead() != KEY4) && (IRTVPressed() != RC_STOP))
	{	p_l = QUADRead(qH[0]);
		p_r = QUADRead(qH[1]);
		LCDSetPos(2,10);
		LCDPrintf ("%4.0d\n", p_l);
		LCDSetPos(3,10);
		LCDPrintf ("%4.0d", p_r);
		
		LCDSetPos(7,11);  
		LCDPrintf("Stop");
	}
}

void CaliIncl()
{ int twice;
	
	LCDClear();
	LCDPrintf("Calibrating\ninclinometer\n");
	LCDPrintf("Adjust sensor\nuntil it shows \n0 deg");
	//LCDPrintf("\nThen press Stop!");
	LCDSetPos(7,11);
	LCDPrintf("Stop");
	
	while ((KEYRead() != KEY4) && (IRTVPressed() != RC_STOP))
	{	
		float angle_lr;
		//angle of the hip left right
		LCDSetPos(5,0);	
		twice = OSGetAD(2);   	
    	angle_lr = OSGetAD(2)-619;  	// middle of the sensor
		angle_lr = ( angle_lr *0.68);	        // 1 tick = 0.68°(??)
		LCDPrintf("%6.2f\n", angle_lr);
	}
}

int CaliWeight()
{	
	int
	done = false,
	p_w = -5,
	p_w_old = 0,
	s_w = 5,
	max = 0,
	center = 0;
	
	
	LCDClear();
	LCDPrintf("Calibrate weight");
	LCDPrintf("Press Start!");
	LCDSetPos(7,11);
	LCDPrintf("Start");
	while ((KEYRead() != KEY4) && (IRTVPressed() != RC_PLAY));
	
	// motor to left
	MOTORDrive(mH[2],-60);
	
	LCDClear();
	LCDPrintf("Calibrating \nweight");
	LCDPrintf("\nPlease wait!");
	LCDSetPos(7,11);
	
	while (done == false)
	{	
		// get inputs
		p_w = QUADRead(qH[2]);
		s_w = (p_w - p_w_old);
		
		// drive motor until speed = 0, left end
		if ( s_w == 0)
		{	LCDSetPos(4,8);
			LCDPrintf( "Reset");
			QUADReset(qH[2]);
			done = true;
		}
				
		OSWait(5);	
		LCDSetPos(4,0);
		LCDPrintf( "%5d",p_w);				
			
		p_w_old = p_w;
	}
	// motor = 0 and reset Quad
	MOTORDrive(mH[2],0);
	OSWait(5);
	QUADReset(qH[2]);
	
	done = false;
	// motor to right
	MOTORDrive(mH[2],60);
		
	while (done == false)
	{	// get inputs
		p_w = QUADRead(qH[2]);
		s_w = (p_w - p_w_old);
		
		// drive motor until speed = 0, right end
		if ( s_w == 0)
		{	LCDSetPos(4,8);
			max = QUADRead(qH[2]);
			LCDPrintf( "Maximum");
			done = true;		
		}
			
		OSWait(5);	
		LCDSetPos(4,0);
		LCDPrintf( "%5d",p_w);				
		
		p_w_old = p_w;
	}
	// motor 0
	MOTORDrive(mH[2],0);
	LCDSetPos(7,11);
	LCDPrintf("Stop");
	
	// drive weight to center
	MOTORDrive(mH[2],-40);
	OSWait(10);
	center = max /2;
	LCDSetPos(5,0);
	LCDPrintf( "%5d",center);	
	done = false;
	while ( done == false)
	{	
		if (QUADRead(qH[2]) > center +4500)
			MOTORDrive(mH[2],-12); 
		else
			MOTORDrive(mH[2],-5);
		p_w = QUADRead (qH[2]);
		LCDSetPos(6,0);
		LCDPrintf( "%5d",p_w);	
		
		// when center then stop 	
		
		if ( QUADRead(qH[2]) < center +200 )
		{	LCDSetPos(5,8);
			LCDPrintf( "Center");
			
			done = true;		
		}
	}
	// motor = 0
	MOTORDrive(mH[2],0);
	LCDSetPos(6,0);
	LCDPrintf( "%5d",QUADRead (qH[2]));
	OSWait(10);
	
	//reset at center !!
	QUADReset(qH[2]);	
	LCDSetPos(6,0);
	LCDPrintf( "%5d",QUADRead (qH[2]));
	return center;	
}