倒立振子へ戻って

#define GYRO_Offset 590
int status = 0;
int x = 0;
float P = 1;
float kp = 0;
float D = 0;
float kd = 0;

int kakudo = 0;
int furuikakudo = 0;
int genzaikakudo　＝90;
int furuikakudo =0;
task main()
{

SetSensorHTGyro(S4);
int pow =0;
while(true){
if(ButtonPressed(BTNLEFT, false)){

P = P + 0.1 ;

}

if(ButtonPressed(BTNRIGHT, false)){

kp = kp + 0.1 ;
}

status=SensorRaw(IN_4);
NumOut(60,LCD_LINE3,status);
kakudo= (status-GYRO_Offset)/10;//100msごとの数値に直す。
P=(kakudo)*kp;
D=(kakudo-furuikakudo)*kd;
pow = P + D;
NumOut(60,LCD_LINE4,pow);
RotateMotorEx(OUT_AC,20,pow,0,false,false);

Wait(100);
furuikakudo=kakudo;
genzaikakudo(90)+kakkudo
POW=genzaikakudo-kakudo
}

}