Home→Forums→MonoBrick EV3 Firmware→GyroBoy
This topic contains 2 replies, has 2 voices, and was last updated by Helmut Wunder 2 years, 9 months ago.

AuthorPosts

February 24, 2015 at 18:28 #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; 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 (); } }
February 25, 2015 at 11:15 #5271Dude, 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 = SollwertZielwert Integr = Integr + err PWMpwr= (KP*err) + (KI*integr)*dt + (KD*(errerrorold))/dt;
and 3rd,
then you will finally surely need to tune (and finetune) 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 = SollwertZielwert Integr berechnen (Integr = Integr + err) PWMpwr= (KP*err) + (KI*integr)*dt + (KD*(errerrorold))/dt; errorold=err PWMpwr auf Motoren anwenden dann wieder loop von vorn!
February 26, 2015 at 09:45 #5272just a typo about err:
err = SollwertZielwert is wrong of course, it should have been
err = SollwertIstwert (= targetValcurrentVal) 
AuthorPosts
You must be logged in to reply to this topic.
Follow