Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

PID/PID.cpp@10:12f9371f3e0f, 2016-05-16 (annotated)

Committer:
alfaleader
Date:
Mon May 16 09:50:44 2016 +0000
Revision:
10:12f9371f3e0f
Parent:
9:2779500685cb 
d

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 10:12f9371f3e0f 5 //TODO: finetunen, andere Kp Ki Kd waarden
alfaleader 9:2779500685cb 6 Kp = 10;
alfaleader 9:2779500685cb 7 Ki = 0.0001;
alfaleader 9:2779500685cb 8 Kd = 5;
alfaleader 9:2779500685cb 9 set_point = 0; // camera hoek
alfaleader 9:2779500685cb 10 proportional = 0;
alfaleader 9:2779500685cb 11 last_proportional =0;
alfaleader 9:2779500685cb 12 integral = 0;
alfaleader 9:2779500685cb 13 derivative = 0;
alfaleader 9:2779500685cb 14 errorPID = 0;
alfaleader 9:2779500685cb 15 dir = 0;
alfaleader 10:12f9371f3e0f 16 delay = 0;
alfaleader 9:2779500685cb 17 }
alfaleader 9:2779500685cb 18
alfaleader 9:2779500685cb 19 void PIDControll::PIDaanpassing(float angle){
alfaleader 9:2779500685cb 20 proportional = set_point - angle;
alfaleader 9:2779500685cb 21 integral += proportional;
alfaleader 9:2779500685cb 22 derivative = proportional - last_proportional;
alfaleader 9:2779500685cb 23 last_proportional = proportional;
alfaleader 9:2779500685cb 24
alfaleader 9:2779500685cb 25 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
alfaleader 9:2779500685cb 26 (errorPID > 0)?(dir = 1):(dir = 0);
alfaleader 9:2779500685cb 27
alfaleader 9:2779500685cb 28 // errorPID is restricted between -400 and 400
alfaleader 10:12f9371f3e0f 29 //TODO: andere maximum of minimum errorPID (verhogen? verlagen?)
alfaleader 9:2779500685cb 30 if(errorPID > 400)
alfaleader 9:2779500685cb 31 errorPID = 400;
alfaleader 9:2779500685cb 32 else if(errorPID < -400)
alfaleader 9:2779500685cb 33 errorPID = -400;
alfaleader 9:2779500685cb 34
alfaleader 9:2779500685cb 35 stop = 0;
alfaleader 10:12f9371f3e0f 36 //TODO: finetunes: mss hoger of lager
alfaleader 9:2779500685cb 37 delay = 0.1/abs(errorPID); // speed should be proportional to error, therefore time delay
alfaleader 9:2779500685cb 38 //between steps should be inverse proportional to error.
alfaleader 10:12f9371f3e0f 39 if (abs(errorPID)< Kp/2) stop = 1; // 0.5 deg noise margin
alfaleader 9:2779500685cb 40 }