jakob.kampichler/libcreate
jakob.kampichler/libcreate
1
0
Fork 0
forked from compAIR/libcreate
Code Pull requests Activity

fixed encoder overflow

Browse source
This commit is contained in:
DuSack1220 2023-03-13 23:49:10 +01:00 • committed by Konstantin Lampalzer
parent fd2638114a
commit 1bc2d768f7
1 changed files with 12 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/create.cpp
View file

@ -156,17 +156,23 @@ namespace create {
// Compute ticks since last update // Compute ticks since last update
int ticksLeft = totalTicksLeft - prevTicksLeft; int ticksLeft = totalTicksLeft - prevTicksLeft;
int ticksRight = totalTicksRight - prevTicksRight; int ticksRight = totalTicksRight - prevTicksRight;
prevTicksLeft = totalTicksLeft;
prevTicksRight = totalTicksRight;
// Handle wrap around // Handle wrap around
if (std::abs(ticksLeft) > 0.9 * util::V_3_MAX_ENCODER_TICKS) { if (ticksLeft > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
ticksLeft = (ticksLeft % util::V_3_MAX_ENCODER_TICKS) + 1; 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;
} }
if (std::abs(ticksRight) > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
ticksRight = (ticksRight % util::V_3_MAX_ENCODER_TICKS) + 1; 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;
} }
prevTicksLeft = totalTicksLeft;
prevTicksRight = totalTicksRight;
// Compute distance travelled by each wheel // Compute distance travelled by each wheel
leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV) leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV)
* model.getWheelDiameter() * util::PI; * model.getWheelDiameter() * util::PI;