Test code for wheel encoders to calculate distance travelled by robot

Dependencies:   Motorslam QEI mbed

Fork of QEI_HelloWorld by Aaron Berk

Files at this revision

API Documentation at this revision

Comitter:
aminomar
Date:
Wed May 04 18:57:41 2016 +0000
Parent:
1:30696e4d196b
Commit message:
Test code for wheel encoder to calculate distance travelled by robot;

Changed in this revision

Motorslam.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
QEI_lib.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 30696e4d196b -r 18287291c192 Motorslam.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motorslam.lib	Wed May 04 18:57:41 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/aminomar/code/Motorslam/#550e01736339
diff -r 30696e4d196b -r 18287291c192 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed May 04 18:57:41 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/aminomar/code/QEI/#5f28ceec8280
diff -r 30696e4d196b -r 18287291c192 QEI_lib.lib
--- a/QEI_lib.lib	Wed Aug 11 09:15:10 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/aberk/programs/QEI_lib/latest
\ No newline at end of file
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();
 }