/***************************************************************************/
/* encoderTest.c                                                           */
/* Balancing robot algorithm test                                          */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:23.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 <math.h>
#include <stdlib.h>
#include "eyebot.h"

/*Declare all the handles*/
MotorHandle motorLeft;
MotorHandle motorRight;
QuadHandle quadLeft;
QuadHandle quadRight;
TimerHandle distanceTimer;
  
float distance_left_old = 0;
float distance_right_old = 0;

/*metre per tick*/
static const float MPT = (M_PI*0.051)/540;

/*time step of the motor velocity is called*/
static const float DT = 0.02;

void getDistance(){

  int new_left = 0;
  int new_right = 0;

  float distance_left_new = 0;
  float distance_right_new = 0;
  float velocity_left = 0;
  float velocity_right = 0;

  new_left = QUADRead(quadLeft);
  new_right = QUADRead(quadRight);
  // LCDPrintf("OL:%d   OR:%d\n",old_left, old_right);
  
  distance_left_new = new_left*MPT;
  distance_right_new = new_right*MPT;

  velocity_left = (distance_left_new - distance_left_old)/ DT;
  velocity_right = (distance_right_new - distance_right_old)/ DT; 
  distance_left_old = distance_left_new;
  distance_right_old = distance_right_new;

 /*  /\*prints the menu*\/ */
/*   LCDSetPos(0,2); */
/*   LCDPutString("LEFT  RIGHT"); */

/*   /\*prints the distance*\/ */
/*   LCDSetPos(1,1); */
/*   LCDPutFloatS(distance_left_new,3,2); */
/*   LCDSetPos(1,9); */
/*   LCDPutFloatS(distance_right_new,3,2); */

 /*  /\*prints the velocity*\/ */
/*   LCDSetPos(2,1); */
/*   LCDPutFloatS(velocity_left,3,2); */
/*   LCDSetPos(2,9); */
/*   LCDPutFloatS(velocity_right,3,2); */

  //CDPrintf("DL:%.2f\t DR:%.2f\n",distance_left_new,distance_right_new);
  //LCDPrintf("L:%.2f R:%.2f\n",velocity_left, velocity_right);
  LCDPrintf("L:%.6f\n",velocity_left);
}

int main(void)
{
  int speed_left = 0;
  int speed_right = 0
;
  /*initialize ports and sensors*/
  motorLeft = MOTORInit(MOTOR_LEFT);
  motorRight = MOTORInit(MOTOR_RIGHT);
  quadLeft = QUADInit(QUAD_LEFT);
  quadRight = QUADInit(QUAD_RIGHT); 
  QUADReset(quadRight);
  QUADReset(quadLeft);
  
  LCDPrintf("ROBOT INITIALIZED \n");
  OSWait(100);
  LCDClear();

  distanceTimer = OSAttachTimer(2,(TimerFnc)getDistance);
  LCDMenu("L+","R+","zero","END");
  while(KEYRead()!=KEY4){
    if(KEYRead()==KEY1){
      speed_left += 5;
    }
    else if(KEYRead()==KEY2){
      speed_right += 5;
    }
    else if(KEYRead()==KEY3){
      speed_left = 0;
      speed_right = 0;
    }
    //    MOTORDrive(motorLeft,speed_left);
    //  MOTORDrive(motorRight,speed_right);
   /*  LCDSetPos(4,1); */
/*     LCDPutInt(speed_left); */
/*     LCDSetPos(4,9); */
/*     LCDPutInt(speed_right); */
  }
  
  /*release all handles*/
  OSDetachTimer(distanceTimer);
  MOTORRelease(motorLeft);
  MOTORRelease(motorRight);
  QUADRelease(quadLeft);
  QUADRelease(quadRight);
}