//----------  close loop control for Rock steady, A.Pickel------------
//----------------last update  13.02.03-------------------

#include "RScontrol.h"

void FeetControl () //control for feet movement 180°
{	
	// get inputs
	p_l = QUADRead(qH[0]);
	p_r = QUADRead(qH[1]);	
	
	//get speed
	// clicks / sec = speed
	s_l = ( p_l_old - p_l )*5 / SCALE;
	s_r = ( p_r_old - p_r )*5 / SCALE;
	
	//error position
	e_p_l = (p_l_old - p_r_old); 
	e_p_r = (p_r_old - p_l_old);
	
	// error speed and error position
	e_s_l =	w_s_l -	s_l - (e_p_r * e_pp); // clickspersec
	e_s_r =	w_s_r -	s_r - (e_p_l * e_pp); // clickspersec
	
				
	//calculate outputs
	o_l =  (o_l_old + Kp *( ( e_s_l - e_s_l_old) + ( c_1 * (e_s_l_old) )))/21;
	o_r =  (o_r_old + Kp *( ( e_s_r - e_s_r_old) + ( c_1 * (e_s_r_old) )))/21;
	//o_l = (o_l_old + Kp *( ( e_s_l  - e_s_l_old) + ( c_1 * (e_s_l + e_s_l_old) )))/21.25;
	//o_r = (o_r_old + Kp *( ( e_s_r  - e_s_r_old) + ( c_1 * (e_s_r + e_s_r_old) )))/21.25;
	
	//outputs to motor
	MOTORDrive(mH[0], - (Round ((o_l<100) ? ( (o_l>0) ? o_l: 1):100)));
	MOTORDrive(mH[1], - (Round ((o_r<100) ? ( (o_r>0) ? o_r: 1):100)));
			
	// vars to old
	p_l_old = p_l;
	p_r_old = p_r;
   
	o_l_old = Limit ((int)(o_l),100, 0);
	o_r_old = Limit ((int)(o_r),100, 0);
   	   		
	e_s_l_old = e_s_l;
	e_s_r_old = e_s_r;	
	
}
		
void HipBalance (int p_f_l, int p_f_r)  // control for hipbalance
{
		// get inputs
		p_hi_l = QUADRead(qH[0]);
		p_hi_r = QUADRead(qH[1]);
		
		// error of actual and desired position		
		p_hi_l = p_hi_l - p_f_l;
		p_hi_r = p_hi_r - p_f_r;
				
		//calculate output	
		outp_l = (outp_l_old - 4 * p_hi_l +((0.1/5) * p_hi_l_old )) /9;
		outp_r = (outp_r_old - 4 * p_hi_r+ ((0.1/5) * p_hi_r_old )) /9;
		
		// outputs to motor
		MOTORDrive(mH[0],(int) outp_l);
		MOTORDrive(mH[1],(int) outp_r);
		
		// vars to old
		outp_l_old = outp_l;
		outp_r_old = outp_r;
		p_hi_l_old =p_hi_l;			
		p_hi_r_old =p_hi_r;
}

void WeightMove(int pos, int max, int speed)
{
	// limit the movement to max and min
	if ((pos > max -100) ||(pos < -max +100 ))
		return;
			
	int
	done = false;
	
	// movement to right pos
	if ( pos > QUADRead(qH[2]))
	{
		MOTORDrive(mH[2],speed);	
		while ( done == false)
		{	
			MOTORDrive(mH[2], 40);
								
			if ( QUADRead(qH[2]) > pos -5 )
			{	MOTORDrive(mH[2],0);
				done = true;		
			}
		}
	}
	MOTORDrive(mH[2],0);
	
	//movement to left pos
	if ( pos < QUADRead(qH[2]))
	{
		MOTORDrive(mH[2],speed);	
		while ( done == false)
		{	
			MOTORDrive(mH[2], -40);
								
			if ( QUADRead(qH[2]) < pos +5 )
			{	MOTORDrive(mH[2],0);
				done = true;		
			}
		}
	}
	MOTORDrive(mH[2],0);	
}

void WeightControl (double angle_lr, int max)
{	
	// fuzzy rules for weightmovement
	
	if (angle_lr > 2.5 && angle_lr <3)
		WeightMove (QUADRead(qH[2]) - 150 , max, 50);
	if (angle_lr > 3 && angle_lr <5)
		WeightMove (QUADRead(qH[2]) - 500 , max, 70);
	if (angle_lr > 5 && angle_lr <7)
		WeightMove (QUADRead(qH[2]) - 1000, max, 80);
	if (angle_lr > 7)
		WeightMove (QUADRead(qH[2]) - 2000, max, 100);
	
	//if (angle_lr > -0.4 && angle_lr < 0.4)
	//	WeightMove (0, max, 30);
		
	if (angle_lr < -2.5 && angle_lr > -3)
		WeightMove (QUADRead(qH[2]) + 150 , max, 50);
	if (angle_lr < -3 && angle_lr > -5)
		WeightMove (QUADRead(qH[2]) + 500 , max, 70);		
	if (angle_lr < -5 && angle_lr > -7)
		WeightMove (QUADRead(qH[2]) + 1000, max, 80);
	if (angle_lr < -7)
		WeightMove (QUADRead(qH[2]) + 2000, max, 100);
}