Diff: DC_Motor_Controller.cpp

Revision:

9:49b59b308767

Child:

10:fe56f6800a72

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DC_Motor_Controller.cpp	Tue May 25 06:17:55 2021 +0000
@@ -0,0 +1,94 @@
+#include "DC_Motor_Controller.h"
+
+//************************************************//
+// COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE //
+//                                                //
+                #define DEBUG_MODE                //
+//                                                //
+//************************************************//
+
+
+void DC_Motor_Controller::goto_angle(int angle){      //  Move motor to specific angle from home point
+        
+    #ifdef DEBUG_MODE 
+    Serial device(USBTX, USBRX);  // tx, rx
+    device.baud(9600);
+    #endif
+
+    int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0);    //  Calculating target pulse number
+    int cur_pulse = dc_motor.getPulses();
+    int direction = 0;
+    
+    #ifdef DEBUG_MODE 
+    device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
+    #endif
+    
+    while ( tar_pulse != cur_pulse ){
+        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)
+        {
+            #ifdef DEBUG_MODE 
+            device.printf("GIVEUP ");
+            #endif
+            break;
+        }
+        
+        // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
+        else if (direction != out1) 
+        {
+            out1 = out2 = 0;
+            #ifdef DEBUG_MODE 
+            device.printf("PAUSE ");
+            #endif
+            wait(0.3); 
+        }
+      
+        out1 = direction;
+        out2 = !direction;
+        #ifdef DEBUG_MODE 
+        device.printf(direction ? "> " : "< ");
+        #endif
+        
+        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
+        
+        #ifdef DEBUG_MODE 
+        device.printf("%d ", cur_pulse);
+        #endif
+
+    }
+    #ifdef DEBUG_MODE 
+    device.printf("\n----- done -----\n");
+    #endif
+    out1 = 0;       //  Stop motor
+    out2 = 0;
+};
+    
+void DC_Motor_Controller::reset(){       // Setting home point for increment encoder
+    /*while (home_button == 0){       // Continue to turn clockwise until home button pressed
+        out1 = 1;
+        out2 = 0;
+    }*/
+    out1 = 0;
+    out2 = 0;
+    dc_motor.reset();       //  Reset pulse_
+    wait(0.3);
+};
+
+void DC_Motor_Controller::move_angle(int angle){      //  move for relative distance
+    reset();
+    goto_angle(angle);
+    reset();
+};
+
+void DC_Motor_Controller::set_out(int a, int b)
+{ 
+    out1 = a;
+    out2 = b;
+}
+
+int DC_Motor_Controller::get_pulse()
+{
+    return dc_motor.getPulses();
+}
\ No newline at end of file