Diff: DC_Motor_Controller.cpp

Revision:

13:675456f1f401

Parent:

11:e880912260b5

Child:

14:c9611cf036ad

--- a/DC_Motor_Controller.cpp	Wed May 26 07:43:28 2021 +0000
+++ b/DC_Motor_Controller.cpp	Thu May 27 05:43:48 2021 +0000
@@ -3,19 +3,13 @@
 //************************************************//
 // COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE //
 //                                                //
-//                #define DEBUG_MODE                //
+                #define DEBUG_MODE                //
 //                                                //
 //************************************************//
 
-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_pulse(int tar_pulse, float speed){      //  Move motor to specific angle from home point
+void DC_Motor_On_Off::goto_pulse(int tar_pulse, float speed){      //  Move motor to specific angle from home point
     int cur_pulse = dc_motor.getPulses();
     int direction = 0;
     
@@ -53,7 +47,7 @@
         cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
         
         #ifdef DEBUG_MODE 
-        device.printf("%d ", cur_pulse);
+        device.printf("[%d] ", cur_pulse);
         #endif
 
     }
@@ -64,6 +58,71 @@
     out2 = 0;
 };
 
+void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd)
+{
+    #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);
+}
+
+
+void DC_Motor_PID::goto_pulse(int tar_pulse, float placeholder){      //  Move motor to specific angle from home point
+    int cur_pulse = dc_motor.getPulses();
+    
+    #ifdef DEBUG_MODE 
+    Serial device(USBTX, USBRX);  // tx, rx
+    device.baud(9600);
+    device.printf("\n------ PID current %d -> target %d -----\n", cur_pulse, tar_pulse);
+    #endif
+    
+    while ( tar_pulse != cur_pulse ){
+        double speed = this->pid->calculate(tar_pulse,cur_pulse);
+        
+        #ifdef DEBUG_MODE 
+        if(!this->pid)
+            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)
+        {
+            #ifdef DEBUG_MODE 
+            device.printf("GIVEUP ");
+            #endif
+            break;
+        }
+      
+        out1 = speed > 0 ? speed : 0;
+        out2 = speed > 0 ? 0 : -speed;
+        
+        #ifdef DEBUG_MODE 
+        device.printf("%.3f ", speed);
+        #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");
+    #endif
+    out1 = 0;       //  Stop motor
+    out2 = 0;
+};
+
+
+
+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, float speed){      //  Move motor to specific angle from home point