PIDライントレースロボット５ステアリング型へ変更

ある程度までPD型を考えてみたが、キャタピラ型でのPID制御に限界を感じたのでステアリング型へ変更。
キャタピラ型の問題点は回転するときに必ず曲がる方の出力を下げなければいけなくてロスしてしまうこと。
曲がったあとで自然に舵角を中心にもどすようにしたいがどのようにすればよいか不明。

task main()

{SetSensorColorRed(IN_2);

SetSensorTouch(IN_1);
SetSensorTouch(IN_4);

#define TIME 100

int standerd;

int POW;

int currentsensor;

int previoussensor;

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_LINE6,SENSOR_2);//ディスプレイにセンサー２の数値を表示
NumOut(80,LCD_LINE7,kp);//ディスプレイにkpの数値を表示
NumOut(80,LCD_LINE8,kd);//ディスプレイにkdの数値を表示

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

RotateMotor(OUT_C,60,10);

OnFwd(OUT_A,-40 );

Wait(TIME);}

else{

RotateMotor(OUT_C,60,-10);

OnFwd(OUT_A,-40 );

Wait(TIME);}

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

}}