const tSensors GyroSensor          = (tSensors) S1;   //gyro sensor//
const tSensors GyroSensor2          = (tSensors) S2;   //gyro sensor//
const tSensors GyroSensor3          = (tSensors) S3;   //gyro sensor//

#define GyroScale 4
#define half_h 2     // Increment used in Runge-Kutta integration
#define t_scale 500




task main ()
{
  float f1, f2, ff1, ff2, fff1, fff2;   // aa is the filter time-constant to take care of the gyro drift

 float GyroBias = 600, GyroBias2=600, GyroBias3=600;
 float Gyro_value=0, a=0.1, Gyro_value2=0, Gyro_value3=0;

  float theta=0, theta_old=0, t_old=0;
  float theta2=0, theta_old2=0, theta3=0, theta_old3=0;

  float e0, kp=3.1;
  float e02, e03;

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

 	theta_old2 = 0;

 	ff1=0;

 		ClearTimer(T1);
	// Find the gyro bias associated w/ the balanced position
	// Hold the robot stationary until you hear the beep.  this is needed to compute the gyro bias

	while (time1[T1] < 5000) {
		// filter the sensor output

		Gyro_value = SensorValue(GyroSensor);
		Gyro_value2 = SensorValue(GyroSensor2);
		Gyro_value3 = SensorValue(GyroSensor3);
		wait1Msec(10);
 		GyroBias = (1-a)*GyroBias + a*Gyro_value;
 		GyroBias2 = (1-a)*GyroBias2 + a*Gyro_value2;
 		GyroBias3 = (1-a)*GyroBias3 + a*Gyro_value3;
   }
/*  you need to let the program compute your gyro biases - my gyro biases are all different
   GyroBias = 620;
   GyroBias = 591;
   GyroBias3 = 601;
*/
  PlaySound(soundDownwardTones);
  nMotorEncoder[motorA] = 0;
  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 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)
  	ff2 = (SensorValue(GyroSensor2)-GyroBias2)/GyroScale; // f(tn+h/2)
  	fff2 = (SensorValue(GyroSensor3)-GyroBias3)/GyroScale; // f(tn+h/2)
 	  wait1Msec(half_h);
 		theta = theta_old + (f1+2*f2)*(nSysTime-t_old)/t_scale;
 		theta2 = theta_old2 + (ff1+2*ff2)*(nSysTime-t_old)/t_scale;
 		theta3 = theta_old3 + (fff1+2*fff2)*(nSysTime-t_old)/t_scale;
 		theta_old = theta;
 		theta_old2 = theta2;
 		theta_old3 = theta3;
 		f1 = (SensorValue(GyroSensor)-GyroBias)/GyroScale; // f(tn)
 		ff1 = (SensorValue(GyroSensor2)-GyroBias2)/GyroScale; // f(tn)
 		fff1 = (SensorValue(GyroSensor3)-GyroBias3)/GyroScale; // f(tn)
 		t_old = nSysTime;

   e0 = ((-theta) - (float)nMotorEncoder[motorB]);
   e02 = ((-theta2+theta) - (float)nMotorEncoder[motorA]);
   e03 = ((theta3) - (float)nMotorEncoder[motorC]);
   motor[motorB] = kp*e0;
   motor[motorA] = kp*e02;
   motor[motorC] = kp*e03;
 	}
}