Test code for wheel encoders to calculate distance travelled by robot
Dependencies: Motorslam QEI mbed
Fork of QEI_HelloWorld by
Diff: main.cpp
- Revision:
- 2:18287291c192
- Parent:
- 1:30696e4d196b
diff -r 30696e4d196b -r 18287291c192 main.cpp --- a/main.cpp Wed Aug 11 09:15:10 2010 +0000 +++ b/main.cpp Wed May 04 18:57:41 2016 +0000 @@ -1,16 +1,49 @@ +#include "Motorslam.h" #include "QEI.h" +#include "mbed.h" + +#define PULSES_PER_REV 24 +#define WHEEL_DIAMETER 42.00 //mm +#define CIRCUMFERENCE 131.95 //mm Serial pc(USBTX, USBRX); //Use X4 encoding. //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING); //Use X2 encoding by default. -QEI wheel (p29, p30, NC, 624); +QEI wheelLeft (p10, p9, NC, 24);//chanA, chan B +QEI wheelRight (p30, p29, NC, 24); +Motorslam motor( p21, p5, p6, p22, p8, p7, p18 ); //pwmLeft(A), leftIn1, leftIn2, pwmRight(B), rightIn1, rightIn2, standby + +float distance; +float fPwmPeriodLeft; +float fPwmPulsewidthLeft; +float fPwmPeriodRight; +float fPwmPulsewidthRight; int main() { + + fPwmPeriodLeft = 0.001f; // 1KHz + fPwmPulsewidthLeft = 0.50; // 49% duty cycle: slower, to make robot straight + fPwmPeriodRight = 0.001f; // 1KHz + fPwmPulsewidthRight = 0.50; // 51% duty cycle: faster, to make it straight + motor.setPwmLeftperiod(fPwmPeriodLeft); + motor.setPwmRightperiod(fPwmPeriodRight); + motor.setPwmLeftpulsewidth(fPwmPulsewidthLeft); + motor.setPwmRightpulsewidth(fPwmPulsewidthRight); - while(1){ - wait(0.1); - pc.printf("Pulses is: %i\n", wheel.getPulses()); + while(1) + { + motor.moveForward(); + + pc.printf("Pulses on left: %i\n", wheelLeft.getPulses());//LEFT + distance = ((wheelLeft.getPulses() / 24.00) * (131.95)); + pc.printf("distance on left: %f\n\n", distance); + + pc.printf("Pulses on right: %i\n", wheelRight.getPulses());//RIGHT + distance = ((wheelRight.getPulses() / 24.00) * (131.95)); + pc.printf("distance on right: %f\n\n", distance); + + wait(1); } - + //motor.moveStop(); }