GyroBoy

[nebelmichael]

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;

			while (touch.Read() == 0) {

				angle = sensor.Read ();

				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;

				System.Threading.Thread.Sleep (dt);

				}

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

			}
		}

[Helmut Wunder]

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?
then, what about this line:
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!

[Helmut Wunder]

just a typo about err:
err = Sollwert-Zielwert is wrong of course, it should have been
err = Sollwert-Istwert (= targetVal-currentVal)

[Author]

Posts