//---------- Rock steady head and feet movement, A.Pickel------------
//----------------last update  5.02.03-------------------
#include "RSglobal.h"
#include "RScali.h"
#include "RSmath.h"
#include "RSinit.h"
#include "RSprint.h"
#include "RScontrol.h"
#include "RSglobal.h"
	
int main ()
{	
	int
	m_pos =0 ,
	m_pos_old = 0,
	prog = 0,
	m_max = 0,		// center of the weight
	p_f_l  = 0,		// position foot left
	p_f_r  = 0;		// position foot right
		
	double
	angle_lr = 0,		// angle left right
	angle_old_lr = 0;	// old angle left right
				
	// init remote control
	IRTVInit(SPACE_CODE,15,0,0x03ff,DEFAULT_MODE,5,15);
			
	Reset();   	// reset varis
	QuIni();	// init Quaddecoder		
	MoIni();	// init Motors
	
	m_max = CaliWeight(); // Calibrate Weight and return max-movement
	while ((KEYRead() != KEY4) && (IRTVPressed() != RC_PLAY));
	
	CaliFeet();	// Calibrate feet
	
	CaliIncl();	// Calibrate Inclinometer
	
	//start 
	LCDClear();
	LCDMenu( "bal","feet"," ", "exit");

	LCDDeg(1,83,1);	//display degrees
	
	LCDArea( 21, 8, 105, 12, 1); //display upper bar
	LCDArea( 22, 7, 104, 11, 0);
	
	LCDArea( 21, 18, 105, 22, 1);//display lower bar
	LCDArea( 22, 17, 104, 21, 0);
	
	LCDSetPos(2,1); //label bars
	LCDPrintf("R");
	LCDSetPos(2,14);
	LCDPrintf("L");
	
	LCDSetPos(3,3);
	LCDPrintf("Weight Pos");	
	
	
	while ((prog !=KEY4) && (IRTVPressed() != RC_STOP))
	{	
		int key = KEYRead(); // if key pressed exe following lines 
		if (key>0)
		{	p_f_l = QUADRead(qH[0]); //get actual inputs
			p_f_r = QUADRead(qH[1]);			
			prog = key;
		}
				
		//Calculate angle
		angle_lr = Sen2Angle(angle_lr);
						
		//delete old cross
		LCDCross(10 , (int)((angle_old_lr*2.5 + 63)) , 2);
	
		// draw  new cross
		LCDCross(10 , (int)((angle_lr*2.5 + 63)  ) , 1);
		
		//delete old weight pos
		LCDDeg (20,-m_pos_old +83, 0);
		
		//draw weight pos
		m_pos = (20+(QUADRead(qH[2])/215));
		LCDDeg (20,-m_pos + 83, 1);
		//LCDDeg (20,m_pos + 63, 1);
						
		//old  values
		angle_old_lr = angle_lr;
		m_pos_old = m_pos;	
		
		if (prog == KEY1) // if key1 pressed 
		{	//start weight control
			WeightControl(angle_lr, m_max);		
			//start balance control
			HipBalance(p_f_l, p_f_r);
		}
		
		if (prog == KEY2) //if key2 pressed
		{	//start feet control
			FeetControl();	
		}
			
	}
	MOQuRelease();
	IRTVTerm();
}