Diff: DC_Motor_Controller.cpp

Revision:

15:87df75ee8731

Parent:

14:c9611cf036ad

Child:

16:3a1b95e2ecb8

--- a/DC_Motor_Controller.cpp	Thu May 27 07:32:49 2021 +0000
+++ b/DC_Motor_Controller.cpp	Mon May 31 09:44:13 2021 +0000
@@ -3,7 +3,7 @@
 //************************************************//
 // COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE //
 //                                                //
-//                #define DEBUG_MODE                //
+                #define DEBUG_MODE                //
 //                                                //
 //************************************************//
 
@@ -47,7 +47,7 @@
         cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
         
         #ifdef DEBUG_MODE 
-        device.printf("[%d] ", cur_pulse);
+        device.printf("Pulse:%d\n", cur_pulse);
         #endif
 
     }
@@ -58,14 +58,14 @@
     out2 = 0;
 };
 
-void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd)
+void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd, double min = 0.05)
 {
     #ifdef DEBUG_MODE 
     Serial device(USBTX, USBRX);  // tx, rx
     device.baud(9600);
     device.printf("Kp: %.3f, Ki: %.3f, Kd: %.3f", Kp, Ki, Kd);
     #endif
-    this->pid=new PID(0.01,1,0.05,Kp,Kd,Ki);
+    this->pid=new PID(0.01,1,min,Kp,Kd,Ki);
 }
 
 
@@ -79,6 +79,8 @@
     #endif
     
     while ( tar_pulse != cur_pulse ){
+        
+        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
         double speed = this->pid->calculate(tar_pulse,cur_pulse);
         
         #ifdef DEBUG_MODE 
@@ -86,7 +88,7 @@
             device.printf("PID NOT SET BUT CALLED! NULL POINTER ");
         #endif
         // if margin of error is less than +-3 (+- 1 degree), give up 
-        if(abs(tar_pulse - cur_pulse) < 0)
+        if(abs(tar_pulse - cur_pulse) < 2)
         {
             #ifdef DEBUG_MODE 
             device.printf("GIVEUP ");
@@ -97,16 +99,12 @@
         out1 = speed > 0 ? speed : 0;
         out2 = speed > 0 ? 0 : -speed;
         
+        
         #ifdef DEBUG_MODE 
-        device.printf("%.3f ", speed);
+        device.printf("Speed:%.0f ", speed*1000);
+        device.printf("Pulse:%d \n", cur_pulse);
         #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----- PID done -----\n");
@@ -121,8 +119,8 @@
             out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR)
 {
     _ppr = PPR;
-    out1.period(0.002);
-    out2.period(0.002);
+    out1.period(0.005);
+    out2.period(0.005);
 }
 
 void DC_Motor_Controller::goto_angle(int angle, float speed){      //  Move motor to specific angle from home point