Cristi Stoican / Mbed 2 deprecated nRF51822_SimpleChat

PID

Dependencies:   BLE_API mbed nRF51822

RST.cpp@1:d3e12393b71d, 2017-01-12 (annotated)

Committer:
stoicancristi
Date:
Thu Jan 12 16:04:37 2017 +0000
Revision:
1:d3e12393b71d
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stoicancristi 1:d3e12393b71d 1 #include "RST.h"
stoicancristi 1:d3e12393b71d 2
stoicancristi 1:d3e12393b71d 3 RST::RST(float* R, float* S, float* T, RST_aditionalData SP):m_R(R), m_S(S), m_T(T), m_SP(SP)
stoicancristi 1:d3e12393b71d 4 {
stoicancristi 1:d3e12393b71d 5 //
stoicancristi 1:d3e12393b71d 6 }
stoicancristi 1:d3e12393b71d 7
stoicancristi 1:d3e12393b71d 8 float RST::ComputeCommand(float inputADC)
stoicancristi 1:d3e12393b71d 9 {
stoicancristi 1:d3e12393b71d 10 static float last_references[MAX_GRADE];
stoicancristi 1:d3e12393b71d 11 static float last_outputs[MAX_GRADE];
stoicancristi 1:d3e12393b71d 12 static float last_commands[MAX_GRADE];
stoicancristi 1:d3e12393b71d 13 float R_component = 0;
stoicancristi 1:d3e12393b71d 14 float T_component = 0;
stoicancristi 1:d3e12393b71d 15 float S_component = 0;
stoicancristi 1:d3e12393b71d 16 float outputPWM;
stoicancristi 1:d3e12393b71d 17
stoicancristi 1:d3e12393b71d 18 for(int i=0;i<(m_SP.gradR+1);i++)
stoicancristi 1:d3e12393b71d 19 {
stoicancristi 1:d3e12393b71d 20 last_references[0] = m_SP.setpoint;
stoicancristi 1:d3e12393b71d 21 R_component = R_component + m_R[i]*last_references[i];
stoicancristi 1:d3e12393b71d 22 }
stoicancristi 1:d3e12393b71d 23 for(int i=0;i<m_SP.gradR;i++)
stoicancristi 1:d3e12393b71d 24 {
stoicancristi 1:d3e12393b71d 25 last_references[i+1] = last_references[i];
stoicancristi 1:d3e12393b71d 26 }
stoicancristi 1:d3e12393b71d 27 for(int i=0;i<(m_SP.gradT+1);i++)
stoicancristi 1:d3e12393b71d 28 {
stoicancristi 1:d3e12393b71d 29 last_outputs[0] = inputADC;
stoicancristi 1:d3e12393b71d 30 T_component = T_component + m_T[i]*last_outputs[i];
stoicancristi 1:d3e12393b71d 31 }
stoicancristi 1:d3e12393b71d 32 for(int i=0;i<m_SP.gradT;i++)
stoicancristi 1:d3e12393b71d 33 {
stoicancristi 1:d3e12393b71d 34 last_outputs[i+1] = last_outputs[i];
stoicancristi 1:d3e12393b71d 35 }
stoicancristi 1:d3e12393b71d 36 for(int i=1;i<(m_SP.gradS+1);i++)
stoicancristi 1:d3e12393b71d 37 {
stoicancristi 1:d3e12393b71d 38 S_component = S_component + last_commands[i]*m_S[i];
stoicancristi 1:d3e12393b71d 39 }
stoicancristi 1:d3e12393b71d 40
stoicancristi 1:d3e12393b71d 41 outputPWM = T_component - R_component - S_component;
stoicancristi 1:d3e12393b71d 42 last_commands[0] = outputPWM;
stoicancristi 1:d3e12393b71d 43
stoicancristi 1:d3e12393b71d 44 for(int i=0;i<m_SP.gradS;i++)
stoicancristi 1:d3e12393b71d 45 {
stoicancristi 1:d3e12393b71d 46 last_commands[i+1] = last_commands[i];
stoicancristi 1:d3e12393b71d 47 }
stoicancristi 1:d3e12393b71d 48
stoicancristi 1:d3e12393b71d 49 return outputPWM;
stoicancristi 1:d3e12393b71d 50 }
stoicancristi 1:d3e12393b71d 51
stoicancristi 1:d3e12393b71d 52
stoicancristi 1:d3e12393b71d 53