Diff: pid_encoder.cpp

Revision:

0:929eee8e2c2c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid_encoder.cpp	Sun Sep 24 05:24:06 2017 +0000
@@ -0,0 +1,55 @@
+#include "pid_encoder.h"
+
+Encoder::Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) {
+}
+
+void Encoder::setPpr(int Ppr){
+        ppr = Ppr;
+        A.rise(this, &Encoder::A_count);
+        B.rise(this, &Encoder::B_count);
+        origin();
+}
+
+void Encoder::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 = (count / 1200) * 360 * 3 / 20;
+    calculate(target, (float)degree);
+}
+
+float Encoder::deg(){
+    return degree;
+}
+
+float Encoder::get_deg_cylinder(){
+    return deg_cylinder;
+}
+
+float Encoder::rad(){
+    return radian;
+}
+
+int Encoder::pulse(){
+    return count;
+}
+
+float Encoder::N(){
+    return n;
+}
+
+void Encoder::origin(){
+    count = 0;
+}
+
+void Encoder::A_count(){
+    count ++;
+}
+
+void Encoder::B_count(){
+    count --;
+}
\ No newline at end of file