/* ----------------------------------------------------------------- */

/* 'ZControlClass.h'									    */

/* Header for ZControlClass.cc			 			         */

/* 													    */

/* 	last modified 07/04/2003								    */

/* 	PID- Controller for Balancing							    */

/* 													    */

/* 													    */

/* 													    */

/* 													    */

/* 	(C) Jochen Zimmermann								    */

/* ----------------------------------------------------------------- */



//avoid redefinition during static binding

#ifndef ZCONTROL

#define ZCONTROL



#include "ZHeaders.h"



#define OwnPI 3.14159265358979



class Control

{

    public:

	

	static Control* GetTheInstance();

	/*instance retriever*/

	

	void Init();

	/*initialises members and parameters*/

	

	LinearAlgebra CalcMeasCOG(LinearAlgebra lfoot,LinearAlgebra rfoot);

	/*calculates NPCM out of sensory data*/

	

	LinearAlgebra GetMeasCOG();

	/*returns measured NPCM*/

	

	LinearAlgebra CalcVirtCOG(bool project_to_footlevel=true);

	/*calculates NPCM virually out of assumed robot-state*/

	

	LinearAlgebra GetVirtCOG();

	/*returns Virtual NPCM*/

	

	void	CalcSlope(int* roll,int* pitch);

	/*claculates slope out of difference of both NPCMs*/

	

	void	GetSlope (int* roll,int* pitch);

	/*returns calculated slope angles*/

	

	int OwnArcSin(double val);

	/*calculates arcus sine with look-up-table*/

	

	float OwnSin (int  angle);

	/*calculates sine with look-up table*/

	

	LinearAlgebra GetLeftCOG ();

	/*returns centre of force on left foot, passed through from feet-class*/

	

	LinearAlgebra GetRightCOG();

	/*returns centre of force on right foot, passed through from feet-class*/

	

	void InterruptOn();

	/*starts control loop in interrupt, desired value has to be assigned*/

	

	void InterruptOn(LinearAlgebra hleft,LinearAlgebra hright,float percent_on_right_foot=0.5);

	/*starts control loop in interrupt, with passed desired value*/

	

	void InterruptOff();

	/*end control loop in interrupt*/

	

	static void InterruptBalance();

	/*kicker, needed to simulate c++-member function as non-classed c-function*/

	

	void Balance();

	/*open loop control function,that is closed by running in interrupt*/

	

	void SetDesiredValues(LinearAlgebra hleft,LinearAlgebra hright,float percent_on_right_foot);

	/*sets desired values for control loop*/

	

		

    private:

	

	LinearAlgebra 	measuredcog,	//internal buffer for measured NPCM

				virtualcog,	//internal buffer for calculated virtual NPCM

				lcog,		//internal buffer for left  foot's centre of force

				rcog;		//internal buffer for right foot's centre of force

	

	LinearAlgebra 	hold_left,	//assumed left  foot's position for desired value

				hold_right;	//assumed right foot's position for desired value

	

	float 		load;		//internal buffer for total load retrievable from feet-class

	

	double 	roll,			//manipulated value 		for  rolling direction

			pitch,			//     "        "   		 "  pitching     "   

			deltay,			//actual 		control error 	 "  pitching     "   

			deltaz,			//  "     	  "       "  	 "   rolling     "   

			lastdeltay,		//previous 	  "       "   	 "  pitching     "   

			lastdeltaz,		//    "   	  "       "   	 "   rolling     "   

			onebeforelastdeltay,//last but one   "       "   	 "  pitching     "   

			onebeforelastdeltaz;// "    "   "    "       "   	 "   rolling     "   

		

	int 	  	rollslope,		//angle of surface in  rolling  direction

			pitchslope;		//  "   "     "    "  pitching      "    

	double 	proll,	//proportional amplification for  rolling     "    

			ppitch,	//      "            "        "  pitching     "    

			iroll,	//    integral       "        "   rolling     "    

			ipitch,	//        "          "        "  pitching     "    

			droll,	//differential       "        "   rolling     "    

			dpitch;	//     "             "        "  pitching     "    

	

	double 	virtualdelta[NUMBEROFSERVOS];	//internal output, consideres even small changes, avoiding typecast

	int    	deltaservo[NUMBEROFSERVOS],	//output for realtive servo driving

			olddeltaservo[NUMBEROFSERVOS],//last output for change recognition

			speed;					//driving speed

	

	static float ownsin[90];	//sine look-up table with 1° resolution for slope calculation

	

	TimerHandle  InterruptHandle;	//handle for control loop interrupt

	bool 	   interrupt_active;//latch to indicate an active interrupt

	

	Control();

	/*standard constructor for initialisations*/ 

	

	static Control SingleInstance;	

	/*The SingleInstance of this Class*/

	

};



#endif