Diff: encorder.h

Revision:

10:04f2a82cfd89

Child:

31:285c9898da03

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encorder.h	Sun Jul 16 04:06:49 2017 +0000
@@ -0,0 +1,143 @@
+#define PI      3.141592654f
+#define Nmax    18000
+#define invalidN    0.1
+
+class Position_pid_enc {
+public  :
+    void set_parameter(float Kp, float Ki, float Kd) {
+        kp = Kp;
+        ki = Ki;
+        kd = Kd;
+    }
+
+    void position_cal(float target, float nowval, float dt) {
+        old = now;
+        now = nowval - target;
+        p = now;
+        i = i + (now + old)/2.0f * dt;
+        d = (now - old) / dt;
+        result = kp*p + ki*i + kd*d;
+        if (result > 1.0f) {
+            result = 1.0f;
+        } else if (result < -1.0f) {
+            result = -1.0f;
+        }
+    }
+
+    float duty() {
+        return result;
+    }
+
+private :
+    float kp, ki, kd,
+          old, now,
+          p, i, d, result;
+};
+/*
+class Speed_pid
+{
+public:
+    void set_parameter(float Kp, float Ki, float Kd) {
+        kp = Kp;
+        ki = Ki;
+        kd = Kd;
+    }
+    
+    float duty(){
+        return result;
+    }
+    
+    float e,p,i,d;
+
+protected :
+    float kp,ki,kd;
+    float e1,e2;
+    //float p,i,d;
+    float result;
+    
+    void pid_cal(float target, float val, float dt){
+        e2 = e1;
+        e1 = e;
+        e = target - val;
+        p = e - e1;
+        i = e;
+        d = (e-2*e1+e2);
+        result = result + (kp*p+ki*i+kd*d);
+        if (result > 1.0f) {
+            result = 1.0f;
+        } else if (result < -1.0f) {
+            result = -1.0f;
+        }
+        if ((target == 0) && (fabs(e) < invalidN)) {
+            result = 0;
+        }
+    }  
+};
+*/
+class Encoder : public Position_pid_enc //: public Speed_pid
+{
+public:
+    Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) {
+    }
+
+    void setup(int Ppr) {
+        ppr = Ppr;
+        A.rise(this, &Encoder::A_count);
+        B.rise(this, &Encoder::B_count);
+        origin();
+    }
+    
+    void cal(float target, float dt) {
+        radian = (float) count/ppr*2*PI;
+        degree = radian / PI * 180.0;         //
+        w = (radian - oldtheta) / dt;
+        v = 0.05*w/27;
+        n = w*30/(PI);
+        oldtheta = radian;
+        //pid_cal(target, n/Nmax, dt);
+        deg_cylinder_cal();                 //
+        position_cal(target, deg_cylinder, dt);
+    }
+    
+    void deg_cylinder_cal(){
+        deg_cylinder = (count / 1200) * 360 * 3 / 20;
+    }
+    
+    float deg(){
+        return degree;
+    }
+    
+    float rad() {
+        return radian;
+    }
+    
+    long pulse(){
+        return count;
+    }
+    
+    float N(){
+        return n;
+    }
+
+    void origin() {
+        count = 0;
+    }
+
+private :
+    InterruptIn A;
+    InterruptIn B;
+
+    int ppr;
+    signed long count;
+    float radian,oldtheta;
+    float w,v,n;
+    float degree, deg_cylinder;
+
+    void A_count() {
+        count ++;
+    }
+
+    void B_count() {
+        count --;
+    }
+};
\ No newline at end of file