//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
PID/PID.cpp
- Committer:
- alfaleader
- Date:
- 2016-05-16
- Revision:
- 10:12f9371f3e0f
- Parent:
- 9:2779500685cb
File content as of revision 10:12f9371f3e0f:
#include "PID.h" PIDControll::PIDControll() { //variabelen instellen als we ons PIDControll object aanmaken //TODO: finetunen, andere Kp Ki Kd waarden 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; delay = 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 //TODO: andere maximum of minimum errorPID (verhogen? verlagen?) if(errorPID > 400) errorPID = 400; else if(errorPID < -400) errorPID = -400; stop = 0; //TODO: finetunes: mss hoger of lager 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 }