/***************************************************************************/
/* balance.c                                                               */
/* Balancing robot algorithm with PID control for driving straight         */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.4                                                            */
/* Date:28.08.2003                                                         */ 
/* -------------------------------                                         */
/* Compilation  procedure:                                                 */
/*       Linux                                                             */
/*      gcc68 -c  XXXXXX.c (to create object file)                         */
/*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
/***************************************************************************/
/* In this version:                                                        */
/* Codes are restructured and reorganised to ensure proper execution       */
/***************************************************************************/

#include <math.h>
#include <stdlib.h>
#include "eyebot.h"
#include "ppwa.h"
#include "sensors.h"
#include "kalman.h"

/*Scale factor and constants*/
static const float MPT = (M_PI*0.051)/540;  /*Metres per tick*/
static const float GY_SCALE = 0.002798;     /*Gyro scaling to rad/s*/
static const float DT = 0.02;               /*Time step for encoder reading*/


/*Gain values for state control*/
static const float X_K = -31.6;            /*Vehicle Positon*/
static const float Xdot_K = -36.1;         /*Vehicle Velocity*/
static const float PHI_K = 1641.5;         /*Tilt angle*/
static const float PHIdot_K = 127.2;       /*Tilt Angle velocity*/


/*PID Gains*/
static const float KP = 0.35;
static const float KI = 0.025;
static const float KD = 0.75;

/*Handle declarations*/   
MotorHandle motorLeft;
MotorHandle motorRight;
QuadHandle quadLeft;
QuadHandle quadRight;

static float distance_old = 0;
static int r_old = 0;
static int e_old = 0;
static int e_old2 = 0;
static int e_func = 0;
static int r_motor = 0;

static float X = 0;
static float Xdot = 0;
static float PHI = 0;
static float PHIdot = 0;
static float command = 0;
static  int motor_command = 0;
static int enc_left = 0;
static int enc_right = 0;

/*
 *Initialize the filter
 */
void stateInit(){
  
  q_bias = gyroRaw()*GY_SCALE;
  angle = incAngle(); 
}

void balance()
{
 
  stateUpdate(gyroRaw()*GY_SCALE);
  kalmanUpdate(incAngle());

  enc_left = QUADRead(quadLeft);
  enc_right = QUADRead(quadRight);

  /*States values*/
  PHI = incAngle();
  PHIdot = rate;
  X = ((enc_left+enc_right)/2)*MPT;
  Xdot = (X - distance_old)/DT;

  e_func = enc_left-enc_right;
  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;
  distance_old = X;

  motor_command =(int)(-1*((X*X_K)+(Xdot*Xdot_K)+(PHI*PHI_K)+(PHIdot*PHIdot_K)));
  
  if(motor_command>100){
    motor_command = 100;
  }
  if(motor_command<-100){
    motor_command = -100;
  }
  
  MOTORDrive(motorLeft,motor_command-r_motor);
  MOTORDrive(motorRight,motor_command+r_motor);

}

/* Main program*/
int main(void)
{
  ServoHandle gyro;
  TimerHandle balanceTimer;

  /*Initialize motors and encoders*/  
  motorLeft = MOTORInit(MOTOR_LEFT);
  motorRight = MOTORInit(MOTOR_RIGHT);
  quadLeft = QUADInit(QUAD_LEFT);
  quadRight = QUADInit(QUAD_RIGHT);
  
  /*initialize ports and sensors*/
  tpuInit();
  OSWait(50);
  gyroInit(gyro);
  OSWait(50);
  inclinoInit();
  OSWait(50);
  QUADReset(quadRight);
  QUADReset(quadLeft);

  stateInit();
  OSWait(200);
  LCDPrintf("ROBOT INITIALIZED \n");
  LCDClear();

  balanceTimer = OSAttachTimer(2,(TimerFnc)balance);
  while(KEYRead()!=KEY4){
  }
  /*release all handles*/
  OSDetachTimer(balanceTimer);
  SERVORelease(gyro);
  MOTORRelease(motorLeft);
  MOTORRelease(motorRight);
  QUADRelease(quadLeft);
  QUADRelease(quadRight);
  
}