Team4.nxc

#define BLACK 42
#define WHITE 54
#define LIGHT_VALUER SENSOR_2
#define LIGHT_VALUEL SENSOR_3
#define l 11
#define THRESHOLD 80
#define MIC SENSOR_1
#define NEAR 20
float R = 24;
float kp = 1.9;
float ki = 0.0009;
float kd = 13;
float offset = (BLACK + WHITE ) /2 ;
float Tp =40;
float integralR =0;    float integralL =0;
float lastErrorR =0;    float lastErrorL =0;
float derivativeR =0; float derivativeL =0;
float turnR;      float turnL;
float powerRight;
float powerLeft;
float errorR;     float errorL;
float vR;
float vL= 100;
float omega;
float t;
int count = 0;
int temp;
float errorSemi = 26.5/24;
task followLine()
{
   while (true)
   {
   if(count ==1 )
   {
     R = R * errorSemi ;
     vR = vL *(R-(l/2))/(R+(l/2))  ;
      // omega = vL/(R+(l/2));
      // t = ((PI *  R) / omega );
      OnFwd(OUT_A, vL);
      OnFwd(OUT_C, vR);
     //Wait(t);
      Wait(2300) ;
      count =2;
   }

    while(count ==2)
   {
   errorR = LIGHT_VALUER - offset;
   errorL = LIGHT_VALUEL - offset;
   integralR = integralR + errorR;
   integralL = integralL + errorL;
   derivativeR = errorR - lastErrorR;
   derivativeL = errorL - lastErrorL;
   turnR = kp * errorR +ki * integralR + kd * derivativeR ;
   turnL = kp * errorL +ki * integralL + kd * derivativeL ;
   powerLeft = Tp + turnR - turnL;
   powerRight = Tp + turnL - turnR;

   OnFwd(OUT_C, powerLeft);
   OnFwd(OUT_A, powerRight);

   lastErrorR = errorR;
   lastErrorL = errorL;
  }
   }


}
task detectSound()
{
     while(true){
       until(MIC > THRESHOLD & count ==0);
       count = 1;
       until (MIC > THRESHOLD && count ==2);
       Stop(true);
       }
}
task detectObstacles()
{
   while(true)
   {
      while(SensorUS(IN_4)>NEAR) ;

           if (count >= 1)
           {
                temp = count;
                count = -1;
                OnFwd(OUT_A,100);
                OnRev(OUT_C,100);
                Wait(250);
                OnFwd(OUT_A,50);
                OnRev(OUT_C,50);
                until(LIGHT_VALUEL <= offset);

               count = temp;
            }


   }
}


task main() {
  SetSensorSound(IN_1);
  SetSensorLight(IN_2);
  SetSensorLight(IN_3);
  SetSensorUltrasonic (IN_4);

  Precedes(detectSound, followLine,detectObstacles);



}