/***************************************************************************/
/* SensorTest.c                                                            */
/* Program to test the functionality of sensors                            */
/* For use with Digital gyro and inclino  sensor                           */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:07.07.2003                                                         */ 
/* -------------------------------                                         */
/* Compilation  procedure:                                                 */
/*       Linux                                                             */
/*      gcc68 -c  XXXXXX.c (to create object file)                         */
/*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
/***************************************************************************/
/* In this version:                                                        */
/***************************************************************************/

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

#define MAX 1000

/*
 *Global Variables
 */
int counter = 0;
int gyZeroValue = 0;
float incArray[MAX];
float incVEL[MAX];
int gyRAW[MAX];
int incRAW[MAX];

static const float dt = 0.02;    //time step 20ms

/*
 *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(',');
      // LCDPrintf("GYRAW:%d\n",gyRAW[i]);
      // OSWait(30);
      sendInteger((incArray[i]*1000));
      sendCharacter(',');
      sendInteger(incRAW[i]);
      sendCharacter(',');
      sendInteger(incVEL[i]*1000);
      sendCharacter('\n');
    }
    LCDPrintf("data sent");
    AUBeep();
    /*terminate transmission*/
    /*OSSendRS232(&term,SERIAL1);*/
  }
}

/*
 *Get gyro zero velocity value
 */
void getZeroVelocity()
{
  int i;
  int initial = 0;

  for(i=0;i<10;i++){
    initial+=gyroRaw();
    OSWait(10);
  }
  gyZeroValue = initial/10;

  LCDPrintf("Gyro Zero:%d\n",gyZeroValue);
}
/*
 *read gyro and inclinometer value
 */
void readSensor()
{
  int gyRawValue;
  float iangle;
  float iangleprev;
  float ianglevel;

  /*gyro*/
  gyRawValue= gyroRaw();
 
  /*inclino*/
  iangle= incAngle();
  ianglevel = (iangle-iangleprev)/0.02;
  iangleprev = iangle;

 
  /*saving data*/
  gyRAW[counter]=gyRawValue;
  incArray[counter] = iangle;
  incRAW[counter] = incRaw();
  incVEL[counter] = ianglevel;
  counter++;
}

int main(void)
{
  int i;
  ServoHandle gyro;
  ServoHandle servo;
  TimerHandle readSensorTimer;

  /*initialize ports and sensors*/
  gyroInit(gyro);
  tpuInit();
  inclinoInit();
  servo = SERVOInit(SERVO10);
 
  getZeroVelocity();
  OSWait(100);
  readSensorTimer = OSAttachTimer(2,(TimerFnc)readSensor);
  
  /*rotate gyro 8 times +30 to -30 degrees*/
  for(i=0;i<8;i++){
    
    SERVOSet(servo,198);
    OSWait(100);
    
    SERVOSet(servo,58);
    OSWait(100);
  }
    SERVOSet(servo,128);
    OSWait(50);

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