/***************************************************************************/
/* motorPID.c                                                              */
/* motor PID algorithm test                                                */
/* 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"

/*Declare all the handles*/
MotorHandle motorLeft;
MotorHandle motorRight;
QuadHandle quadLeft;
QuadHandle quadRight;

/*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;

/*PID gains*/
 float KP =2.3;
 float KI =0.5;
 float KD =0.7;

static const int MOTOR_SCALE = 63;

int command =0;
float v_desired =-0.6;

static float r_old = 0;
static float e_old = 0;
static float e_old2 = 0;
static float distance_old = 0;

void getDistance(){
 
  int enc_left_new = 0;
    
  float e_func = 0;
  float r_motor = 0;
 
  float distance_new = 0;
  float velocity = 0;
  
  enc_left_new = QUADRead(quadLeft);
    
  distance_new = enc_left_new*MPT;
  velocity = (distance_new - distance_old)/DT;

  e_func = v_desired-velocity; 
  r_motor = r_old + KP*(e_func-e_old)+KI*(e_func+e_old)/2
    +KD*(e_func-2*e_old+e_old2);
  
  command = (int) (r_motor*MOTOR_SCALE);

  if(command >100){
    command = 100;
  }
  else if(command<-100){
    command = -100;  
  }
  else{
    command=command;
  }

  MOTORDrive(motorLeft,command);
  
  distance_old = distance_new;
  e_old2 = e_old;
  e_old = e_func;
  r_old = r_motor;

  /*prints the distance*/
  LCDSetPos(1,0);
  LCDPrintf("V:%.4f",velocity);
  LCDSetPos(2,0);
  LCDPrintf("Command:%d",command);
  LCDSetPos(3,0);
  LCDPrintf("KI:%.2f",KI);
}

int main(void){

  TimerHandle distanceTimer;

  /*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();
  LCDMenu("+","-","zero","END");
  distanceTimer = OSAttachTimer(2,(TimerFnc)getDistance);
  
  while(KEYRead()!=KEY4){

    if(KEYRead()==KEY1){
      KI+=0.5;
    }
    else if(KEYRead()==KEY2){
      KI-=0.5;
    }
    else if(KEYRead()==KEY3){
     v_desired  +=0.1;
    }
  }

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