ライントレースロボット ステアリング型 オンオフ制御

こちらが完成形ただし幅が２ｃｈのニチバン黒テープをあっさりまたぐ。
ETロボコンは幅が3chのラインで最小R半径は30ｃｈとかなりシビアな戦いをしている。

１単位あたりのタイムを50ミリSECに変えてもそれほど挙動は変わらないし、
このへんがNXTの限界なのか？コードが長過ぎて悪いから遅い反応しかできてないのか？
ううーむ曲がれない。

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 kp=1;

int ki;

int kd=1;

int P;

int I;

int D;

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

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

while(true){

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

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

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

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

if(Sensor(IN_1)==1){

kp = kp + 1 ;

}

if(Sensor(IN_4)==1){

kd = kd + 1 ;

}

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;

if(currentsensor<28){

steering=steering+20;//steering値更新steering値プラマイ30以上であるならそれ以上推移しない

if(steering >41 ){

steering = 40;}

RotateMotor(OUT_C,60,steering);//motorsyuturyoku

OnFwd(OUT_A,-25 );

Wait(TIME);

RotateMotor(OUT_C,60,-steering);
}

else{

steering=steering-20;//steering値更新

if(steering <-41 ){

steering = -40;}

RotateMotor(OUT_C,60,steering);

OnFwd(OUT_A,-25 );

Wait(TIME);

RotateMotor(OUT_C,60,-steering);
}

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

}}