/***************************************************************************/
/* measureCurrent.c                                                        */
/* Program to measure the current going into the motor                     */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:15.08.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"
#include "sensors.h"

MotorHandle motorLeft;
MotorHandle motorRight;

static const float CURRENT_SCALE =4.096/1024;

int command = 0;
float current =0;
int AD1 = 0;
int AD2 = 0;

void measureCurrent(){

  AD2 = analogSensor(2);
  current = (AD2 - AD1)*CURRENT_SCALE;
  
  LCDSetPos(0,0);
  LCDPrintf("Command:%d",command); 
  LCDSetPos(1,0);
  LCDPrintf("1:%d  2:%d",AD1,AD2); 
  LCDSetPos(2,0);
  LCDPrintf("I:%.4f",current);
  
}

int main(void)
{

  TimerHandle currentTimer;
  motorLeft = MOTORInit(MOTOR_LEFT);
  motorRight = MOTORInit(MOTOR_RIGHT);

  OSWait(100);
  LCDPrintf("ROBOT INITIALIZED \n");
  AD1 = analogSensor(2);
  LCDClear();
  currentTimer = OSAttachTimer(20,(TimerFnc)measureCurrent);
  while(KEYRead()!=KEY4){

    if(KEYRead()==KEY3){
      command+=10;
    }
    if(KEYRead()==KEY2){
      command-=10;
    }
    if(KEYRead()==KEY1){
      command =0;
    }
    //   MOTORDrive(motorLeft,command);
    MOTORDrive(motorRight,command);
 
  }

  /*release all handles*/
  OSDetachTimer(currentTimer);
  MOTORRelease(motorLeft);
  MOTORRelease(motorRight);
}