//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

// Robot on two wheels
// Change 'bias' to make the robot turn faster or slower (zero for stationary)
//  k1, k2, k3, k4 feedback gains are specified below

const tSensors GyroSensor          = (tSensors) S1;   //gyro sensor//

#define GyroScale 4
#define half_h 1     // Increment used in Runge-Kutta integration
#define t_scale 500
#define minDist 20  // distance before robot turns
#define v 0.195      // This is the desired velocity of the robot

task main ()
{
  float a=0.2, aa=0.001;   // aa is the filter time-constant to take care of the gyro drift
  float theta_bias = 0;
  float a_pwr=1, f_pwr=0;
  // This is the feedback loop gain
  float k1 = 0.0;     // position feedback gain
  float k2 = 90;      // velocity feedback gain
  float k3 = 8;      // tilt feedback gain
  float k4 = 10.0;    // angular velocity feedback gain
  long f1=0, f2=0, Gyro_value = 0;
  int pwr=0, pwr_th, pwr_w, batt=0;
  float GyroBias = 591.9;  // This is the bias that I had to apply to my gyro sensor.  You may need to find your own gyro bias
  float k_pwr;  // This is the gain that is computed adaptively based on the battery voltage (as the battery is drained, the gain is increased)

  int powermove=0;

  float theta=0, theta_old=0, t_old=0;
  float x=0, x_old=0, x_dot=0;

  // This causes the motors to stop when they are set to zero
	bFloatDuringInactiveMotorPWM = false;
 	theta_old = 0;
 	t_old= nSysTime;
 	f1=0;
 	x_old=0;

/*
	ClearTimer(T1);
	// Find the gyro bias associated w/ the balanced position
	// Hold the robot in the balanced position for 3 sec to find the gyro bias
	GyroBias = 590;
	while (time1[T1] < 100) {
		// filter the sensor output
		Gyro_value = SensorValue(GyroSensor);
		wait1Msec(1);
 		GyroBias = (1-a)*GyroBias + a*Gyro_value;
   }
   // play a sound when the training is over
   PlaySound(soundBlip);
*/
  // I ended up hard-coding the bias after measuring it a few times.
  // comment out the following line if you want the Gyro bias to be measured adaptively
	//GyroBias = 590.5;

nxtDisplayCenteredTextLine(0,(string)GyroBias);
	// Measure the battery voltage and compensate for it by adjusting the gain (k_pwr)
  batt=nAvgBatteryLevel;
  k_pwr = 0.7 + (0.7-1.1)/(8816-8196)*(batt-8816);

  nMotorEncoder[motorC] = 0;
  nMotorEncoder[motorA] = 0;

 while(true) {

		// Runge-Kutta integration (http://en.wikipedia.org/wiki/Runge-kutta)
  	wait1Msec(half_h);
  	f2 = (SensorValue(GyroSensor)-GyroBias)/GyroScale; // f(tn+h/2)
 	  wait1Msec(half_h);
 		theta = theta_old + (f1+2*f2)*(nSysTime-t_old)/t_scale;
 		theta_old = theta;
  	x = nMotorEncoder[motorC];
  	// compute the linear velocity
  	x_dot = x-x_old;
 		f1 = (SensorValue(GyroSensor)-GyroBias)/GyroScale; // f(tn)
 		t_old = nSysTime;
	  x_old = x;

	  // Compute the long-term average of tilt
    theta_bias = theta_bias*(1-aa) + theta*aa;

    		powermove=((((nMotorEncoder[motorA]+nMotorEncoder[motorC])/2)-((nMotorEncoder[motorA]+nMotorEncoder[motorC])/2)/2)*(22/7))/8;

nxtDisplayCenteredTextLine(2,"%0d",nMotorEncoder[motorA]);
nxtDisplayCenteredTextLine(3,"%0d",nMotorEncoder[motorC]);
nxtDisplayCenteredTextLine(4,"%0d",nMotorEncoder[motorA]+nMotorEncoder[motorC]);
nxtDisplayCenteredTextLine(5,"%0d",powermove);
 		pwr_th = k3*(theta-theta_bias);
 		pwr_w = k4*f1;
 		pwr = pwr_th + pwr_w + k1*x + k2*(x_dot-(v));
 		f_pwr = (1-a_pwr)*f_pwr + a_pwr*pwr;
 		// k_e is to keep the robot going straight
		motor[motorA] = (k_pwr*f_pwr)+powermove;
		motor[motorC] = (k_pwr*f_pwr)+powermove;

 	}
}