kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_fuzi by kusano kiyoshige

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers pid_.h Source File

pid_.h

00001 
00002 class Position_pid {
00003 public  :
00004     void setup(float Kp, float Ki, float Kd) {
00005         kp = Kp;
00006         ki = Ki;
00007         kd = Kd;
00008     }
00009 
00010     void cal(float target, float nowval, float dt) {
00011         old = now;
00012         now = nowval - target;
00013         p = now;
00014         i = i + (now + old)/2.0f * dt;
00015         d = (now - old) / dt;
00016         result = kp*p + ki*i + kd*d;
00017         if (result > 1.0f) {
00018             result = 1.0f;
00019         } else if (result < -1.0f) {
00020             result = -1.0f;
00021         }
00022     }
00023 
00024     float duty() {
00025         return result;
00026     }
00027 
00028 private :
00029     float kp, ki, kd,
00030           old, now,
00031           p, i, d, result;
00032 };
00033 
00034 /*class Speed_pid {
00035 public  :
00036     void setup(float Kp, float Ki, float Kd) {
00037         kp = Kp;
00038         ki = Ki;
00039         kd = Kd;
00040     }
00041 
00042     void cal(float target, float nowval, float dt) {
00043         e2 = e1;
00044         e1 = e;
00045         e = nowval - target;
00046         p = e - e1;
00047         i = e*dt;
00048         d = (e-2*e1+e2)/dt;
00049         result = result + (kp*p+ki*i+kd*d);
00050         if (result > 1.0f) {
00051             result = 1.0f;
00052         } else if (result < -1.0f) {
00053             result = -1.0f;
00054         }
00055     }
00056 
00057     float duty() {
00058         return result;
00059     }
00060 
00061 private :
00062     float kp, ki, kd,
00063           e, e1, e2,
00064           p, i, d, result;
00065 };*/