DC motor PID position control

Dependencies:   QEI mbed

Fork of QEI_HelloWorld by Aaron Berk

Files at this revision

API Documentation at this revision

Comitter:
worasuchad
Date:
Wed Nov 07 17:05:37 2018 +0000
Parent:
1:30696e4d196b
Commit message:
DC motor PID position control

Changed in this revision

QEI.lib Show annotated file Show diff for this revision Revisions of this file
QEI_lib.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 30696e4d196b -r 916e20f507f2 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Nov 07 17:05:37 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
diff -r 30696e4d196b -r 916e20f507f2 QEI_lib.lib
--- a/QEI_lib.lib	Wed Aug 11 09:15:10 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/aberk/programs/QEI_lib/latest
\ No newline at end of file
diff -r 30696e4d196b -r 916e20f507f2 main.cpp
--- a/main.cpp	Wed Aug 11 09:15:10 2010 +0000
+++ b/main.cpp	Wed Nov 07 17:05:37 2018 +0000
@@ -1,16 +1,111 @@
-#include "QEI.h"
+ 
+/***************************< File comment >***************************/  
+/* project:   Lamborghini48                                           */ 
+/* project description : mobile robot class                           */ 
+/*--------------------------------------------------------------------*/ 
+/* version : 0.1                                                      */ 
+/* board : NUCLEO-F411RE                                              */ 
+/* detail : DC motor control                                          */
+/*--------------------------------------------------------------------*/ 
+/* create : 26/10/2018                                                */ 
+/* programmer : Worasuchad Haomachai                                  */ 
+/**********************************************************************/ 
+ 
+/*--------------------------------------------------------------------*/ 
+/*                           Include file                             */ 
+/*--------------------------------------------------------------------*/
+#include "math.h"
+#include "QEI.h"                        // https://os.mbed.com/cookbook/QEI
 
 Serial pc(USBTX, USBRX);
+
 //Use X4 encoding.
 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
 //Use X2 encoding by default.
-QEI wheel (p29, p30, NC, 624);
+QEI wheel (D2, D3, NC, 200, QEI::X4_ENCODING);
+
+/*--------------------------------------------------------------------*/ 
+/*                         Global variable                            */ 
+/*--------------------------------------------------------------------*/
+PwmOut pwm(D5);                         // this is the PWM pin for the motor for how much we move it to correct for its error
+DigitalOut dir1(D6);                    //these pins are to control the direction of the motor (clockwise/counter-clockwise)
+DigitalOut dir2(D7);
+
+double angle = 0;                       //set the angles
 
-int main() {
+double setpoint = 360;                  // 3072;        // I am setting it to move through 100 degrees
+double Kp = 0.05;                       // 0.015;       // you can set these constants however you like depending on trial & error
+double Ki = 0.001;                        // 0.000071;    // 0.000109;
+double Kd = 0.0;                        // 0.93866;     // 0.1009;
+
+float last_error = 0;
+float _error = 0;
+float changeError = 0;
+float totalError = 0;
+float pidTerm = 0;
+float pidTerm_scaled = 0;               // if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
 
-    while(1){
-        wait(0.1);
-        pc.printf("Pulses is: %i\n", wheel.getPulses());
+/*--------------------------------------------------------------------*/ 
+/*                           prototype fun                            */ 
+/*--------------------------------------------------------------------*/
+void PIDcalculation();              // find PID value
+
+/*--------------------------------------------------------------------*/ 
+/*                              main                                  */ 
+/*--------------------------------------------------------------------*/   
+int main() {
+    pc.baud(115200);
+    while(1)
+    {
+        PIDcalculation();               // find PID value
+        
+        if (angle < setpoint) 
+        {
+            dir1 = 1;                   // Forward motion
+            dir2 = 0;
+        } 
+        else 
+        {
+            dir1 = 0;                   // Reverse motion
+            dir2 = 1;
+        }
+    
+        pwm.write(pidTerm_scaled / 255.00f);
+        
+        // Serial.println("WHEEL ANGLE:");
+        pc.printf("%.3f\t\t", setpoint);
+        pc.printf("%.3f\n\r", angle);
+        //pc.printf("%d\n\r", wheel.getPulses());
     }
 
 }
+/*--------------------------------------------------------------------*/ 
+/*                              PID                                   */ 
+/*--------------------------------------------------------------------*/ 
+void PIDcalculation(){
+    /* 
+    *  angle = ( 0.1171875 * count); // count to angle conversion = 360/(64*12*4)
+    *  reduction ratio: 64:1 , pulses per revolution: 12CPR
+    */
+    
+    angle = ( 0.004591 * wheel.getPulses());        // 360/(98*200*4)
+    _error = setpoint - angle;
+    
+    changeError = _error - last_error;              //derivative term
+    totalError += _error;                           //accumalate errors to find integral term
+    pidTerm = (Kp * _error) + (Ki * totalError) + (Kd * changeError);//total gain
+    
+    // pidTerm = constrain(pidTerm, -255, 255);     //constraining to appropriate value
+    if(pidTerm > 255)
+    {
+      pidTerm = 255;
+    }
+    else if(pidTerm < -255)
+    {
+      pidTerm = -255;  
+    }
+    
+    pidTerm_scaled = abs(pidTerm);                  //make sure it's a positive value
+    
+    last_error = _error;
+}
diff -r 30696e4d196b -r 916e20f507f2 mbed.bld
--- a/mbed.bld	Wed Aug 11 09:15:10 2010 +0000
+++ b/mbed.bld	Wed Nov 07 17:05:37 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file