# GyroBoy

Viewing 3 posts - 1 through 3 (of 3 total)
• Author
Posts
• #5269

Hello guys!

I have done some code for my GyroBoy, but unfortunately it will not work.

Can anybody give me a hint? Please do not show me the solution, I would like to finish it on my own.

Thanks

``````		public static void Main (string[] args)
{
EV3GyroSensor sensor = new EV3GyroSensor (SensorPort.In1);
sensor.Reset ();
sensor.Mode = GyroMode.Angle;

EV3TouchSensor touch = new EV3TouchSensor (SensorPort.In3);

Motor motorA = new Motor (MotorPort.OutA);
Motor motorD = new Motor (MotorPort.OutD);
motorA.ResetTacho ();
motorA.SetPower (0);
motorD.ResetTacho ();
motorD.SetPower (0);

double angle;
double kp = 300;
double ki = 0.2;
double kd = 100;
double scale = 10;
double power;
double integral = 0;
double derivative = 0;
double last_delta = 0;
int dt = 20;

integral = integral + (angle * dt);
derivative = (angle - last_delta) / dt;

power = (kp * angle + ki * integral + kd * derivative) / scale;

if (power > 90) {
power = 90;
}

if (power < -90) {
power = -90;
}

motorA.SetPower (Convert.ToSByte (power));
motorD.SetPower (Convert.ToSByte (power));

LcdConsole.WriteLine (Convert.ToString (angle));

last_delta = angle;

}

motorA.Brake ();
motorD.Brake ();

}
}``````
#5271

Dude, as I already wrote you in our German forum, you will …

1st of all need to stop the correct loop time which surely is different from 20 (ms) !
`dt= currTime- saveTime;`

2nd,
where do you calculate the error (err) – maybe I missed it?
integral = integral + (angle * dt);
???
the correct PID formulas are the following:

``````err = Sollwert-Zielwert
Integr = Integr + err
PWMpwr= (KP*err) + (KI*integr)*dt + (KD*(err-errorold))/dt;``````

and 3rd,
then you will finally surely need to tune (and fine-tune) your PID constants!

a hint for your PID regulation:
the loop propably will have to look something like the following:

``````loop:
dt= currTime- saveTime;
saveTime= currTime
err = Sollwert-Zielwert
Integr berechnen (Integr = Integr + err)
PWMpwr= (KP*err) + (KI*integr)*dt + (KD*(err-errorold))/dt;
errorold=err
PWMpwr auf Motoren anwenden
dann wieder loop von vorn!``````
#5272 