Diff: DC_Motor_Controller.cpp

Revision:

10:fe56f6800a72

Parent:

9:49b59b308767

Child:

11:e880912260b5

diff -r 49b59b308767 -r fe56f6800a72 DC_Motor_Controller.cpp
--- a/DC_Motor_Controller.cpp	Tue May 25 06:17:55 2021 +0000
+++ b/DC_Motor_Controller.cpp	Tue May 25 07:24:10 2021 +0000
@@ -7,19 +7,21 @@
 //                                                //
 //************************************************//
 
+DC_Motor_Controller::DC_Motor_Controller(PinName out_M1, PinName out_M2, PinName in_A, PinName in_B, int PPR) : 
+            out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR)
+{
+    out1.period(0.002);
+    out2.period(0.002);
+}
 
-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
+void DC_Motor_Controller::goto_pulse(int tar_pulse, float speed){      //  Move motor to specific angle from home point
     int cur_pulse = dc_motor.getPulses();
     int direction = 0;
     
     #ifdef DEBUG_MODE 
+    Serial device(USBTX, USBRX);  // tx, rx
+    device.baud(9600);
     device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
     #endif
     
@@ -27,7 +29,7 @@
         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)
+        if(abs(tar_pulse - cur_pulse) < 10)
         {
             #ifdef DEBUG_MODE 
             device.printf("GIVEUP ");
@@ -36,7 +38,7 @@
         }
         
         // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
-        else if (direction != out1) 
+        else if (direction != (out1 > 0)) 
         {
             out1 = out2 = 0;
             #ifdef DEBUG_MODE 
@@ -45,11 +47,8 @@
             wait(0.3); 
         }
       
-        out1 = direction;
-        out2 = !direction;
-        #ifdef DEBUG_MODE 
-        device.printf(direction ? "> " : "< ");
-        #endif
+        out1 = direction * speed;
+        out2 = !direction * speed;
         
         cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
         
@@ -64,25 +63,26 @@
     out1 = 0;       //  Stop motor
     out2 = 0;
 };
+
+
+void DC_Motor_Controller::goto_angle(int angle, float speed){      //  Move motor to specific angle from home point
+        
+    goto_pulse(((double) angle / 360.0)* (2.0 * 792.0), speed);    //  Calculating target pulse number
+};
     
 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::move_angle(int angle, float speed){      //  move for relative distance
+    
+    goto_pulse(get_pulse() + ((double) angle / 360.0)* (2.0 * 792.0), speed); 
 };
 
-void DC_Motor_Controller::set_out(int a, int b)
+void DC_Motor_Controller::set_out(float a, float b)
 { 
     out1 = a;
     out2 = b;