/***************************************************************************/
/* driveTest.c                                                             */
/* Robot drive straight algorithm                                          */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:12.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"

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

/*Time step for encoder reading*/
static const float DT = 0.02;

static int r_old = 0;
static int e_old = 0;
static int e_old2 =0;

/*PID gains*/
 float KP = 0.35;
 float KI = 0.025;
 float KD = 0.75;

MotorHandle motorLeft;
MotorHandle motorRight;
QuadHandle quadLeft;
QuadHandle quadRight;

void driveControl(){
  
  int enc_left_new = 0;
  int enc_right_new = 0;
  
  int e_func = 0;
  int r_motor = 0;
  int command = 30;

  float distance_new;
  float distance_old;
  float velocity;

  enc_left_new = QUADRead(quadLeft);
  enc_right_new = QUADRead(quadRight);

  distance_new = enc_left_new*MPT;

  e_func = enc_left_new-enc_right_new;
  r_motor =(int)( r_old+KP*(e_func-e_old)+KI*(e_func+e_old)/2
                 +KD*(e_func-2*e_old+e_old2));

  e_old2 = e_old;
  e_old = e_func;
  r_old = r_motor;

  LCDSetPos(3,0);
  LCDPrintf("distance:%.4f\n",distance_new);
  LCDSetPos(5,0);
  LCDPrintf("ER:%d\n",enc_right_new);
 
  MOTORDrive(motorLeft,command-r_motor);
  MOTORDrive(motorRight,command+r_motor); 
}

int main(void)
{
  TimerHandle motorTimer;

  motorLeft = MOTORInit(MOTOR_LEFT);
  motorRight = MOTORInit(MOTOR_RIGHT);
  quadLeft = QUADInit(QUAD_LEFT);
  quadRight = QUADInit(QUAD_RIGHT); 
 
  QUADReset(quadRight);
  QUADReset(quadLeft);

  OSWait(100);
  LCDPrintf("ROBOT INITIALIZED \n");

  motorTimer = OSAttachTimer(2,(TimerFnc)driveControl);
  while(KEYRead()!=KEY4){

    if(KEYRead()==KEY3){
      KI+=0.05;
    }
    if(KEYRead()==KEY2){
      KI-=0.05;
    }
    // LCDSetPos(1,0);
    // LCDPrintf("KI:%.4f\n",KI);
  }

  /*release all handles*/
  OSDetachTimer(motorTimer);
  MOTORRelease(motorLeft);
  MOTORRelease(motorRight);
  QUADRelease(quadLeft);
  QUADRelease(quadRight);
}