/*
 *Testing Murata gyro
 *reading data from analog input
 *version: 1.0
 *author: Rich Chi Ooi
 *Date: 06.06.2003
 */

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

#define MAX 1000
/*
 *Global Variables
 */
int neutral = 0;    //gyro zero value
int counter =0;
float gyAngle = 0.00;
float gyArray[MAX];
int gyRAW[MAX];
static const float dt = 0.02;    //time step 20ms

/*
 *get the zero velocity value
 */
void getNeutral()
{
  int initial = 0;
  int i;

  OSWait(100);
  for(i=1;i<10;i++){
    initial+=analogSensor(4);
    OSWait(10);
  }
  neutral = initial/11;
  LCDPrintf("Neutral:%d\n",neutral);
}

/*
 *prints the raw value of the sensor
 */
void getRaw(){

  int raw; 
  raw = analogSensor(4);
  LCDPrintf("RAW:%d\n",raw);
}

/*
 *gets the angle value from gyro
 */
void getGyAngle(){

  int rawVal;

  rawVal = analogSensor(4);
  gyAngle +=(rawVal - 339)*dt;
  gyRAW[counter]=rawVal;
  gyArray[counter]=gyAngle;
  counter++;
  LCDPrintf("GYAngle:%f\n",gyAngle); 
}

/*
 *converts the float value to integer for data sending
 */
void floatConvert()
{
  int i;

  for(i=0;i<1000;i++){
    gyArray[i]=gyArray[i]*100;
  }
}

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

   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(gyRAW[i]);
      sendCharacter(',');
      sendInteger((gyArray[i]*100));
      sendCharacter('\n');
    }
    LCDPrintf("data sent");
    AUBeep();
    /*terminate transmission*/
    /*OSSendRS232(&term,SERIAL1);*/
  }
}

int main(void)
{
  int i;
  ServoHandle servo;
  TimerHandle getGyAngleTimer;

  servo = SERVOInit(SERVO10);
  LCDPrintf("Testing the\n MURATA gyro\n");
  
  SERVOSet(servo,128);
  OSWait(50);

  getGyAngleTimer = OSAttachTimer(2,(TimerFnc)getGyAngle);
  
  /*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);

  /* Release all handles*/
  SERVORelease(servo);
  OSDetachTimer(getGyAngleTimer);
  upload(); //upload data
}