kusano kiyoshige / Mbed 2 deprecated 17robo_fuzi

ライブラリ化を行った後

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_Practice1 by kusano kiyoshige

pid.h@10:04f2a82cfd89, 2017-07-16 (annotated)

Committer:
echo_piyo
Date:
Sun Jul 16 04:06:49 2017 +0000
Revision:
10:04f2a82cfd89
Parent:
0:bf96e953cdb8 
(??)????????????encorder.h?Speed_pid??Position_pid?????Main?????setup????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 10:04f2a82cfd89 1
echo_piyo 0:bf96e953cdb8 2 class Position_pid {
echo_piyo 0:bf96e953cdb8 3 public :
echo_piyo 0:bf96e953cdb8 4 void setup(float Kp, float Ki, float Kd) {
echo_piyo 0:bf96e953cdb8 5 kp = Kp;
echo_piyo 0:bf96e953cdb8 6 ki = Ki;
echo_piyo 0:bf96e953cdb8 7 kd = Kd;
echo_piyo 0:bf96e953cdb8 8 }
echo_piyo 0:bf96e953cdb8 9
echo_piyo 0:bf96e953cdb8 10 void cal(float target, float nowval, float dt) {
echo_piyo 0:bf96e953cdb8 11 old = now;
echo_piyo 0:bf96e953cdb8 12 now = nowval - target;
echo_piyo 0:bf96e953cdb8 13 p = now;
echo_piyo 0:bf96e953cdb8 14 i = i + (now + old)/2.0f * dt;
echo_piyo 0:bf96e953cdb8 15 d = (now - old) / dt;
echo_piyo 0:bf96e953cdb8 16 result = kp*p + ki*i + kd*d;
echo_piyo 0:bf96e953cdb8 17 if (result > 1.0f) {
echo_piyo 0:bf96e953cdb8 18 result = 1.0f;
echo_piyo 0:bf96e953cdb8 19 } else if (result < -1.0f) {
echo_piyo 0:bf96e953cdb8 20 result = -1.0f;
echo_piyo 0:bf96e953cdb8 21 }
echo_piyo 0:bf96e953cdb8 22 }
echo_piyo 0:bf96e953cdb8 23
echo_piyo 0:bf96e953cdb8 24 float duty() {
echo_piyo 0:bf96e953cdb8 25 return result;
echo_piyo 0:bf96e953cdb8 26 }
echo_piyo 0:bf96e953cdb8 27
echo_piyo 0:bf96e953cdb8 28 private :
echo_piyo 0:bf96e953cdb8 29 float kp, ki, kd,
echo_piyo 0:bf96e953cdb8 30 old, now,
echo_piyo 0:bf96e953cdb8 31 p, i, d, result;
echo_piyo 0:bf96e953cdb8 32 };
echo_piyo 0:bf96e953cdb8 33
echo_piyo 10:04f2a82cfd89 34 /*class Speed_pid {
echo_piyo 0:bf96e953cdb8 35 public :
echo_piyo 0:bf96e953cdb8 36 void setup(float Kp, float Ki, float Kd) {
echo_piyo 0:bf96e953cdb8 37 kp = Kp;
echo_piyo 0:bf96e953cdb8 38 ki = Ki;
echo_piyo 0:bf96e953cdb8 39 kd = Kd;
echo_piyo 0:bf96e953cdb8 40 }
echo_piyo 0:bf96e953cdb8 41
echo_piyo 0:bf96e953cdb8 42 void cal(float target, float nowval, float dt) {
echo_piyo 0:bf96e953cdb8 43 e2 = e1;
echo_piyo 0:bf96e953cdb8 44 e1 = e;
echo_piyo 0:bf96e953cdb8 45 e = nowval - target;
echo_piyo 0:bf96e953cdb8 46 p = e - e1;
echo_piyo 0:bf96e953cdb8 47 i = e*dt;
echo_piyo 0:bf96e953cdb8 48 d = (e-2*e1+e2)/dt;
echo_piyo 0:bf96e953cdb8 49 result = result + (kp*p+ki*i+kd*d);
echo_piyo 0:bf96e953cdb8 50 if (result > 1.0f) {
echo_piyo 0:bf96e953cdb8 51 result = 1.0f;
echo_piyo 0:bf96e953cdb8 52 } else if (result < -1.0f) {
echo_piyo 0:bf96e953cdb8 53 result = -1.0f;
echo_piyo 0:bf96e953cdb8 54 }
echo_piyo 0:bf96e953cdb8 55 }
echo_piyo 0:bf96e953cdb8 56
echo_piyo 0:bf96e953cdb8 57 float duty() {
echo_piyo 0:bf96e953cdb8 58 return result;
echo_piyo 0:bf96e953cdb8 59 }
echo_piyo 0:bf96e953cdb8 60
echo_piyo 0:bf96e953cdb8 61 private :
echo_piyo 0:bf96e953cdb8 62 float kp, ki, kd,
echo_piyo 0:bf96e953cdb8 63 e, e1, e2,
echo_piyo 0:bf96e953cdb8 64 p, i, d, result;
echo_piyo 10:04f2a82cfd89 65 };*/