PDI regulator

[Muffinman]

hi, i have built a track with a ultrasonic sensor at one end, the track is held by a motor which can tilt the track so that the ball which i put on the track can roll in differant directions.
my task is to program the motor and the sensor so that the motor and sensor will regulate the tilt and make the ball stick in the middle of the track.
we get some faulty measures from the sensor(sometimes show that its 255 cm away while it should be around 30 or like under 5 cm) so i have tried to program it to ignore those measures and tilt it a smaller value against the right direction.
it doesnt work at all like i want and it would be nice if u could give me some feedback and tips on my programming.
im using eclipse with the lejos plugin to program.

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);


}
}
}