/****************************************************************/
/* 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 "sendData.h"
#include "sensors.h"

#define MAX 1000

float incArray[MAX];
int counter =0;

/*
 *gets the inclinometer angle
 */
void getIncAngle()
{
  float iangle = incAngle();
  incArray[counter] = iangle;
  counter++;
}

/*
 *uploads data to PC via RS232
 */
void upload()
{
  int i;
  int iKey;

  //  floatConvert();
  LCDPrintf("Upload Data?\n");
  OSWait(50);
  LCDMenu(" "," ","YES","END");
  iKey=KEYGet();
  
  if(iKey==KEY3){
    OSInitRS232(SER115200,NONE,SERIAL1);

    for(i=0; i<=MAX;i++){
      sendInteger((incArray[i]*100));
      sendCharacter('\n');
    }
    LCDPrintf("data sent");
    AUBeep();
    /*terminate transmission*/
    /*OSSendRS232(&term,SERIAL1);*/
  }
}

int main(void)
{

  int i;
  ServoHandle servo;
  TimerHandle getIncAngleTimer;

  inclinoInit();
  servo = SERVOInit(SERVO10);
  SERVOSet(servo,128);
  OSWait(50);

  getIncAngleTimer = OSAttachTimer(2,(TimerFnc)getIncAngle);

 /*rotate gyro 5 times +30 to -30 degrees*/
  for(i=0;i<5;i++){
    
    SERVOSet(servo,198);
    OSWait(100);
    
    SERVOSet(servo,58);
    OSWait(100);
  }
  SERVOSet(servo,128);
  OSWait(50);
  
  OSDetachTimer(getIncAngleTimer);
  SERVORelease(servo);
  upload();
}