taiyou komazawa / Mbed 2 deprecated Nucleo_fliping_arm

2018 HongoMechaTech A

Dependencies:   mbed

lib/PID_Control.cpp@0:e83b840a5f86, 2018-09-18 (annotated)

Committer:
Komazawa_sun
Date:
Tue Sep 18 03:11:01 2018 +0000
Revision:
0:e83b840a5f86
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Komazawa_sun 0:e83b840a5f86 1 #include "PID_Control.h"
Komazawa_sun 0:e83b840a5f86 2
Komazawa_sun 0:e83b840a5f86 3 PID_Control::PID_Control(double kp, double ki, double kd)
Komazawa_sun 0:e83b840a5f86 4 {
Komazawa_sun 0:e83b840a5f86 5 set_PID(kp, ki, kd);
Komazawa_sun 0:e83b840a5f86 6 integral = 0.0;
Komazawa_sun 0:e83b840a5f86 7 }
Komazawa_sun 0:e83b840a5f86 8
Komazawa_sun 0:e83b840a5f86 9 double PID_Control::PID(double present, double target, double interval)
Komazawa_sun 0:e83b840a5f86 10 {
Komazawa_sun 0:e83b840a5f86 11 diff[0] = diff[1];
Komazawa_sun 0:e83b840a5f86 12 diff[1] = target - present;
Komazawa_sun 0:e83b840a5f86 13 integral += ((diff[0] + diff[1]) / 2.0) * interval;
Komazawa_sun 0:e83b840a5f86 14 p = kp * diff[1];
Komazawa_sun 0:e83b840a5f86 15 i = ki * integral;
Komazawa_sun 0:e83b840a5f86 16 d = kd * (diff[1] - diff[0]);
Komazawa_sun 0:e83b840a5f86 17 control = p + i + d;
Komazawa_sun 0:e83b840a5f86 18 //printf("p[%f] i[%f] d[%f] ", p, i, d);
Komazawa_sun 0:e83b840a5f86 19 //printf("p:%.3f\t:%.3f\tc:%.3f\t:%.3f\r\n", present, target, control, interval);
Komazawa_sun 0:e83b840a5f86 20 //printf("t:%.3f\r\n", target);
Komazawa_sun 0:e83b840a5f86 21 return control;
Komazawa_sun 0:e83b840a5f86 22 }
Komazawa_sun 0:e83b840a5f86 23
Komazawa_sun 0:e83b840a5f86 24 void PID_Control::reset()
Komazawa_sun 0:e83b840a5f86 25 {
Komazawa_sun 0:e83b840a5f86 26 diff[0] = 0;
Komazawa_sun 0:e83b840a5f86 27 diff[1] = 0.0;
Komazawa_sun 0:e83b840a5f86 28 integral = 0.0;
Komazawa_sun 0:e83b840a5f86 29 control = 0;
Komazawa_sun 0:e83b840a5f86 30 }
Komazawa_sun 0:e83b840a5f86 31
Komazawa_sun 0:e83b840a5f86 32 void PID_Control::set_PID(double kp, double ki, double kd)
Komazawa_sun 0:e83b840a5f86 33 {
Komazawa_sun 0:e83b840a5f86 34 this->kp = kp;
Komazawa_sun 0:e83b840a5f86 35 this->ki = ki;
Komazawa_sun 0:e83b840a5f86 36 this->kd = kd;
Komazawa_sun 0:e83b840a5f86 37 reset();
Komazawa_sun 0:e83b840a5f86 38 }