/* fileName: acc.c
 * purpose: Class to handle acceleration sensors
 * author: Jesse Pepper <peppe-jj@ee.uwa.edu.au>
 * version: 0.00a
 * bugs:
 * notes:
 */

#include <eyebot.h>

#include "globalDefines.h"
#include "acc.h"
#include "biped.h"

extern globals g;

static float                 accFB;
static float                 accLR;
static TimerHandle           accTimerHandle;
static float                 influence;

/* Prototypes for Private functions */

static void accTimer( void );

/* Public functions */

void accInit( int timerScale, float newInfluence )
	{
	accFB = 0.0;
	accLR = 0.0;
	accTimerHandle = OSAttachTimer( timerScale, (TimerFnc)accTimer );
	if (accTimerHandle == 0)
		g.e = accError;
	influence = newInfluence;
	}

float accGetFB( void )
	{
	return (accFB);
	}

float accGetLR( void )
	{
	return (accLR);
	}

/* Private functions */

static void accTimer( void )
	{
	static bool side=false;
	int nextFB=0;
	int nextLR=0;
	float nextAccFB=0.0;
	float nextAccLR=0.0;
	
	if (side == false)
		{
		side = true;
		nextFB = OSGetAD( cAccFBPort );

		nextAccFB = ((float)nextFB - cAccFBZero)/cAccFBZero;

		accFB = accFB*(1-influence) + nextAccFB*influence;
		}
	else
		{
		side = false;
		nextLR = OSGetAD( cAccLRPort );

		nextAccLR = ((float)nextLR - cAccLRZero)/cAccLRZero;

		accLR = accLR*(1-influence) + nextAccLR*influence;
		}
	}