Danielle Kruijver / Mbed 2 deprecated Controll_SBT_main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos

pid.cpp@1:3d46a0660d05, 2019-04-12 (annotated)

Committer:
DanielleKruijver
Date:
Fri Apr 12 09:55:53 2019 +0000
Revision:
1:3d46a0660d05
Parent:
0:3fbb3fa4ff64 
Main SBT controllll

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Awiegmink 0:3fbb3fa4ff64 1 /*
Awiegmink 0:3fbb3fa4ff64 2 * File: pid.cpp
Awiegmink 0:3fbb3fa4ff64 3 * Author: User
Awiegmink 0:3fbb3fa4ff64 4 *
Awiegmink 0:3fbb3fa4ff64 5 * Created on 27 februari 2019, 10:28
Awiegmink 0:3fbb3fa4ff64 6 */
Awiegmink 0:3fbb3fa4ff64 7 #include <iostream>
Awiegmink 0:3fbb3fa4ff64 8 #include <cmath>
Awiegmink 0:3fbb3fa4ff64 9 #include "pid.h"
Awiegmink 0:3fbb3fa4ff64 10
Awiegmink 0:3fbb3fa4ff64 11 using namespace std;
Awiegmink 0:3fbb3fa4ff64 12
Awiegmink 0:3fbb3fa4ff64 13 control::PID::PID( double dt, double Kp, double Kd, double Ki, double Fc )
Awiegmink 0:3fbb3fa4ff64 14 {
Awiegmink 0:3fbb3fa4ff64 15 pimpl = new PIDImpl(dt,Kp,Kd,Ki,Fc);
Awiegmink 0:3fbb3fa4ff64 16 }
Awiegmink 0:3fbb3fa4ff64 17 double control::PID::calculate( double reference, double pv )
Awiegmink 0:3fbb3fa4ff64 18 {
Awiegmink 0:3fbb3fa4ff64 19 return pimpl->calculate(reference,pv);
Awiegmink 0:3fbb3fa4ff64 20 }
Awiegmink 0:3fbb3fa4ff64 21 control::PID::~PID()
Awiegmink 0:3fbb3fa4ff64 22 {
Awiegmink 0:3fbb3fa4ff64 23 delete pimpl;
Awiegmink 0:3fbb3fa4ff64 24 }
Awiegmink 0:3fbb3fa4ff64 25
Awiegmink 0:3fbb3fa4ff64 26 /**
Awiegmink 0:3fbb3fa4ff64 27 * Implementation
Awiegmink 0:3fbb3fa4ff64 28 */
Awiegmink 0:3fbb3fa4ff64 29 control::PIDImpl::PIDImpl( double dt, double Kp, double Kd, double Ki, double Fc ) :
Awiegmink 0:3fbb3fa4ff64 30 _dt(dt),
Awiegmink 0:3fbb3fa4ff64 31 _Kp(Kp),
Awiegmink 0:3fbb3fa4ff64 32 _Kd(Kd),
Awiegmink 0:3fbb3fa4ff64 33 _Ki(Ki),
Awiegmink 0:3fbb3fa4ff64 34 _Fc(Fc),
Awiegmink 0:3fbb3fa4ff64 35 _pre_error(0),
Awiegmink 0:3fbb3fa4ff64 36 _integral(0)
Awiegmink 0:3fbb3fa4ff64 37 {
Awiegmink 0:3fbb3fa4ff64 38 }
Awiegmink 0:3fbb3fa4ff64 39 /*The input for the calculate function will be the reference and the 'previous
Awiegmink 0:3fbb3fa4ff64 40 * value (pv)' that comes in reality from the sensors. This data is stored in
Awiegmink 0:3fbb3fa4ff64 41 * DataStore so we should add these values. This is different for all the PID
Awiegmink 0:3fbb3fa4ff64 42 * controllers
Awiegmink 0:3fbb3fa4ff64 43 */
Awiegmink 0:3fbb3fa4ff64 44 double control::PIDImpl::calculate( double reference, double pv )
Awiegmink 0:3fbb3fa4ff64 45 {
Awiegmink 0:3fbb3fa4ff64 46
Awiegmink 0:3fbb3fa4ff64 47 // Calculate error
Awiegmink 0:3fbb3fa4ff64 48 double error = reference - pv;
Awiegmink 0:3fbb3fa4ff64 49
Awiegmink 0:3fbb3fa4ff64 50 // Proportional term
Awiegmink 0:3fbb3fa4ff64 51 double Pout = _Kp * error;
Awiegmink 0:3fbb3fa4ff64 52
Awiegmink 0:3fbb3fa4ff64 53 // Integral term
Awiegmink 0:3fbb3fa4ff64 54 _integral += error * _dt;
Awiegmink 0:3fbb3fa4ff64 55 double Iout = _Ki * _integral;
Awiegmink 0:3fbb3fa4ff64 56
Awiegmink 0:3fbb3fa4ff64 57 // Derivative term
Awiegmink 0:3fbb3fa4ff64 58 //double derivative = (error - _pre_error) / _dt;
Awiegmink 0:3fbb3fa4ff64 59 //double Dout = _Kd * derivative;
Awiegmink 0:3fbb3fa4ff64 60 double Dout = _Kd * (_Fc/(1+_Fc* _integral));
Awiegmink 0:3fbb3fa4ff64 61 // Calculate total output
Awiegmink 0:3fbb3fa4ff64 62 double output = Pout + Iout + Dout;
Awiegmink 0:3fbb3fa4ff64 63
Awiegmink 0:3fbb3fa4ff64 64 // Save error to previous error
Awiegmink 0:3fbb3fa4ff64 65 _pre_error = error;
Awiegmink 0:3fbb3fa4ff64 66
Awiegmink 0:3fbb3fa4ff64 67 return output;
Awiegmink 0:3fbb3fa4ff64 68 }
Awiegmink 0:3fbb3fa4ff64 69
Awiegmink 0:3fbb3fa4ff64 70 control::PIDImpl::~PIDImpl()
Awiegmink 0:3fbb3fa4ff64 71 {
Awiegmink 0:3fbb3fa4ff64 72 }