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@3:27407c4984cf, 2014-02-13 (annotated)

Committer:
komaida424
Date:
Thu Feb 13 16:07:07 2014 +0000
Revision:
3:27407c4984cf
Parent:
2:59ac9df97701 
revsion.1.30

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 3:27407c4984cf 8 differential_limit = 200;
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 3:27407c4984cf 17 void PID::init(float KP,float KI,float KD,float PID_LIM,float DIFFERENTIAL_LIM)
komaida424 2:59ac9df97701 18 {
komaida424 2:59ac9df97701 19 // interval = INTERVAL;
komaida424 2:59ac9df97701 20 pid_limit = PID_LIM;
komaida424 3:27407c4984cf 21 differential_limit = DIFFERENTIAL_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 3:27407c4984cf 31 float differential;
komaida424 2:59ac9df97701 32 cur = value - target;
komaida424 2:59ac9df97701 33 // if ( (old >= 0 && cur <= 0) || (old <= 0 && cur >= 0) || fabsf(cur) < 1 )
komaida424 2:59ac9df97701 34 // integral = 0;
komaida424 2:59ac9df97701 35 integral += ( cur + old ) * interval * 0.5f;
komaida424 3:27407c4984cf 36 // if ( integral < -differential_limit ) integral = -differential_limit;
komaida424 3:27407c4984cf 37 // if ( integral > integral_limit ) integral = integral_limit;
komaida424 3:27407c4984cf 38 differential = ( kd * ( cur - old ) / interval );
komaida424 3:27407c4984cf 39 if ( differential < -differential_limit ) differential = -differential_limit;
komaida424 3:27407c4984cf 40 if ( differential > differential_limit ) differential = differential_limit;
komaida424 3:27407c4984cf 41 float pid = ( kp * cur ) + ( ki * integral ) + differential;
komaida424 2:59ac9df97701 42 if ( pid < -pid_limit ) pid = -pid_limit;
komaida424 2:59ac9df97701 43 if ( pid > pid_limit ) pid = pid_limit;
komaida424 2:59ac9df97701 44 old = cur;
komaida424 2:59ac9df97701 45 return pid;
komaida424 2:59ac9df97701 46 }
komaida424 2:59ac9df97701 47
komaida424 2:59ac9df97701 48 void PID::reset()
komaida424 2:59ac9df97701 49 {
komaida424 2:59ac9df97701 50 integral = 0;
komaida424 2:59ac9df97701 51 old = 0;
komaida424 2:59ac9df97701 52 }
komaida424 2:59ac9df97701 53 ;
komaida424 2:59ac9df97701 54
komaida424 2:59ac9df97701 55
komaida424 2:59ac9df97701 56