Actually fixed wrap around

This commit is contained in:
Bernhard Klauninger 2023-03-15 13:14:06 +01:00 committed by Konstantin Lampalzer
parent 1bc2d768f7
commit 2c58777e45

View file

@ -159,15 +159,15 @@ namespace create {
// Handle wrap around
if (ticksLeft > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
ticksLeft = 2 * util::V_3_MAX_ENCODER_TICKS + prevTicksLeft - ticksLeft;
} else if (ticksLeft < 0.9 * -util::V_3_MAX_ENCODER_TICKS) {
ticksLeft = util::V_3_MAX_ENCODER_TICKS - prevTicksLeft + util::V_3_MAX_ENCODER_TICKS + ticksLeft;
ticksLeft -= util::V_3_MAX_ENCODER_TICKS;
} else if (ticksLeft < -0.9 * util::V_3_MAX_ENCODER_TICKS) {
ticksLeft += util::V_3_MAX_ENCODER_TICKS;
}
if (ticksRight > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
ticksRight = util::V_3_MAX_ENCODER_TICKS + prevTicksRight + util::V_3_MAX_ENCODER_TICKS - ticksRight;
} else if (ticksLeft < 0.9 * -util::V_3_MAX_ENCODER_TICKS) {
ticksRight = util::V_3_MAX_ENCODER_TICKS - prevTicksRight + util::V_3_MAX_ENCODER_TICKS + ticksRight;
ticksRight -= util::V_3_MAX_ENCODER_TICKS;
} else if (ticksRight < -0.9 * util::V_3_MAX_ENCODER_TICKS) {
ticksRight += util::V_3_MAX_ENCODER_TICKS;
}
prevTicksLeft = totalTicksLeft;