キャタピラ型再挑戦
task main()
{SetSensorColorRed(IN_2);
SetSensorTouch(IN_1);
SetSensorTouch(IN_4);
#define TIME 50
int standerd=30;//変数standerdに白黒の中点を入力するこれは以後の出力の基準点となる
float POW;
int currentsensor;
int previoussensor;
float kp=1;
int ki;
float kd=0.5;
float P;
int I;
float D;
previoussensor=Sensor(IN_2);//エラーを防ぐためセンサー値を初期化してからループ
while(true){
NumOut(60,LCD_LINE6,SENSOR_2);//ディスプレイにセンサー2の数値を表示
NumOut(60,LCD_LINE7,kp);//ディスプレイにkpの数値を表示
NumOut(60,LCD_LINE8,kd);//ディスプレイにkdの数値を表示
if(Sensor(IN_1)==1){
kp = kp + 0.1 ;
}
if(Sensor(IN_4)==1){
kd = kd + 0.1 ;
}
currentsensor=Sensor(IN_2);//現在の光量を計測する
P=(currentsensor-standerd)*kp;
D=(currentsensor-previoussensor)*kd;
if( P < 0 ){
P = P ; }
if( D < 0 ){
D = D ; }
POW =P + D;
OnFwd(OUT_C,30 - POW);
OnFwd(OUT_A,30 + POW);
Wait(TIME);
previoussensor=currentsensor;//モータ出力後現在の値を過去の値に代入し塗り替える
}}