José Claudio / Mbed 2 deprecated QuadCopter-Sensor-Serial

Quadcopter working with accelerômeter and accelerometer, and bluetooth radio for communication

Dependencies:   mbed

Controllers/PID.cpp@0:56b8c86181b1, 2013-05-21 (annotated)

Committer:
jose_claudiojr
Date:
Tue May 21 14:12:13 2013 +0000
Revision:
0:56b8c86181b1
Quadcopter code with accelerometer and gyroscope.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jose_claudiojr 0:56b8c86181b1 1 #include "PID.h"
jose_claudiojr 0:56b8c86181b1 2
jose_claudiojr 0:56b8c86181b1 3 PID::PID(float kp, float ki, float kd, float dt)
jose_claudiojr 0:56b8c86181b1 4 {
jose_claudiojr 0:56b8c86181b1 5 this->kp = kp;
jose_claudiojr 0:56b8c86181b1 6 this->ki = ki;
jose_claudiojr 0:56b8c86181b1 7 this->kd = kd;
jose_claudiojr 0:56b8c86181b1 8 this->dt = dt;
jose_claudiojr 0:56b8c86181b1 9
jose_claudiojr 0:56b8c86181b1 10 reset();
jose_claudiojr 0:56b8c86181b1 11 }
jose_claudiojr 0:56b8c86181b1 12
jose_claudiojr 0:56b8c86181b1 13 void PID::reset()
jose_claudiojr 0:56b8c86181b1 14 {
jose_claudiojr 0:56b8c86181b1 15 lastErro = 0.0f;
jose_claudiojr 0:56b8c86181b1 16 lastSumErro = 0.0f;
jose_claudiojr 0:56b8c86181b1 17 _pid = 0.0f;
jose_claudiojr 0:56b8c86181b1 18
jose_claudiojr 0:56b8c86181b1 19 integral = 0.0f;
jose_claudiojr 0:56b8c86181b1 20
jose_claudiojr 0:56b8c86181b1 21 }
jose_claudiojr 0:56b8c86181b1 22
jose_claudiojr 0:56b8c86181b1 23 void PID::setGains(float kp, float ki, float kd)
jose_claudiojr 0:56b8c86181b1 24 {
jose_claudiojr 0:56b8c86181b1 25 this->kp = kp;
jose_claudiojr 0:56b8c86181b1 26 this->ki = ki;
jose_claudiojr 0:56b8c86181b1 27 this->kd = kd;
jose_claudiojr 0:56b8c86181b1 28 }
jose_claudiojr 0:56b8c86181b1 29
jose_claudiojr 0:56b8c86181b1 30 float PID::compute(float setPoint, float currentPoint)
jose_claudiojr 0:56b8c86181b1 31 {
jose_claudiojr 0:56b8c86181b1 32 float error = setPoint - currentPoint;
jose_claudiojr 0:56b8c86181b1 33 float derivative = (error - lastErro)/dt;
jose_claudiojr 0:56b8c86181b1 34 integral = (integral + (error*dt));
jose_claudiojr 0:56b8c86181b1 35
jose_claudiojr 0:56b8c86181b1 36 float contribuicao = (kp*error) + (ki*integral) + (kd*derivative);
jose_claudiojr 0:56b8c86181b1 37
jose_claudiojr 0:56b8c86181b1 38
jose_claudiojr 0:56b8c86181b1 39
jose_claudiojr 0:56b8c86181b1 40 //_pid = contribuicao;
jose_claudiojr 0:56b8c86181b1 41
jose_claudiojr 0:56b8c86181b1 42 lastErro = error;
jose_claudiojr 0:56b8c86181b1 43 //lastSumErro += error;
jose_claudiojr 0:56b8c86181b1 44
jose_claudiojr 0:56b8c86181b1 45 return contribuicao;
jose_claudiojr 0:56b8c86181b1 46 }
jose_claudiojr 0:56b8c86181b1 47
jose_claudiojr 0:56b8c86181b1 48 float PID::getP()
jose_claudiojr 0:56b8c86181b1 49 {
jose_claudiojr 0:56b8c86181b1 50 return kp;
jose_claudiojr 0:56b8c86181b1 51 }
jose_claudiojr 0:56b8c86181b1 52
jose_claudiojr 0:56b8c86181b1 53 float PID::getI()
jose_claudiojr 0:56b8c86181b1 54 {
jose_claudiojr 0:56b8c86181b1 55 return ki;
jose_claudiojr 0:56b8c86181b1 56 }
jose_claudiojr 0:56b8c86181b1 57
jose_claudiojr 0:56b8c86181b1 58 float PID::getD()
jose_claudiojr 0:56b8c86181b1 59 {
jose_claudiojr 0:56b8c86181b1 60 return kd;
jose_claudiojr 0:56b8c86181b1 61 }
jose_claudiojr 0:56b8c86181b1 62 /*
jose_claudiojr 0:56b8c86181b1 63 float PID::getuP()
jose_claudiojr 0:56b8c86181b1 64 {
jose_claudiojr 0:56b8c86181b1 65 return up;
jose_claudiojr 0:56b8c86181b1 66 }
jose_claudiojr 0:56b8c86181b1 67
jose_claudiojr 0:56b8c86181b1 68 float PID::getuI()
jose_claudiojr 0:56b8c86181b1 69 {
jose_claudiojr 0:56b8c86181b1 70 return ui;
jose_claudiojr 0:56b8c86181b1 71 }
jose_claudiojr 0:56b8c86181b1 72
jose_claudiojr 0:56b8c86181b1 73 float PID::getuD()
jose_claudiojr 0:56b8c86181b1 74 {
jose_claudiojr 0:56b8c86181b1 75 return ud;
jose_claudiojr 0:56b8c86181b1 76 }
jose_claudiojr 0:56b8c86181b1 77 */