Horia Carbune / Mbed 2 deprecated nRF51822_BLE_Control_Refactor

Refactoring and other updates

Dependencies:   BLE_API mbed nRF51822

Fork of nRF51822_SimpleChat by Cristi Stoican

RSTController.cpp@7:806b08205b25, 2017-05-10 (annotated)

Committer:
carbune92
Date:
Wed May 10 07:15:19 2017 +0000
Revision:
7:806b08205b25
Parent:
3:b6e4e5529a52 
fixed minor issues

Who changed what in which revision?

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