//----------  global varis for Rock steady, A.Pickel------------
//----------------last update  13.02.03------------------ 

#include "eyebot.h"
#include "fastmath.h"
#include "IRnokia.h"

#define SCALE 1
#define LEG_LEFT  MOTOR_FR
#define MOTOR_LR  MOTOR_BL
#define LEG_RIGHT MOTOR_FL

#define QUAD_LE QUAD_FR
#define QUAD_LR QUAD_BL
#define QUAD_RI QUAD_FL

QuadHandle	qH[3];
MotorHandle	mH[3];

double 	outp_l = 0,
		outp_r = 0,
		outp_l_old = 0,
		outp_r_old = 0,
		p_hi_l = 0,		// position hip left                              
		p_hi_r = 0,		// position hip right
		p_hi_l_old = 0,		// old position hip left                          ,
		p_hi_r_old = 0;		// old position hip right

double			
	//----- Close loop control varis-----------
	o_r,	        // output right
	o_l,		// output left
	
	o_r_old,	// output right old
   	o_l_old,	// output left old

	s_l,		// speed left
	s_r,		// speed right

	e_s_l,		// speed error left
	e_s_r,		// speed error right

	e_s_l_old,	// speed error left old
	e_s_r_old,	// speed error right old

	p_l,		// position left
	p_r,		// position right

	p_l_old,	// position left old	
   	p_r_old,	// position right old

	w_s_l,		// desired speed left
	w_s_r,		// desired speed right

	w_p_l,		// desired position left
	w_p_r,		// desired position right

	Kp,		// controller p portion
	c_1,		// 1 + ( T / Tn )
	T,		// time period
	Ti,		// integral time

	e_pp,		// error position gain
	e_p_l, 		//error position
	e_p_r;		//error position right

void Reset()
{	// "reset" global vars
	p_l =0 ;
	p_r = 0;

	p_l_old = 0;
   	p_r_old = 0;
	
	o_l = 0;
	o_r = 0;

	o_l_old = 0;
   	o_r_old = 0;

	s_l = 0;
	s_r = 0;

	e_s_l = 0;
	e_s_r = 0;

	e_s_l_old = 0;
	e_s_r_old = 0;

	w_p_l = 0;
	w_p_r = 0;

	w_s_l = 0;
	w_s_r = 0;

	e_p_l=0;
	e_p_r=0;
}


void init()
{	//init varis
	w_s_l = 30;		// desired left speed
	w_s_r = 30;		// desired right speed
	e_pp = 6;		// position error gain
	Ti = 0.2; 		// integration time sec
   	Kp = 12;		// Kp ( PWM / clickspersec) amplification factor
	T = 1.0 / 10* SCALE;  // cycle duration (perioden dauer) T base of the calcualtions for PI control
	c_1 = ( T / (Ti));      // parameter for control algorithm	
}