GyroBoy

This topic contains 2 replies, has 2 voices, and was last updated by Author Image Helmut Wunder 2 years, 9 months ago.

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

    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 ();
    
    			}
    		}
    #5271
    Author Image
    Helmut Wunder
    Participant

    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!
    #5272
    Author Image
    Helmut Wunder
    Participant

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

Viewing 3 posts - 1 through 3 (of 3 total)

You must be logged in to reply to this topic.

Posted in

Make a donation