syouichi imamori / Mbed OS MulticopterQuadX

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   IAP

PID/PID.cpp@2:59ac9df97701, 2013-11-15 (annotated)

Committer:
komaida424
Date:
Fri Nov 15 20:53:36 2013 +0000
Revision:
2:59ac9df97701
Child:
3:27407c4984cf
ver.1.25

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 2:59ac9df97701 1 #include "mbed.h"
komaida424 2:59ac9df97701 2 #include "SerialLcd.h"
komaida424 2:59ac9df97701 3 #include "PID.h"
komaida424 2:59ac9df97701 4
komaida424 2:59ac9df97701 5 PID::PID()
komaida424 2:59ac9df97701 6 {
komaida424 2:59ac9df97701 7 interval = 0.05;
komaida424 2:59ac9df97701 8 integral_limit = 50;
komaida424 2:59ac9df97701 9 pid_limit = 300;
komaida424 2:59ac9df97701 10 kp = 0.5;
komaida424 2:59ac9df97701 11 ki = 0.5;
komaida424 2:59ac9df97701 12 kd = 0.2;
komaida424 2:59ac9df97701 13 integral = 0;
komaida424 2:59ac9df97701 14 old = 0;
komaida424 2:59ac9df97701 15 }
komaida424 2:59ac9df97701 16
komaida424 2:59ac9df97701 17 void PID::init(float KP,float KI,float KD,float PID_LIM,float INTEGRAL_LIM)
komaida424 2:59ac9df97701 18 {
komaida424 2:59ac9df97701 19 // interval = INTERVAL;
komaida424 2:59ac9df97701 20 pid_limit = PID_LIM;
komaida424 2:59ac9df97701 21 integral_limit = INTEGRAL_LIM;
komaida424 2:59ac9df97701 22 kp = KP;
komaida424 2:59ac9df97701 23 ki = KI;
komaida424 2:59ac9df97701 24 kd = KD;
komaida424 2:59ac9df97701 25 integral = 0;
komaida424 2:59ac9df97701 26 old = 0;
komaida424 2:59ac9df97701 27 }
komaida424 2:59ac9df97701 28
komaida424 2:59ac9df97701 29 float PID::calc(float value,float target,float interval)
komaida424 2:59ac9df97701 30 {
komaida424 2:59ac9df97701 31 cur = value - target;
komaida424 2:59ac9df97701 32 // if ( (old >= 0 && cur <= 0) || (old <= 0 && cur >= 0) || fabsf(cur) < 1 )
komaida424 2:59ac9df97701 33 // integral = 0;
komaida424 2:59ac9df97701 34 integral += ( cur + old ) * interval * 0.5f;
komaida424 2:59ac9df97701 35 if ( integral < -integral_limit ) integral = -integral_limit;
komaida424 2:59ac9df97701 36 if ( integral > integral_limit ) integral = integral_limit;
komaida424 2:59ac9df97701 37 float pid = kp * cur + ki * integral + kd * ( cur - old ) / interval;
komaida424 2:59ac9df97701 38 if ( pid < -pid_limit ) pid = -pid_limit;
komaida424 2:59ac9df97701 39 if ( pid > pid_limit ) pid = pid_limit;
komaida424 2:59ac9df97701 40 old = cur;
komaida424 2:59ac9df97701 41 return pid;
komaida424 2:59ac9df97701 42 }
komaida424 2:59ac9df97701 43
komaida424 2:59ac9df97701 44 void PID::reset()
komaida424 2:59ac9df97701 45 {
komaida424 2:59ac9df97701 46 integral = 0;
komaida424 2:59ac9df97701 47 old = 0;
komaida424 2:59ac9df97701 48 }
komaida424 2:59ac9df97701 49 ;
komaida424 2:59ac9df97701 50
komaida424 2:59ac9df97701 51
komaida424 2:59ac9df97701 52