Test code for wheel encoders to calculate distance travelled by robot
Dependencies: Motorslam QEI mbed
Fork of QEI_HelloWorld by
main.cpp
- Committer:
- aminomar
- Date:
- 2016-05-04
- Revision:
- 2:18287291c192
- Parent:
- 1:30696e4d196b
File content as of revision 2:18287291c192:
#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 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) { 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(); }