/***************************************************************************/
/* sensors.c                                                               */
/* Collection of function to initialize and read sensor values             */
/* For use with Digital gyro and inclino  sensor                           */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.3                                                            */
/* 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 "ppwa.h"
#include "sensors.h"

#define FACTOR 0.000580479; //converts inclino to radians

/*
 * Global Variables
 */

int midValue = 0;
int gy_middle = 0;

/*
 * Reading from A/D port to get raw sensor data
 * For use with analog gyro
 */
int analogSensor (int x)  /* return the value of the analog input Number x */
{
  int rawData = 0;
	
  rawData = OSGetAD(x);   
  return rawData;
}

/*
 *Initialize the TPU for reading digital sensors
 *Servo 5(TPU 6) accreadX()
 *Servo 6(TPU 7) accreadY()
 */
void tpuInit()
{
  LCDPrintf("Ready to read TPU Channel!\n");
  accinit();
  accrelease();
  accinit();
  LCDPrintf("TPU Init Complete\n"); 
}

/*
 *Initialize the gyro
 */
void gyroInit(ServoHandle gyroName){
 
   /*Initialize static servo signal to the gyro*/
  if (!(gyroName = SERVOInit(SERVO8))){
    LCDPrintf("Gyro Serv Err!\n");
  }
  else {
    SERVOSet(gyroName,128);	/*set servo signal to middle position*/
    LCDPrintf("Gyro Init OK\n");
  }
}

/*
 *Initialize the inclinometer
 */
void inclinoInit(){
  int i;
  int initial= 0;
  
  OSWait(200);
  for(i=0;i<10;i++){
    initial += accreadX();
    OSWait(20);
  }
  midValue = initial/10;
  LCDPrintf("inclino middle value is:%d\n",midValue);
  LCDPrintf("inclino Init Complete\n"); 
}

/*
 * Reads TPU7 and convert inclinometer value to angle
 * in radians
 */
float incAngle(){
  int rawValue;
  float angle;

  rawValue = accreadX();
  angle =(rawValue - midValue)*FACTOR;
  // LCDPrintf("Angle:%.2f\n",angle);
  // LCDPrintf("Raw:%d\n",rawValue);
  return angle;
}

/*
 *Returns the inclino raw value
 */
int incRaw(){

  int inRaw;
  inRaw = accreadX();
  return inRaw;
}

/*
 *Returns the gyro raw value
 */
int gyroRaw(){

  int gyRaw;

  gyRaw = accreadY();
  // LCDPrintf("GYraw:%d\n",gyRaw);

  return gyRaw;
}

float gyVelocity(){

 float velocity = 0;
 int raw = 0;
 
 raw = accreadY();
 velocity = (raw - gy_middle)*0.002798;

 return velocity;
}

void gyroMiddle(){

int  initial = 0;
 int i;

  for(i=0;i<10;i++){
    initial+=accreadY();
    OSWait(10);
  }
  gy_middle = initial/10;
  LCDPrintf("GY middle:%d",gy_middle);
}