/****************************************************************/
/* Routine for reading gyro sensor                              */
/* and sending back to the pc                                   */
/* -------------------------------                              */
/* Author: Rich Chi Ooi   UWA                                   */
/* Date  : 03.09.2003                                           */
/* Version: 1.4                                                 */
/* Compilation  procedure:                                      */
/*       Linux                                                  */
/*      gcc68 -c  XXXXXX.c (to create object file)              */
/*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                    */
/*Upload data :							*/
/*	ul filename.txt						*/
/****************************************************************/
/* In this version: Code restructured to for reusability        */ 
/****************************************************************/
  
#include <stdlib.h>
#include "stdio.h"
#include "eyebot.h"
#include "ppwa.h"
#include "sensors.h"
#define MAX 10000


/*Gyro Scaling to rad/s */
static const float  GY_SCALE = 0.002798;

/*Time step for encoder reading*/
static const float DT = 0.02;

/*Global Variables*/
int GY_RAW[MAX];
float GY_ANGLE[MAX];
int counter = 0;
int average = 0;

/*
 *Reads the gyro value 
 */
static void gyroRead(){
  int angVel;          /*gyro velocity*/
  
  angVel = accreadY();
  LCDPrintf("RAW:%d\n",angVel);
  GY_RAW[counter] = angVel;
  counter++;
}

/*
 *Uploading data to PC
 */
static void uploadData()
{
  FILE *ser;
  int iKey;
  int i;

  LCDPrintf("Upload Data?\n");
  OSWait(50);
  LCDMenu(" "," ","YES","END");
  iKey=KEYGet();

  if(iKey==KEY3){
    OSInitRS232(SER115200,NONE,SERIAL1);
    ser = fdopen(SERIAL1, "w");
    if (ser==NULL){
      LCDPrintf("serial init error\n"); 
    }else{
      LCDPrintf("init OK >%d<\n", (int) ser);
    }

    fprintf(ser,"GY_RAW,GY_ANGLE\n");
    for(i=0;i<MAX;i++){
      fprintf(ser,"%d",GY_RAW[i]);
      fprintf(ser,"\n");
    }

    fclose(ser);
    LCDPrintf("data sent");
    AUBeep();
  }
}

/*
 *main to execute functions
 */
int main(void)
{  
  int i;
  ServoHandle gyro;
  //  TimerHandle getGyroTimer;

  /*initialize ports and sensors*/
  tpuInit();
  gyroInit(gyro);

  /* Timer to read gyro value every 10ms*/
  // getGyroTimer = OSAttachTimer(2,(TimerFnc)gyroRead);
  
  while(counter!=MAX){
    gyroRead();
    OSWait(50);
  }  

  OSWait(50);
  
  /* Release all handles*/
  SERVORelease(gyro);
  // OSDetachTimer(getGyroTimer);

  LCDPrintf("reading ends !\n");
  OSWait(100);

  /*upload data*/
  uploadData();
}