//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
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?
User | Revision | Line number | New 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 | } |