Diff: motor/Motor.cpp

Revision:

11:35809512ec11

Parent:

6:858a5116688e

Child:

12:817da876ae2f

diff -r be9de79cf6b0 -r 35809512ec11 motor/Motor.cpp
--- a/motor/Motor.cpp	Tue Nov 12 13:55:17 2019 +0000
+++ b/motor/Motor.cpp	Wed Nov 13 15:59:06 2019 +0000
@@ -1,12 +1,13 @@
 #include "mbed.h"
 
 #include "Motor.h"
+#include "QEI.h"
 
 ///////////////////////////////////////////////////////////////////
 // Constructor
 ///////////////////////////////////////////////////////////////////
-Motor::Motor(PinName in1, PinName in2, PinName pwm)
-: in1(in1), in2(in2), pwm(pwm) 
+Motor::Motor(PinName in1, PinName in2, PinName pwm, QEI wheel)
+: in1(in1), in2(in2), pwm(pwm), wheel(wheel)
 {
     this->pwm.period_ms(10);
 }
@@ -15,19 +16,62 @@
 // Public methods
 ///////////////////////////////////////////////////////////////////
 
-void Motor::moveForward()
+void Motor::moveForward(int pulse)
 {
-    in1 = 1; 
-    in2 = 0;    
+    moveDistance(-0.5, pulse, 1); 
 }
 
-void Motor::moveBackward()
-{
-    in1 = 0; 
-    in2 = 1;
+void Motor::moveBackward(int pulse)
+{  
+    moveDistance(0.5, pulse, -1);
+}
+
+void Motor::moveLeft(float speed)
+{  
+    moveDistance(speed, 3000, -1);
+}
+
+void Motor::moveRight(float speed)
+{  
+    moveDistance(speed, 3000, 1);
 }
 
 void Motor::setSpeed(float speed)
 {
     pwm.write(speed);
 }
+
+void Motor::moveDistance(float speed, int pulse, int pulse_direction)
+{
+    // Variables to allow for any pulse reading   
+    float initial_pulse_reading = wheel.getPulses();
+    float current_pulse_reading = wheel.getPulses();
+    
+    // - is forward on our robot
+    setSpeed(speed);
+    
+    // loop for moving forward
+    if (pulse_direction > 0)
+    {
+        while(current_pulse_reading <= (initial_pulse_reading + pulse * pulse_direction))
+        {
+            current_pulse_reading = wheel.getPulses();
+        }
+    }
+    else
+    {
+        while(current_pulse_reading >= (initial_pulse_reading + pulse * pulse_direction))
+        {
+            current_pulse_reading = wheel.getPulses();
+        }
+    }        
+
+    setSpeed(0);
+}
+
+
+void Motor::move(float speed)
+{
+    // - is forward on our robot
+    setSpeed(speed);    
+}
\ No newline at end of file