//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
Diff: PID/PID.cpp
- Revision:
- 9:2779500685cb
- Child:
- 10:12f9371f3e0f
diff -r c573f315319a -r 2779500685cb PID/PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID/PID.cpp Sun May 15 16:15:44 2016 +0000 @@ -0,0 +1,37 @@ +#include "PID.h" + +PIDControll::PIDControll() { + //variabelen instellen als we ons PIDControll object aanmaken + Kp = 10; + Ki = 0.0001; + Kd = 5; + set_point = 0; // camera hoek + proportional = 0; + last_proportional =0; + integral = 0; + derivative = 0; + errorPID = 0; + dir = 0; +} + +void PIDControll::PIDaanpassing(float angle){ + proportional = set_point - angle; + integral += proportional; + derivative = proportional - last_proportional; + last_proportional = proportional; + + errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); + (errorPID > 0)?(dir = 1):(dir = 0); + + // errorPID is restricted between -400 and 400 + if(errorPID > 400) + errorPID = 400; + else if(errorPID < -400) + errorPID = -400; + + stop = 0; + delay = 0.1/abs(errorPID); // speed should be proportional to error, therefore time delay + //between steps should be inverse proportional to error. + if (abs(errorPID)< Kp/2) stop = 1; // 0.5 deg noise margin + +} \ No newline at end of file