/***************************************************************************/
/* TimingKN.c                                                              */
/* Timing test algorithm for balanceKN.                                    */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:22.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 "ppwa.h"
#include "sensors.h"
#include "kalman.h"

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

/*Gyro Scaling to rad/s */
static const float  GY_SCALE = 0.002798;

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

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

float distance_left_old = 0;
float distance_right_old = 0;
int enc_left_new = 0;
int enc_right_new = 0;

float distance_left_new = 0;
float distance_right_new = 0;

float X = 0;
float Xdot = 0;
float PHI = 0;
float PHIdot = 0;
float command = 0;
    
float gy_angle = 0;
float in_angle = 0;
int motor_command = 0;

/*Gain values for state control*/
static const float X_K = -31.6;
static const float Xdot_K = -36.1;
static const float PHI_K = 1641.5;
static const float PHIdot_K = 127.2; 

/*
 *Initialize the filter
 */
void stateInit(){

  int gyRaw = 0;
  
  gyRaw = gyroRaw();
  q_bias =gyRaw*GY_SCALE;
  angle = incAngle(); 
}

int main(void)
{
  int start_time = 0;
  int end_time =0;
  int i =0;

  ServoHandle gyro;
  TimerHandle balanceTimer;

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

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

  start_time = OSGetCount();
  for(i=0;i<10000;i++){
    gy_angle = gyroRaw()*GY_SCALE;
    in_angle = incAngle();
    
    stateUpdate(gy_angle);
    kalmanUpdate(in_angle);
    
    PHI = in_angle;
    PHIdot = rate;
    
    enc_left_new = QUADRead(quadLeft);
    //  enc_right_new = QUADRead(quadRight);
    
    distance_left_new = enc_left_new*MPT;
    // distance_right_new = enc_right_new*MPT;
    
    X = distance_left_new;
    Xdot = (distance_left_new - distance_left_old)/DT;
    distance_left_old = distance_left_new;
    distance_right_old = distance_right_new;
    
    command =-1*((X*X_K)+(Xdot*Xdot_K)+(PHI*PHI_K)+(PHIdot*PHIdot_K));
    
    if(command>100){
      motor_command = 100;
    }
    else if(command<-100){
      motor_command = -100;
    }
    else{
      motor_command = (int) command;
    }
    MOTORDrive(motorLeft,motor_command);
    MOTORDrive(motorRight,motor_command);
    
    
    /*  LCDSetPos(1,0); */
    /*   LCDPrintf(" X:%.4f\n XD:%.4f\n",X,Xdot); */
    /*   LCDSetPos(3,0); */
    /*   LCDPrintf(" P:%.4f\n PD:%.4f\n",PHI,PHIdot); */
    /*   LCDSetPos(5,0); */
    /*   LCDPrintf("CMD:%.2f\n",command); */
  }
  end_time = OSGetCount();
  LCDPrintf("Time:%d",(end_time-start_time));
 
  /*release all handles*/
  OSDetachTimer(balanceTimer);
  SERVORelease(gyro);
  MOTORRelease(motorLeft);
  MOTORRelease(motorRight);
  QUADRelease(quadLeft);
  QUADRelease(quadRight);
  
}