main.cpp

00001 #include "Motorslam.h"
00002 #include "QEI.h"
00003 #include "mbed.h"
00004 
00005 #define PULSES_PER_REV        24
00006 #define WHEEL_DIAMETER        42.00 //mm
00007 #define CIRCUMFERENCE         131.95  //mm
00008 
00009 Serial pc(USBTX, USBRX);
00010 //Use X4 encoding.
00011 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
00012 //Use X2 encoding by default.
00013 QEI wheelLeft (p10, p9, NC, 24);//chanA, chan B
00014 QEI wheelRight (p30, p29, NC, 24);
00015 Motorslam motor( p21, p5, p6, p22, p8, p7, p18 ); //pwmLeft(A), leftIn1, leftIn2, pwmRight(B), rightIn1, rightIn2, standby
00016 
00017 float distance;
00018 float fPwmPeriodLeft;
00019 float fPwmPulsewidthLeft;
00020 float fPwmPeriodRight;
00021 float fPwmPulsewidthRight;
00022 
00023 int main() {
00024         
00025         fPwmPeriodLeft = 0.001f;        // 1KHz
00026         fPwmPulsewidthLeft = 0.50;      // 49% duty cycle: slower, to make robot straight
00027         fPwmPeriodRight = 0.001f;        // 1KHz
00028         fPwmPulsewidthRight = 0.50;      // 51% duty cycle: faster, to make it straight
00029         motor.setPwmLeftperiod(fPwmPeriodLeft);
00030         motor.setPwmRightperiod(fPwmPeriodRight);
00031         motor.setPwmLeftpulsewidth(fPwmPulsewidthLeft);
00032         motor.setPwmRightpulsewidth(fPwmPulsewidthRight);
00033 
00034     while(1)
00035     {
00036         motor.moveForward();
00037         
00038         pc.printf("Pulses on left: %i\n", wheelLeft.getPulses());//LEFT
00039         distance = ((wheelLeft.getPulses() /  24.00) * (131.95));
00040         pc.printf("distance on left: %f\n\n", distance);
00041         
00042         pc.printf("Pulses on right: %i\n", wheelRight.getPulses());//RIGHT
00043         distance = ((wheelRight.getPulses() /  24.00) * (131.95));
00044         pc.printf("distance on right: %f\n\n", distance);
00045         
00046         wait(1);
00047     }
00048         //motor.moveStop();
00049 }