kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

sampleProgram

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

pid.h@7:c4ae1d001d09, 2017-07-01 (annotated)

Committer:
echo_piyo
Date:
Sat Jul 01 10:12:46 2017 +0000
Revision:
7:c4ae1d001d09
Parent:
0:bf96e953cdb8 
rotaryencoder?rad???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:bf96e953cdb8 1 class Position_pid {
echo_piyo 0:bf96e953cdb8 2 public :
echo_piyo 0:bf96e953cdb8 3 void setup(float Kp, float Ki, float Kd) {
echo_piyo 0:bf96e953cdb8 4 kp = Kp;
echo_piyo 0:bf96e953cdb8 5 ki = Ki;
echo_piyo 0:bf96e953cdb8 6 kd = Kd;
echo_piyo 0:bf96e953cdb8 7 }
echo_piyo 0:bf96e953cdb8 8
echo_piyo 0:bf96e953cdb8 9 void cal(float target, float nowval, float dt) {
echo_piyo 0:bf96e953cdb8 10 old = now;
echo_piyo 0:bf96e953cdb8 11 now = nowval - target;
echo_piyo 0:bf96e953cdb8 12 p = now;
echo_piyo 0:bf96e953cdb8 13 i = i + (now + old)/2.0f * dt;
echo_piyo 0:bf96e953cdb8 14 d = (now - old) / dt;
echo_piyo 0:bf96e953cdb8 15 result = kp*p + ki*i + kd*d;
echo_piyo 0:bf96e953cdb8 16 if (result > 1.0f) {
echo_piyo 0:bf96e953cdb8 17 result = 1.0f;
echo_piyo 0:bf96e953cdb8 18 } else if (result < -1.0f) {
echo_piyo 0:bf96e953cdb8 19 result = -1.0f;
echo_piyo 0:bf96e953cdb8 20 }
echo_piyo 0:bf96e953cdb8 21 }
echo_piyo 0:bf96e953cdb8 22
echo_piyo 0:bf96e953cdb8 23 float duty() {
echo_piyo 0:bf96e953cdb8 24 return result;
echo_piyo 0:bf96e953cdb8 25 }
echo_piyo 0:bf96e953cdb8 26
echo_piyo 0:bf96e953cdb8 27 private :
echo_piyo 0:bf96e953cdb8 28 float kp, ki, kd,
echo_piyo 0:bf96e953cdb8 29 old, now,
echo_piyo 0:bf96e953cdb8 30 p, i, d, result;
echo_piyo 0:bf96e953cdb8 31 };