/* ----------------------------------------------------------------- */
/* 'ZServoClass.cc'										    */
/* Servo- Functions									         */
/* 													    */
/* 	last modified 07/04/2003								    */
/* 	used to Initialize and Release Servos					    */
/* 													    */
/* 													    */
/* 													    */
/* 													    */
/* 	(C) Jochen Zimmermann								    */
/* ----------------------------------------------------------------- */


#include "ZHeaders.h"
#include <stdlib.h>
/*
//Headerfile is included by ZHeaders.h
#include "ZServoClass.h"
*/

//Single Instance is globally created
Servo Servo::SingleInstance;


//constructor
//initialises members
Servo::Servo()
{
	limit[RHIPS]=35;
	limit[RHIPB]=127;
	limit[RKNEE]=127;
	limit[RANKB]=127;
	limit[RANKS]=30;
	limit[LHIPS]=35;
	limit[LHIPB]=127;
	limit[LKNEE]=127;
	limit[LANKB]=127;
	limit[LANKS]=30;
	for(int i=0;i<NUMBEROFSERVOS;i++)
		position[i]=128;
	initialized=false;
}


//instance retriever
Servo* Servo::GetTheInstance()
{
	return &SingleInstance;
}


//initialising servos and members
void Servo::Init()
{
	/*right leg*/
	handle[RHIPS]=SERVOInit(R_HipSide);
	OSWait(2);
	handle[RHIPB]=SERVOInit(R_HipBend);
	OSWait(2);
	handle[RKNEE]=SERVOInit(R_Knee);
	OSWait(2);
	handle[RANKB]=SERVOInit(R_AnkleBend);
	OSWait(2);
	handle[RANKS]=SERVOInit(R_AnkleSide);
	OSWait(2);
	/*left leg*/
	handle[LHIPS]=SERVOInit(L_HipSide);
	OSWait(2);
	handle[LHIPB]=SERVOInit(L_HipBend);
	OSWait(2);
	handle[LKNEE]=SERVOInit(L_Knee);
	OSWait(2);
	handle[LANKB]=SERVOInit(L_AnkleBend);
	OSWait(2);
	handle[LANKS]=SERVOInit(L_AnkleSide);	
	OSWait(2);
	limit[RHIPS]=35;
	limit[RHIPB]=127;
	limit[RKNEE]=127;
	limit[RANKB]=80;
	limit[RANKS]=30;
	limit[LHIPS]=35;
	limit[LHIPB]=127;
	limit[LKNEE]=127;
	limit[LANKB]=80;
	limit[LANKS]=30;
	for(int i=0;i<NUMBEROFSERVOS;i++)
		position[i]=128;
	initialized=true;
}


//release servos
void Servo::Release()
{
	if(!initialized) return;
	for(int i=0;i<NUMBEROFSERVOS;i++)
	{
		OSWait(1);
		SERVORelease(handle[i]);		
	}
	initialized=false;
}


//drive servos by absolute positions
//not recommended to use during running interrupt control 
//because this will override the values determined by the controller
int Servo::Set(int* pos, int speed)
{
	//Used to drive Servo to dedicated angle
	//use speed 1(slow) to 10(fast) (probably 7-10 would 
	//make no difference, but 10 has no waitstates)
	//return value is the time consumption
	if(!initialized) 
	{
		Display* lcd=Display::GetTheInstance();
		UserInput* key=UserInput::GetTheInstance();
		lcd->Print("Servos are not initialized!");
		key->Wait();
		return 0;
	}	
	int i,j,k,counter=0,dist,togo,max=0;
	
	if( speed ==0) return 0;
	if( speed < 1) speed = 1;
	
	togo=speed;
	
	for (i=0;i<NUMBEROFSERVOS;i++)
	{
		//cut boundaries
		if (pos[i]>128+limit[i]) pos[i] = 128+limit[i]; else 
      	if (pos[i]<128-limit[i]) pos[i] = 128-limit[i];
	}
	
	//maximum distance to go
	
	for (i=0;i<NUMBEROFSERVOS;i++)
	{
		if( ( abs(pos[i]-position[i]) ) >max )
			max=abs(pos[i]-position[i]);
	}
	
	for(j=0;j<max;j++)
	{
		for(i=0;i<NUMBEROFSERVOS;i++)
		{
			//drive the servos
			togo=(pos[i]-position[i]);
			if(togo>0) SERVOSet(handle[i],position[i]++);
			if(togo<0) SERVOSet(handle[i],position[i]--);
		}
		for(k=0;k<((10-speed)*94);k++)
			OSWait(0);				//consumes 10.66µs per call, so that 94*10.66µs consume approx. 1ms
	}
	return (max*(10-speed)*1000+60);
}


//retrieve actual servoangles
void Servo::Get(int* pos)
{
	if(position==NULL) return;
	for(int i=0;i<NUMBEROFSERVOS;i++)
		pos[i]=position[i];
}


//retrieve servolimit, if pos is negative, the lower limit is to be returned
int  Servo::GetLimit(int pos)
{
	if(pos>0 && pos< NUMBEROFSERVOS)	return 128+limit[ pos];
	if(pos<0 && pos>-NUMBEROFSERVOS)   return 128-limit[-pos];
	else return -1;				//error
}


//drive servos relative to actual position
int Servo::Move(int* delta, int speed)
{
	int ret=0;
	for(int i=0;i<NUMBEROFSERVOS;i++)
		delta[i]+=position[i];
	ret=Set(delta,speed);
	for(int i=0;i<NUMBEROFSERVOS;i++)
		delta[i]=0;
	return ret;
	
}