Sensorimpulse zu Geschwindigkeit

guten Tag,

mein Problem mit Sensorimpulsen ist leider noch nicht gelöst, deswegen brauche ich bitte Eure Vorschläge.
also im anhang füge ein Teil meines Pogramms, wo ich aus der encoderimpulsen Geschwindigkeiten berechnen möchte.
Es geht, um den omnidirektionalen antrieb, wie es schon bekannt ist, die Rädr(3 stück) sind 120° zueinander angeordnet.
die Encoder liefern mir impulse, ich krieg max 1 impuls pro Schritt(Berechnung)
aus dem Impuls muss ich Geschwindigkeit berechnen, die wiederum benutze ich in der vorwärtskinematik, daraus bekomme ich Geschwindigkeiten in der X Y Achse und -drehung.
danach muss ich diese V integrieren-->ich bekomme Position x, y und Orientierung.

das theoretisch sehr schön, praktisch macht der roboter falsche Bewegungen.

ich hab auch probiert, die impulse zu sammeln, also längere Zeit. das ist aber das gleiche,


Unten ein Teil vom Code.
ich warte auf Eure Vorschläge, Danke
gruß


//PWM-Motor ausgeben
control(1);
control(2);
control(3);

//Geschwindigkeit=Differnz der Impulse
velocity.v1_ist= M1_POS-Pos_count_alt_1;
velocity.v2_ist= M2_POS-Pos_count_alt_2;
velocity.v3_ist= M3_POS-Pos_count_alt_3;

//Ist-Position merken
Pos_count_alt_1= M1_POS;
Pos_count_alt_2= M2_POS;
Pos_count_alt_3= M3_POS;

//
vorwaerts_kinematik(velocity.v1_ist,velocity.v2_is t, velocity.v3_ist, &velocity.vx_ist, &velocity.vy_ist, &velocity.vw_ist, velocity.winkel);

velocity.x_ist+=velocity.vx_ist;
velocity.y_ist+=velocity.vy_ist;
velocity.o_ist=velocity.o_temp;