/****************************************************************/
/* inctest.c                                                    */
/* Routine for reading digital inclinometer sensor value     	*/
/* and printing the values to screen  				*/
/* -------------------------------				*/
/* Author: Rich Chi Ooi	  UWA	 				*/
/* Date  : 06.06.2003            				*/
/* Version: 1.0 		  				*/
/* Compilation  procedure: 					*/
/*	gcc68 -c inctestD.c (to create object file)		*/
/*	gcc68 -o XXXXXX.hex inctest.o ppwa.o 			*/
/****************************************************************/

#include <stdlib.h>
#include "eyebot.h"
#include "ppwa.h"
#include "sensors.h"

static const float INCLINO_SCALE = 60/1800;
void getInclino(){

  int inc_raw = 0;
  int gy_raw = 0;
  float angle = 0;

  inc_raw = incRaw();
  angle = incAngle();
  gy_raw = gyroRaw();

 /*prints the value*/
  LCDSetPos(1,0);
  LCDPrintf("raw:%d",inc_raw);
  LCDSetPos(2,0);
  LCDPrintf("raw:%d",gy_raw);
  LCDSetPos(3,0);
  LCDPrintf("angle:%.4f",angle);
}

int main(void)
{
  ServoHandle gyro;
  TimerHandle getIncAngle;

  tpuInit();
  gyroInit(gyro);
  inclinoInit();
  LCDClear();
  getIncAngle = OSAttachTimer(50,(TimerFnc)getInclino);

  while(KEYRead()!=KEY4){
  }
  SERVORelease(gyro);
  OSDetachTimer(getIncAngle);
}