Cristi Stoican / Mbed 2 deprecated Migration

dsf

Dependencies:   BLE_API mbed nRF51822

RST.cpp@0:b5906c81772b, 2017-02-05 (annotated)

Committer:
stoicancristi
Date:
Sun Feb 05 16:31:58 2017 +0000
Revision:
0:b5906c81772b
BLE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stoicancristi 0:b5906c81772b 1 #include "RST.hpp"
stoicancristi 0:b5906c81772b 2
stoicancristi 0:b5906c81772b 3 RST::RST() {
stoicancristi 0:b5906c81772b 4 R = 0;
stoicancristi 0:b5906c81772b 5 S = 0;
stoicancristi 0:b5906c81772b 6 T = 0;
stoicancristi 0:b5906c81772b 7 }
stoicancristi 0:b5906c81772b 8
stoicancristi 0:b5906c81772b 9 float RST::calculateCmd()
stoicancristi 0:b5906c81772b 10 {
stoicancristi 0:b5906c81772b 11 static float last_references[ControllerParams::MAX_ORD];
stoicancristi 0:b5906c81772b 12 static float last_outputs[ControllerParams::MAX_ORD];
stoicancristi 0:b5906c81772b 13 static float last_commands[ControllerParams::MAX_ORD];
stoicancristi 0:b5906c81772b 14
stoicancristi 0:b5906c81772b 15 float R_component;
stoicancristi 0:b5906c81772b 16 float S_component;
stoicancristi 0:b5906c81772b 17 float T_component;
stoicancristi 0:b5906c81772b 18 float outputPWM;
stoicancristi 0:b5906c81772b 19
stoicancristi 0:b5906c81772b 20
stoicancristi 0:b5906c81772b 21 for(int i=0;i<(ordR+1);i++)
stoicancristi 0:b5906c81772b 22 {
stoicancristi 0:b5906c81772b 23 last_references[0] = this->ref;
stoicancristi 0:b5906c81772b 24 R_component = R_component + R[i]*last_references[i];
stoicancristi 0:b5906c81772b 25 }
stoicancristi 0:b5906c81772b 26 for(int i=0;i<ordR;i++)
stoicancristi 0:b5906c81772b 27 {
stoicancristi 0:b5906c81772b 28 last_references[i+1] = last_references[i];
stoicancristi 0:b5906c81772b 29 }
stoicancristi 0:b5906c81772b 30 for(int i=0;i<(ordT+1);i++)
stoicancristi 0:b5906c81772b 31 {
stoicancristi 0:b5906c81772b 32 last_outputs[0] = this->out;
stoicancristi 0:b5906c81772b 33 T_component = T_component + T[i]*last_outputs[i];
stoicancristi 0:b5906c81772b 34 }
stoicancristi 0:b5906c81772b 35 for(int i=0;i<ordT;i++)
stoicancristi 0:b5906c81772b 36 {
stoicancristi 0:b5906c81772b 37 last_outputs[i+1] = last_outputs[i];
stoicancristi 0:b5906c81772b 38 }
stoicancristi 0:b5906c81772b 39 for(int i=1;i<(ordS+1);i++)
stoicancristi 0:b5906c81772b 40 {
stoicancristi 0:b5906c81772b 41 S_component = S_component + last_commands[i]*S[i];
stoicancristi 0:b5906c81772b 42 }
stoicancristi 0:b5906c81772b 43
stoicancristi 0:b5906c81772b 44 outputPWM = T_component - R_component - S_component;
stoicancristi 0:b5906c81772b 45
stoicancristi 0:b5906c81772b 46 for(int i=0;i<ordS;i++)
stoicancristi 0:b5906c81772b 47 {
stoicancristi 0:b5906c81772b 48 last_commands[i+1] = last_commands[i];
stoicancristi 0:b5906c81772b 49 }
stoicancristi 0:b5906c81772b 50
stoicancristi 0:b5906c81772b 51 return outputPWM;
stoicancristi 0:b5906c81772b 52 }
stoicancristi 0:b5906c81772b 53
stoicancristi 0:b5906c81772b 54 void RST::updateParams(ControllerParams &cp) {
stoicancristi 0:b5906c81772b 55 ordR = cp.ordR;
stoicancristi 0:b5906c81772b 56 ordS = cp.ordS;
stoicancristi 0:b5906c81772b 57 ordT = cp.ordT;
stoicancristi 0:b5906c81772b 58
stoicancristi 0:b5906c81772b 59 R = new float[ordR+1];
stoicancristi 0:b5906c81772b 60 S = new float[ordS+1];
stoicancristi 0:b5906c81772b 61 T = new float[ordT+1];
stoicancristi 0:b5906c81772b 62
stoicancristi 0:b5906c81772b 63 int i;
stoicancristi 0:b5906c81772b 64 for (i=0; i<ordR+1; i++) R[i] = cp.R[i];
stoicancristi 0:b5906c81772b 65 for (i=0; i<ordS+1; i++) S[i] = cp.S[i];
stoicancristi 0:b5906c81772b 66 for (i=0; i<ordT+1; i++) T[i] = cp.T[i];
stoicancristi 0:b5906c81772b 67 }
stoicancristi 0:b5906c81772b 68
stoicancristi 0:b5906c81772b 69
stoicancristi 0:b5906c81772b 70 RST::~RST() {
stoicancristi 0:b5906c81772b 71 delete [] R;
stoicancristi 0:b5906c81772b 72 delete [] S;
stoicancristi 0:b5906c81772b 73 delete [] T;
stoicancristi 0:b5906c81772b 74 }