Diff: main.cpp

Revision:

2:18287291c192

Parent:

1:30696e4d196b

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