Diff: MD_PID.cpp

Revision:

0:fe6ec67653f4

Child:

2:5b2331251b3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MD_PID.cpp	Thu Aug 18 06:36:14 2016 +0000
@@ -0,0 +1,132 @@
+#include "mbed.h"
+#include "MD_PID.h"
+#include "QEI.h"
+#include "MD.h"
+
+MD_PID::MD_PID()
+{
+    ref1 = 0;   
+}
+
+MD_PID::MD_PID(MD *md_, double kp_, double ki_, double kd_, int pulse_, QEI *qei_)
+{
+    md = md_;
+    qei = qei_;
+    
+    ref1 = 0;
+    kp = kp_;
+    ki = ki_;
+    kd = kd_;
+    pulse = pulse_;
+    
+    Reset();
+}
+
+double MD_PID::Drive(double ref)
+{
+    /*
+    if(ref1 != ref)
+    {
+        Reset();
+        ref1 = ref;
+    }
+    */
+    if(ref == 0)
+    {
+        duty = 0;
+        md->rotate(duty);
+        Read_Speed();
+    }
+    else
+    {
+        if(kp >= 1.0){
+            duty = ref / kp;
+            Read_Speed();
+        }
+        else
+            duty -= PID(ref);
+            
+        if(duty > 1.0)
+            duty = 1.0;
+        else if(duty < -1.0)
+            duty = -1.0;
+    }
+    
+    if( ( (ref >= 0) ^ (Speed() >= 0) ) && fabs(Speed()) > 0.01)
+        duty = 0;
+    
+    md->rotate(duty);
+    return duty;
+}
+
+double MD_PID::PID(double target)
+{
+    diff[0] = diff[1];
+    //delta = timer.read();
+    //qs = (double)(qei->Read()) / delta / (double)pulse ;
+    //timer.reset();
+    //qei->Reset();
+    Read_Speed();
+    diff[1] = qs - target;
+    integral += ((diff[0] + diff[1]) / 2.0) * delta;
+    p = kp * diff[1];
+    i = ki * integral;
+    d = kd * (diff[1] - diff[0]);
+    return p + i + d;
+}
+
+void MD_PID::Read_Speed()
+{
+    delta = timer.read();
+    qs = (double)(qei->Read()) / delta / (double)pulse ;
+    timer.reset();
+    qei->Reset();
+}
+
+double MD_PID::Speed()
+{
+   return qs;
+}
+
+void MD_PID::Reset()
+{
+    qs = 0;
+    diff[0] = 0;
+    diff[1] = 0.0;
+    integral = 0.0;
+    timer.reset();
+    timer.start();
+    qei->Reset();
+}
+
+void MD_PID::PID_Step(double interval)
+{
+    double time[200],speed[200];
+    Timer ST;
+    ST.reset();
+    ST.start();
+    md->rotate(-1);
+    for(int i = 0; i < 200; i++){
+        PID(0);
+        time[i] = ST.read(); 
+        speed[i] = Speed();
+        wait(interval);
+    }
+    md->rotate(0);
+    for(int i = 0; i < 200; i++){
+        printf("%f\t%f\r\n",time[i],speed[i]);
+    }
+}
+
+MD_PID* Create_MD_PID(  int number, int i2c_addr, I2C *i2c,             // QEI
+                        double kp_, double ki_, double kd_, int pulse_, // PID
+                        PinName pwm, PinName dere)                      // MD
+{
+    QEI *qei = new QEI(number, i2c_addr, i2c);
+    printf("qei\r\n");
+    MD *md = new MD(pwm, dere);
+    printf("md\r\n");
+    MD_PID *md_pid = new MD_PID(md, kp_, ki_, kd_, pulse_, qei);
+    printf("md_pid\r\n");
+    return md_pid;
+}
\ No newline at end of file