From a759cac0ae3bfd8b01f2d571b6d4d8efa27c643a Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Mon, 10 Jul 2023 23:07:49 +0800 Subject: [PATCH] Fixed modulo operator for position estimation from ticks --- src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp b/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp index 64ec31b..76f235d 100644 --- a/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp +++ b/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp @@ -1,5 +1,10 @@ #include +inline int modulo(int a, int b) { + const int result = a % b; + return result >= 0 ? result : result + b; +} + Motor::Motor(unsigned char _pinPWM,unsigned char _pinDir, unsigned char _pinIRQ,unsigned char _pinIRQB, struct ISRVars* _isr) @@ -323,7 +328,7 @@ float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM) { float GearedMotor::getPosition() { debug(); int ticks_per_rev = getRatio() * CPR; - return (float)(Motor::getCurrPulse() % ticks_per_rev)/ticks_per_rev; + return ((float)modulo(Motor::getCurrPulse(), ticks_per_rev))/ticks_per_rev; } MotorWheel::MotorWheel(unsigned char _pinPWM,unsigned char _pinDir,