Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

PID/PID.cpp@9:2779500685cb, 2016-05-15 (annotated)

Committer:
alfaleader
Date:
Sun May 15 16:15:44 2016 +0000
Revision:
9:2779500685cb
Child:
10:12f9371f3e0f
f

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alfaleader 9:2779500685cb 1 #include "PID.h"
alfaleader 9:2779500685cb 2
alfaleader 9:2779500685cb 3 PIDControll::PIDControll() {
alfaleader 9:2779500685cb 4 //variabelen instellen als we ons PIDControll object aanmaken
alfaleader 9:2779500685cb 5 Kp = 10;
alfaleader 9:2779500685cb 6 Ki = 0.0001;
alfaleader 9:2779500685cb 7 Kd = 5;
alfaleader 9:2779500685cb 8 set_point = 0; // camera hoek
alfaleader 9:2779500685cb 9 proportional = 0;
alfaleader 9:2779500685cb 10 last_proportional =0;
alfaleader 9:2779500685cb 11 integral = 0;
alfaleader 9:2779500685cb 12 derivative = 0;
alfaleader 9:2779500685cb 13 errorPID = 0;
alfaleader 9:2779500685cb 14 dir = 0;
alfaleader 9:2779500685cb 15 }
alfaleader 9:2779500685cb 16
alfaleader 9:2779500685cb 17 void PIDControll::PIDaanpassing(float angle){
alfaleader 9:2779500685cb 18 proportional = set_point - angle;
alfaleader 9:2779500685cb 19 integral += proportional;
alfaleader 9:2779500685cb 20 derivative = proportional - last_proportional;
alfaleader 9:2779500685cb 21 last_proportional = proportional;
alfaleader 9:2779500685cb 22
alfaleader 9:2779500685cb 23 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
alfaleader 9:2779500685cb 24 (errorPID > 0)?(dir = 1):(dir = 0);
alfaleader 9:2779500685cb 25
alfaleader 9:2779500685cb 26 // errorPID is restricted between -400 and 400
alfaleader 9:2779500685cb 27 if(errorPID > 400)
alfaleader 9:2779500685cb 28 errorPID = 400;
alfaleader 9:2779500685cb 29 else if(errorPID < -400)
alfaleader 9:2779500685cb 30 errorPID = -400;
alfaleader 9:2779500685cb 31
alfaleader 9:2779500685cb 32 stop = 0;
alfaleader 9:2779500685cb 33 delay = 0.1/abs(errorPID); // speed should be proportional to error, therefore time delay
alfaleader 9:2779500685cb 34 //between steps should be inverse proportional to error.
alfaleader 9:2779500685cb 35 if (abs(errorPID)< Kp/2) stop = 1; // 0.5 deg noise margin
alfaleader 9:2779500685cb 36
alfaleader 9:2779500685cb 37 }