倒立振子２

全然ダメ。ぴーぴー行って止まる。
#define GYRO_Offset 590
int status = 0;
int x = 0;
float P = 1;
float kp = 0;
float D = 0;
float kd = 0 ;
int syuturyokukakudo = 0;
int genzaikakudo = 90;

task main()
{
SetSensorHTGyro(S4);
int pow =0;

while(true){

status=SensorRaw(IN_4);
NumOut(60,LCD_LINE3,status);
genzaikakudo=genzaikakudo + (status-GYRO_Offset)/10;//100msごとの数値に直す。
syuturyokukakudo = 90 - genzaikakudo;
NumOut(60,LCD_LINE4,genzaikakudo);
RotateMotorEx(OUT_AC,20,syuturyokukakudo,0,false,false);

Wait(100);
}

}