#pragma config(Sensor, S2,     lightSensorleft,             sensorLightActive)
#pragma config(Sensor, S3,     lightSensorright,            sensorLightActive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
nMotorEncoder[motorC] = 0;
nMotorEncoder[motorB] = 0;
while(time1[T1] < 3000000)
{
  float i = SensorValue(lightSensorleft);
  float k = SensorValue(lightSensorright);


if(i < 45) // if i is Lessthan 45 motor left motorC is reduce power to 0 and motor right motorB gets power to 80
{
  nxtDisplayCenteredTextLine(1, "SensorLt50: %d", SensorValue[lightSensorleft]);
  nxtDisplayCenteredTextLine(6, "SensorRt: %d", SensorValue[lightSensorright]);
motor[motorC] = 0;
motor[motorB] = 80;
}

else if(k > 45)// if i is Greaterthan 45 motor left motorC gets power to 80 and motor right motorB is reduce power to 0
{
   nxtDisplayCenteredTextLine(1, "SensorLt50: %d", SensorValue[lightSensorleft]);
   nxtDisplayCenteredTextLine(6, "SensorRt: %d", SensorValue[lightSensorright]);

motor[motorC] = 80;
motor[motorB] = 0;
}
}
motor[motorC] = 0;
motor[motorB] = 0;
}

#include "NXCDefs.h"
#include "NBCCommon.h"

#define MOTOR_LEFT      OUT_B
#define MOTOR_RIGHT     OUT_C
#define MOTOR_BOTH      OUT_BC
#define MOTOR_FORK      OUT_A
#define COLORSENSOR   SENSOR_2
#define TOUCH      SENSOR_3
#define ULSONIC      SensorUS(IN_1)
#define LIGHT_BLACK      115
#define LIGHT_WHITE      275

struct RGB {
   unsigned int red;
   unsigned int green;
   unsigned int blue;
};

RGB get_color(int sensorPort) {
   RGB color;
   color.red = ColorSensorValue(sensorPort, INPUT_RED);
   color.green = ColorSensorValue(sensorPort, INPUT_GREEN);
   color.blue = ColorSensorValue(sensorPort, INPUT_BLUE);
   return color;
}

unsigned int get_light(int sensorPort) {
   RGB color = get_color(sensorPort);
   float colorSum = color.red + color.green + color.blue;
   unsigned int light = colorSum / 3.0;
   return light;
}

sub follow_line() {
   
   float kP = 1.0; //6.0
   float kI = 0.0; //0.0001
   float kD = 0.0; //400

   
   float dT = 25;
   
   // Initialize the variables:
   int offset = LIGHT_WHITE - LIGHT_BLACK;
   int tP = 50;
   
   int proportional = 0;
   int integral = 0;
   int derivative = 0;
   
   int lastError = 0;
   
   while (2) {
      int lightValue = get_light(IN_2);
      int error = lightValue - offset;
      proportional = kP * error;
      integral = integral + error;
      derivative = (error - lastError) * dT;
      int output = proportional + kI * dT * integral + kD * derivative;
     
      int speedLeft = tP - output;
      int speedRight = tP + output;

   int value = 0;
     
      if (speedLeft > 100) {
         speedLeft = 100;
      } else if (speedLeft < -100) {
         speedLeft = -100;
      }
     
      if (speedRight > 100) {
         speedRight = 100;
      } else if (speedRight < -100) {
         speedRight = -100;
      }
     
      lastError = error;
   while(COLORSENSOR == INPUT_BLUECOLOR){
      Off(MOTOR_BOTH);
      while (ULSONIC == 32){
         PlayTone(330,400);
         OnFwdSync(OUT_BC, 50, 0); Wait(1000);
      }
      while (COLORSENSOR == INPUT_GREENCOLOR) {
         value = 1;
         Off(MOTOR_BOTH); Wait(100);
         while (ULSONIC == 24) {
             PlayTone(330, 400);
             OnRev(MOTOR_FORK, 100); Wait(2500);
             RotateMotor(MOTOR_BOTH, 50, 400);
             Off(MOTOR_BOTH);
              OnFwd(MOTOR_FORK, 100); Wait(2500);
             while (TOUCH == 1) {
            OnFwd(MOTOR_LEFT, 80);
            OnRev(MOTOR_RIGHT,80);
            Wait(999);
            break;
             }
         }
      }
      
      while (COLORSENSOR == INPUT_REDCOLOR) {
          Off(MOTOR_BOTH); Wait(100);
          while(ULSONIC == 24) {
              PlayTone(330, 400);
                   OnRev(MOTOR_FORK, 100); Wait(2500);
              RotateMotor(MOTOR_BOTH, 50, 400);
              Off(MOTOR_BOTH);
              OnFwd(MOTOR_FORK, 100); Wait(2500);
              while(TOUCH) {
            OnFwd(MOTOR_LEFT, 80);
            OnRev(MOTOR_RIGHT,80);
            Wait(900);
            break;
              }
              while(TOUCH == 0) {
            Off(MOTOR_BOTH);
              }
         }
      }    
      while (COLORSENSOR == INPUT_YELLOWCOLOR) {
          Off(MOTOR_BOTH); Wait(100);
          while(ULSONIC == 24) {
              PlayTone(330, 400);
                   OnRev(MOTOR_FORK, 100); Wait(2500);
              RotateMotor(MOTOR_BOTH, 50, 400);
              Off(MOTOR_BOTH);
              OnFwd(MOTOR_FORK, 100); Wait(2500);
              while(TOUCH) {
            OnFwd(MOTOR_LEFT, 80);
            OnRev(MOTOR_RIGHT,80);
            Wait(900);
            break;
              }
              while (TOUCH == 0){
            Off(MOTOR_BOTH);
              }
         }
      }    
   }

      OnFwd(MOTOR_LEFT, speedLeft);
      OnFwd(MOTOR_RIGHT, speedRight);
    }
   
}
void setup() {
   SetSensorColorFull(IN_2);
   SetSensorLowspeed(IN_1);
   SetSensorTouch(IN_3);
}
void grab(){
   OnFwd(MOTOR_FORK, 100);
   Wait(1000);
   Off(OUT_A);
}
sub not_touch(){
   while(!TOUCH){
      Off(MOTOR_BOTH);
   }
}
task main() {
   grab();
   setup();
   not_touch();
   follow_line();

}