PDI regulator

[Muffinman]

Hi, im trying to make a ball to stay in the middle of a track with an ultrasonic sensor and a motor.
im trying to programm the motor with java, eclipse.
im getting faulty measures when the ball is to close or to far away from the sensor so i have tried to tell it to regulate in a smaller fasion even with the faulty measurments.
it doesnt work at al and if you could find any problems in my programming or if u have any tips it would be welcomed!
import lejos.nxt.*;
public class PDI {

/**
* @param args
*/
public static void main(String[] args) {
// TODO Auto-generated method stub


UltrasonicSensor Sonic = new UltrasonicSensor(SensorPort.S1);
int är = Sonic.getDistance();
int bör = 25;
int fel_int=0;
float reglerfel = (är-bör);
float Kp=-1;
float är_long=0;
for (int i=0; i <2;i++){
är_long=är_long+Sonic.getDistance();
}
är_long=är_long/2;
Motor.A.resetTachoCount();
while (!Button.LEFT.isPressed()) {
LCD.clear();
LCD.drawString(är_long+" ", 3,5);
är_long=(float) (0.5*är_long + 0.5*Sonic.getDistance());
if (!(Sonic.getDistance()==255))
reglerfel = är_long-bör;
else
if(reglerfel<0)
Motor.A.rotateTo(5);
else
Motor.A.rotateTo(-5);


fel_int=Math.round(Kp*reglerfel);
LCD.drawInt(Sonic.getDistance(), 2, 2);

LCD.drawString("fel´: "+fel_int, 3, 3);
LCD.drawString(reglerfel+"regler", 3, 4);
Button.waitForPress(10);



if (Sonic.getDistance()!=255)
if ((fel_int > -5) & (fel_int < 10))
Motor.A.rotateTo(fel_int);
else
if ((fel_int < -5) & !(Sonic.getDistance()==255))
Motor.A.rotateTo(5);
else
Motor.A.rotateTo(-5);


}
}
}