/****************************************************************/
/* Routine for testing digital sensors                          */
/* and sending data back to the pc                              */
/* -------------------------------                              */
/* Author: Rich Chi Ooi   UWA                                   */
/* Date  : 09.07.2003                                           */
/* Version: 1.0                                                 */
/* 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:                                             */ 
/****************************************************************/
  
#include <stdlib.h>
#include "eyebot.h"
#include "ppwa.h"
#define MAX 4000
#define INC_SCALE 0.000580479 //converts inclino to radians
#define GY_SCALE 0.002778 //converts velocity to radians per second

/*Global Variables*/
int GYRAW[MAX];
int INCRAW[MAX];
int GYBIASED[MAX];
double INCANGLE[MAX];
double INCVEL[MAX];
int counter =0;
int incZeroValue = 0;
int gyZeroValue = 0;



/*
 *Sends characters back to the pc via RS232
 */
static void sendCharacter(char character)
{
  int result;
  do
    {
      result = OSSendCharRS232(character, SERIAL1);
      if(result == 3)
	{
	  LCDClear();
	  LCDPrintf("Timeout error.\nRetrying...\n");
	  OSWait(50);
	  LCDClear();
	}
    } while(result == 3);
}

/*
 *Sends Integers to pc via RS232
 */
static void sendInteger(int integer){
  int i;
  double temp;
  int last;
  
  /*Handle negative integers */
  if (integer < 0){
    sendCharacter('-'); 
    integer *= -1;
  }
  
  if (integer == 0){
    sendCharacter('0');
  }
  else{
    temp = integer + 0.1;
    
    i = 0;
    while( temp > 1){
      temp = temp / 10;
      i++;
    }
    temp = temp * 10;
    
    while (i > 0){
      last = temp;
      temp = (temp - last)* 10;
      sendCharacter( last + '0');
      i--;
    }
  }
}

/*
 *Uploading data to PC
 */
static void uploadData(){
  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++){
      /*LCDPrintf("V: %d D: %d\n",values[i],diff[i]);*/
      sendInteger(GYRAW[i]);
      sendCharacter(',');
      sendInteger(INCRAW[i]);
      sendCharacter(',');
      sendInteger(GYBIASED[i]*1000);
      sendCharacter(',');
      sendInteger(INCANGLE[i]*1000);
      sendCharacter(',');
      sendInteger(INCVEL[i]*1000);
      sendCharacter('\n');
    }
    LCDPrintf("data sent");
    AUBeep();
    /*terminate transmission*/
    /*OSSendRS232(&term,SERIAL1);*/
  }
}

/*
 *Initialize static servo signal to the gyro
 */
static void gyroInit(gyroname){ 
  
  if (!(gyroname = SERVOInit(SERVO8))){
    LCDPrintf("Gyro Serv Err!\n");
  }
  else {
    SERVOSet(gyroname,128);     /*set servo signal to middle position*/
    LCDPrintf("Gyro Serv OK\n");
  }
  OSWait(100);
  
  /*using servo6/tpu 7 to read from gyro and prints it to the screen*/
  LCDPrintf("Ready to read TPU 7!\n");
  accinit();
  accrelease();
  accinit();
  LCDPrintf("Gyro Init Complete\n");
} 

/*
 *Initialize the inclinometer
 */
static void inclinoInit(){

  int i;
  int initial = 0;
  
  OSWait(100);
  for(i=0;i<10;i++){
    initial += accreadX();
    OSWait(10);
  }

  incZeroValue = initial/10;
  LCDPrintf("inclino ZERO value is:%d\n",incZeroValue);
  LCDPrintf("inclino Init Complete\n"); 
}

/*
 *Reads the gyro value 
 */
static void gyroRead(){
  int gyRaw = 0;          /*gyro velocity*/
  int gyBiased = 0;       /*gyro velocity difference from rest*/
  int incRaw = 0;
  double incAngle;
  double incAnglePrev;
  double incVEL;

  incRaw = accreadX();
  gyRaw = accreadY();

  incAngle = (incRaw - incZeroValue)*INC_SCALE;
  incVEL = (incAngle - incAnglePrev)/0.02;
  incAnglePrev = incAngle;

  gyBiased = (gyRaw - gyZeroValue)*GY_SCALE;
  LCDPrintf("V: %d A: %.2f\n",gyRaw,incAngle);

  /*Data logging*/
  GYRAW[counter] = gyRaw;
  INCRAW[counter] = incRaw;
  GYBIASED[counter] = gyBiased;
  INCANGLE[counter] = incAngle;
  INCVEL[counter] = incVEL;
  counter++;
}

/*
 *Gets the gyro rest average  value
 */
static void getGyroZero(){

  int i;
  int initial=0;

  OSWait(100);
  for(i=0;i<100;i++){
    initial +=accreadY();
    OSWait(10);
  }
  gyZeroValue = initial/100; 
  LCDPrintf("ZERO value is:%d\n",gyZeroValue);
}

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

  servo = SERVOInit(SERVO10);
  gyroInit(gyro);
  getGyroZero();
  inclinoInit();
  OSWait(500);

  /* Timer to read gyro value every 10ms*/
  getGyroTimer = OSAttachTimer(2,(TimerFnc)gyroRead);
  
  /*rotate gyro 5 times +30 to -30 degrees*/
  for(i=0;i<16;i++){
    
    SERVOSet(servo,198);
    OSWait(100);
    
    SERVOSet(servo,58);
    OSWait(100);	
  }
  
  SERVOSet(servo,128);
  OSWait(50);
  
  /* Release all handles*/
  SERVORelease(servo);
  SERVORelease(gyro);
  OSDetachTimer(getGyroTimer);

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

  /*upload data*/
  uploadData();
}