Diff: DC_Motor_Controller.h

Revision:

7:6e59ed00a6a9

Parent:

5:c040faf21e07

Child:

8:703502486434

--- a/DC_Motor_Controller.h	Wed May 19 08:26:34 2021 +0000
+++ b/DC_Motor_Controller.h	Mon May 24 08:17:09 2021 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "QEI.h"
+#include <cmath>
 
 #ifndef DC_MOTOR_CONTROLLER_H
 #define DC_MOTOR_CONTROLLER_H
@@ -11,38 +12,60 @@
         DigitalOut out1, out2;    //  Motor direction control pin 2
         QEI dc_motor;    //(D8,D7,NC,792);    // Create QEI object for increment encoder
         void goto_angle(int angle){      //  Move motor to specific angle from home point
+            
+            //Serial device(USBTX, USBRX);  // tx, rx
+            //device.baud(19200);
+        
             int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0);    //  Calculating target pulse number
             int cur_pulse = dc_motor.getPulses();
+            int direction = 0;
             
+            //device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
             while ( tar_pulse != cur_pulse ){
-        
-                if (tar_pulse > cur_pulse){     //  Rotate motor clockwise
-                    out1 = 1;
-                    out2 = 0;
+                direction = tar_pulse > cur_pulse; // cw 1 ; acw 0
+              
+                // if margin of error is less than +-3 (+- 1 degree), give up 
+                if(abs(tar_pulse - cur_pulse) < 3)
+                {
+                    //device.printf("GIVEUP ");
+                    break;
                 }
-                else {                          //  Rotate motor counter clockwrise 
-                    out1 = 0;
-                    out2 = 1;
+                
+                // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
+                else if (direction != out1) 
+                {
+                    out1 = out2 = 0;
+                    //device.printf("PAUSE ");
+                    wait(0.3); 
                 }
+              
+                out1 = direction;
+                out2 = !direction;
+                //device.printf(direction ? "> " : "< ");
+                
                 cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
+                
+                //device.printf("%d ", cur_pulse);
+    
             }
+            //device.printf("\n----- done -----\n");
             out1 = 0;       //  Stop motor
             out2 = 0;
         };
     
     public:
-        DC_Motor_Controller(PinName out1_p, PinName out2_p, PinName in1_p,  PinName in2_p, int PPR) : out1(out1_p), out2(out2_p), dc_motor(in1_p, in2_p, NC, PPR)
-        {
-            }
+        DC_Motor_Controller(PinName out1_p, PinName out2_p, PinName in1_p,  PinName in2_p, int PPR) : 
+            out1(out1_p), out2(out2_p), dc_motor(in1_p, in2_p, NC, PPR){}
         
         void reset(){       // Setting home point for increment encoder
             /*while (home_button == 0){       // Continue to turn clockwise until home button pressed
                 out1 = 1;
                 out2 = 0;
             }*/
-            out1 = out2 = 0;
+            out1 = 0;
+            out2 = 0;
             dc_motor.reset();       //  Reset pulse_
-            //wait(1);
+            wait(0.3);
          };
          
         void move_angle(int angle){      //  move for relative distance
@@ -50,6 +73,15 @@
             goto_angle(angle);
             reset();
         };
-
+        
+        void set_out(int a, int b)
+        { 
+            out1 = a;
+            out2 = b;
+        }
+        int get_pulse()
+        {
+            return dc_motor.getPulses();
+        }
 };
 #endif
\ No newline at end of file