/* fileName: stand.c
 * purpose: Makes the robot stand with feedback
 * author: Jesse Pepper <peppe-jj@ee.uwa.edu.au>
 * version: v0.00a
 * bugs:
 * notes: Initial compensation only through Torso
 */

#include <eyebot.h>

#include <math.h>
#include <stdlib.h>

/* Local Headers */
#include "stand.h"
#include "globalDefines.h"
#include "biped.h"

globals g;

void balance( void )
	{
	float fB=0.0;
	float lR=0.0;

	float actualLHipB=0.0;
	float actualRHipB=0.0;

	float currentErrorFB=0.0;
	float currentErrorLR=0.0;
	float previousErrorFB=0.0;
	float previousErrorLR=0.0;
	float adjustment=0.0;

	/* PID controller constants */
	float kP = 0.1;
	float kI = 0.10;
	float kD = 0.05;
	float time = 0.1;
	int ticks = 1;

	int posture[cNumServos] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
	bool done=false;
	int i;

	LCDPrintf( "balance...\n" );
	
	for (i=0; i<cNumServos; i++)
		posture[i] = 0;

	while (!done)
		{
		OSWait( ticks );
		/*OSWait( 100-ticks );*/

		lR = bipedGetAccLR();
		fB = bipedGetAccFB();

		previousErrorFB = currentErrorFB;
		previousErrorLR = currentErrorLR;

		/* adjust the hip angles using PID */
		adjustment = kP*(fB)
		           + kI*(previousErrorFB + fB)*time
		           + kD*(previousErrorFB - fB);

		actualLHipB += adjustment;
		actualRHipB += adjustment;

		posture[lHipB] = (int)actualLHipB;
		posture[rHipB] = (int)actualRHipB;

		bipedSetPosture( posture );
		}
	}

int main( void )
	{
	bipedInit( true, true );
	if (g.e != noError)
		LCDPrintf( "Init failed\n" );
	balance();
	/* SERVORelease */
	return (0);
	}

void fatal( char* message, int returnValue )
	{
	LCDPrintf( message );
	LCDMenu( "", "", "", "BYE" );
	KEYWait( KEY4 );
	exit (returnValue);
	}