Diff: controller.cpp

Revision:

48:39f84359998a

Parent:

47:adbb886ed3cd

Child:

49:a9ed4f4cdef7

Child:

55:f764f223c56a

--- a/controller.cpp	Mon Oct 14 14:50:15 2019 +0000
+++ b/controller.cpp	Mon Oct 14 20:05:57 2019 +0000
@@ -1,10 +1,22 @@
 #include "structures.h"
 #include <math.h>
 
+    extern float Ts;
+    float K = 2/Ts;
+
+/*
+To call this function, the coefficients variable in the main scope needs to have a 
+"*" in front of it. The coefficients can be printed using:
+
+       for ( int i = 0; i < 5; i++ ) {
+        std::cout << *(coefficient_variable_name+i) << std::endl;
+        }
+
+*/
+/*
 float * Filt(float P[], float Q[])
 {
-    extern float Ts;
-    float K = 2/Ts;
+
     static float coefficients[5];
         coefficients[0] = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
         coefficients[1] = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
@@ -13,23 +25,41 @@
         coefficients[4] = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
     return coefficients;
 } 
+*/
+void BuildController(ControllerSettings& StrucName, float P[], float Q[])
+{
+    
+    StrucName.A = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
+    StrucName.B = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
+    StrucName.C = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
+    StrucName.D = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
+    StrucName.E = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);;
+}
 
 void InitializeControllers(void)
 {
     //Define Controller structures, shorthand: Set_
     ControllerSettings Set_ProcessEMG;
-    
-    //Define Memory cells, shorthand: Mem_
-    MemoryIO MemOne;
+    ControllerSettings Set_Base;            //Controller base
+    ControllerSettings Set_EndAffector;     //Controller end affector 
     
-    //Write controller setting values to structures
-    float *CoeffOne[];
-    float CoeffOne = Filt(P[],Q[]);
+    //Define Memory cells of the Input/Output, shorthand: Mem_
+    MemoryIO Mem_ProcessEMG;
+    MemoryIO Mem_Base;
+    MemoryIO Mem_EndAffector;
+    
     
-    SetOne.A = CoeffOne[0];
-    SetOne.B = CoeffOne[1];
-    SetOne.C = CoeffOne[2];
-    SetOne.D = CoeffOne[3];
-    SetOne.E = CoeffOne[4];
-    
+    //Build Controllers and define P and Q, the transfer function of the controller
+    //--Build controller, EMG
+        float PE[] = {1,2,3};
+        float QE[] = {4,5,6};
+        BuildController(Set_ProcessEMG, PE, QE);
+    //--Build controller, Base
+        float PB[] = {1,2,3};
+        float QB[] = {4,5,6};
+        BuildController(Set_Base, PB, QB);
+    //--Build controller, End Affector
+        float PA[] = {1,2,3};
+        float QA[] = {4,5,6};
+        BuildController(Set_EndAffector, PA, QA);
 }
\ No newline at end of file