t.korjakow
/
ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed modulo operator for position estimation from ticks

This commit is contained in:
Tim Korjakow 2023-07-10 23:07:49 +08:00
parent 0f25098175
commit a759cac0ae
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp
View File

@ -1,5 +1,10 @@
#include<MotorWheel.h>
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,