ステアリング型 舵角戻らないタイプ

かじを切ったあとに舵角を戻らないようにセットしてみたが謎の行動爆発を起こす。
舵を切りたい角度マイナス舵を切った角度＝舵をこれから切るべき角度のはずなんだけど
くるくるくるくる舵が回る
ううーむ、ドツボ。

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);

SetSensorTouch(IN_4);

#define TIME 50

int standerd;

int POW;

int currentsensor;

int previoussensor;

int steering =0;

int presteering =0;

int kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

standerd=20;//変数standerdに初期の光量を入力するこれは以後の出力の基準点となる

previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ

while(true){

currentsensor=Sensor(IN_2);//現在の光量を計測する

NumOut(80,LCD_LINE1,currentsensor*5);//ディスプレイにセンサー２の数値を表示

NumOut(80,LCD_LINE4,currentsensor);//ディスプレイにセンサー２の数値を表示

NumOut(80,LCD_LINE5,kp);//ディスプレイにkpの数値を表示

NumOut(80,LCD_LINE6,kd);//ディスプレイにkdの数値を表示

NumOut(80,LCD_LINE7,steering);//ディスプレイにsteeringの数値を表示

currentsensor=Sensor(IN_2);//現在の光量を計測する

P=(currentsensor-standerd)*kp;

D=(currentsensor-previoussensor)*kd;

if( P < 0 ){

P = 0 ; }

else{

P = P ;}

if( D < 0 ){

D = 0 ; }

else{

D = D ;}

POW =P - D;

steering = currentsensor-standerd;//ステアリング角は現センサー値と目標値との差

steering = steering - presteering;//今回ステアリング角度より前回ステアリング角をマイナスする。なお前回ステアリング値の初回値はゼロなので一回目のステアリングには影響しない。

if( steering < -40 ){

steering = -40 ; }

if( steering > 40 ){

steering = 40 ; }

RotateMotor(OUT_C,100,steering);//
presteering = steering ;//出力後今回ステアリング値を過去ステアリング角として保持する。

OnFwd(OUT_A,-25 );

Wait(TIME);

}

previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える

}}