Test code for wheel encoders to calculate distance travelled by robot

Dependencies:   Motorslam QEI mbed

Fork of QEI_HelloWorld by Aaron Berk

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();
}