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