/***************************************************************************/
/* balanceIR.c                                                             */
/* Balancing robot algorithm with PID control for driving straight         */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.3                                                            */
/* Date:14.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"
#include "IRnokia.h"
#include "irtv.h"

#define MAX 5000

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

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

/*Gain values for state control*/
static  float X_K = -31.6;
static const float Xdot_K = -34.3;
static const float PHI_K = 1467.6;
static const float PHIdot_K = 113.8; 

/*PID Gains*/

static const float KP = 0.35;
static const float KI = 0.025;
static const float KD = 0.75;

/* Global variables*/
static float distance_old = 0;
static int r_old = 0;
static int e_old = 0;
static int e_old2 = 0;
static int enc_left_new = 0;
static int enc_right_new = 0;
static int e_func = 0;
static int r_motor = 0;
static int turnL =0;
static int turnR =0;
static int turn_control = 0;

static float X = 0;
static float Xdot = 0;
static float PHI = 0;
static float PHIdot = 0;
static float command = 0;

static float move = 0;
static float gy_angle = 0;
static float in_angle = 0;
static  int motor_command = 0;

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

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

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

  enc_left_new = QUADRead(quadLeft);
  enc_right_new = QUADRead(quadRight);
  
  PHI = in_angle;
  PHIdot = rate;

  X = ((enc_left_new+enc_right_new)/2)*MPT;
  Xdot = ((X - distance_old)/DT)+move;
 
 if(turn_control != 0){
 }else{
  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;
  }
  distance_old = X;

  motor_command =(int) (-1*((X*X_K)+(Xdot*Xdot_K)+(PHI*PHI_K)+(PHIdot*PHIdot_K)));

  if(motor_command>95){
    motor_command = 95;
  }
  if(motor_command<-95){
    motor_command = -95;
  }
 
  MOTORDrive(motorLeft,(motor_command-r_motor+turnL));
  MOTORDrive(motorRight,(motor_command+r_motor+turnR));
 
 
  LCDSetPos(1,0); 
  LCDPrintf("%.2f\t %.2f\n",X_K,move);
  /*   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:%d\n",motor_command); */

}

int main(void)
{
  int code = 0;
  int terminated = 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();

  /* Initialize remote control for Nokia, buffer size: 4, repetition delay: 10 */
  LCDPrintf("IRTVInit: %d\n", IRTVInit(SPACE_CODE,15,0,0x03FF,SLOPPY_MODE,1,10));
 
  OSWait(50);
  balanceTimer = OSAttachTimer(2,(TimerFnc)balance);

  while (KEYRead()!=KEY4||!terminated)
    { switch (code = IRTVRead())
      { case 0: break;
      case RC_0:
	LCDPrintf("0\n");
	break;
		  	
      case RC_1:
	LCDPrintf("1\n"); 
	break;
		    	
      case RC_2:
	LCDPrintf("2\n");
	turn_control = 0;
	X_K = 0;
	move = 0.15;
	break;
		  	
      case RC_3:
	LCDPrintf("3\n");
	break;
			  
      case RC_4: 
	LCDPrintf("4\n");
	turn_control = 1;
	X_K = 0;
	turnL = -5;
	turnR = 5;
	break;
	  		
      case RC_5: 
	LCDPrintf("5\n");
	X_K = -31.6;
	QUADReset(quadRight);
	QUADReset(quadLeft);
	turn_control = 0;
	turnL = 0;
	turnR = 0;
	move = 0;
	X = 0;
	break;
	  		
      case RC_6: 
	LCDPrintf("6\n");
	turn_control = 1;
	X_K = 0;
	turnL = 5;
	turnR = -5;
	break;
	  		
      case RC_7: 
	LCDPrintf("7\n");
	break;
			  
      case RC_8: 
	LCDPrintf("8\n");
	turn_control = 1;
	X_K = 0;
	move = -0.15;
	break;
		  	
      case RC_9: 
	LCDPrintf("9\n");
	break;
      case RC_FF:
	LCDPrintf("FF\n");
	break;	
      case RC_RW:
	LCDPrintf("RW\n");
	break; 
      case RC_PLAY:
	LCDPrintf("Play\n");		
	break;
      case RC_STOP:
	LCDPrintf("Stop\n");
	break;
      case RC_OK:
	LCDPrintf("OK\n");
	break;			
      case RC_STANDBY:
	LCDPrintf("Standby\n");
	terminated = 1;
	break;
      default:
	LCDPrintf("unhandled: %4x\n", code);			
      }	
    }
  
  /*release all handles*/
  OSDetachTimer(balanceTimer);
  SERVORelease(gyro);
  MOTORRelease(motorLeft);
  MOTORRelease(motorRight);
  QUADRelease(quadLeft);
  QUADRelease(quadRight);
  IRTVTerm();
  
}