LeJos autonomous robot not working properly

import lejos.nxt.*;
import lejos.robotics.ColorDetector;
import java.lang.*;
public class BehaviorThread {
  public static void main(String[] aArg)
    throws Exception {
    //KALIBRERING
        LightSensor localLightSensor = new LightSensor(SensorPort.S1); //definerer lyssensor med navnet localLightSensor
        localLightSensor.setFloodlight(true); //tænder lyset
        int high = 0;
        int low = 0;
    LCD.drawString("Left to calibrate", 0, 0);
        Button.LEFT.waitForPress();
        localLightSensor.calibrateHigh();
        high = localLightSensor.readValue();
        LCD.clear();
        LCD.drawString("White calibrated", 0, 0);
    LCD.drawString("Right for black",0,2);
        Button.RIGHT.waitForPress();
        localLightSensor.calibrateLow();
        low = localLightSensor.readValue();
        LCD.clear();
        LCD.drawString("Black calibrated", 0, 0);
    LCD.drawString("Right to run!", 0, 2);
    Button.RIGHT.waitForPress();

    //VARIABLER TIL ULTRALYD
    UltrasonicSensor localUltrasonicSensor = new UltrasonicSensor(SensorPort.S3);
        localUltrasonicSensor.continuous();
        int dist = 0;

    //VARIABLER TIL FARVESENSOR
    ColorSensor localColorSensor = new ColorSensor(SensorPort.S4);
        int color = 0;
        int RED = ColorSensor.Color.RED;
        localColorSensor.setFloodlight(RED);
        color = localColorSensor.getColorID();


    //VARIABLER TIL LINEFOLLOWING
      int offset = (high + low) / 2; //gennemsnit ml sort og hvid
      int Tp = 35; //motor power når der køres ligeud
      int Kp = 35000; //hældningskoefficienten for linjen, konstant til styring af power
      //40000
      int Ki = 42; //konstant til justering af integralet
      int Kd = 600000; //konstant til justering af derivative

      int integral = 0;
      int lastError = 0;
      int derivative = 0;

      int LightValue = 0;
      int error = 0;
      int Turn = 0;
      int PowerA = 0;
      int PowerC = 0;

      NXTMotor mA = new NXTMotor(MotorPort.A);
      NXTMotor mC = new NXTMotor(MotorPort.C);

      int i = 0;
      int g = 0;
      int positive = 0;

      while (!Button.ESCAPE.isDown())
      {

        i = i + 1;
        g = g + 1;

    //Følgende kode crasher robotten, når den har kørt i kort tid. Vi ved ikke, hvorfor.
    /*  class ColorCheck extends Thread {
            public void run()
            {
                //ColorSensor localColorSensor = new ColorSensor(SensorPort.S4);
                int v = 0;
                int color = 0;
                while (v  <= 50) {
                    v = v + 1;
                    color = localColorSensor.getColorID();
                    //LCD.clear();
                    LCD.drawInt(color,0,4);
                    Sound.beep();
                }
            }

            }
        ColorCheck farve = new ColorCheck();
        //farve.setDaemon(true);
        farve.start();
    */

        //LOOP TIL ULTRALYDSSENSOR
        if (i == 750) {
        i = 0;
        dist = localUltrasonicSensor.getDistance();

        while (dist < 30) {
            mA.setPower(0);
            mC.setPower(0);
            mA.forward();
            mC.forward();
            dist = localUltrasonicSensor.getDistance();
            LCD.drawInt(dist,7,2);
                if (dist > 30) {
                break;  }
        }

        }

      //AKTIVERING AF FARVESENSOR
        if (g == 750) {
        g = 0;
        color = localColorSensor.getColorID();
            if (color == 0 || color == 2) {
                if (color == 0) {
                    LCD.drawString("RED-RED-RED-RED-RED-RED",0,1);
                }
                if (color == 2) {
                    LCD.drawString("BLUE-BLUE-BLUE-BLUE-BLUE",0,2);
                }
                //mA.setPower(0);
                //mC.setPower(0);
                //mA.forward();
                //mC.forward();
            }

        }

      //LINEFOLLOW
      LightValue = localLightSensor.readValue();
      error = LightValue - offset;
      integral = integral + error;
      derivative = error - lastError;


      LCD.clear();
      LCD.drawString("Error",0,0);LCD.drawInt(error,7,0);
      LCD.drawString("Light",0,1);LCD.drawInt(LightValue,7,1);
      LCD.drawString("Turn",0,2);LCD.drawInt(Turn,7,2);
      LCD.drawString("Dist",0,3);LCD.drawInt(dist,7,3);


                Turn = Kp * error + Ki * integral + Kd * derivative;
                Turn = Turn / 100000;
                PowerA = Tp + Turn;
                PowerC = Tp - Turn;


                if (PowerA > 0)
                {
                  mA.setPower(PowerA);
                  mA.forward();
                }
                else
                {
                  PowerA = PowerA * -3;
                  mA.setPower(PowerA);
                  mA.backward();
                }
                if (PowerC > 0)
                {
                  mC.setPower(PowerC);
                  mC.forward();
                }
                else
                {
                  PowerC = PowerC * -3;
                  mC.setPower(PowerC);
                  mC.backward();
                }

            lastError = error;
        }

  }
}