Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@56:58cbb056e4be, 2019-10-15 (annotated)

Committer:
JornD
Date:
Tue Oct 15 13:01:10 2019 +0000
Branch:
Branch2
Revision:
56:58cbb056e4be
Parent:
49:a9ed4f4cdef7
Child:
58:897b67113094
=ERROR FREE= ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornD 40:82addb417220 1 #include "structures.h"
JornD 47:adbb886ed3cd 2 #include <math.h>
JornD 56:58cbb056e4be 3 #include "global.cpp"
JornD 43:9579a1afe9cb 4
JornD 56:58cbb056e4be 5 extern const float Ts;
JornD 48:39f84359998a 6 float K = 2/Ts;
JornD 48:39f84359998a 7
JornD 48:39f84359998a 8 /*
JornD 48:39f84359998a 9 To call this function, the coefficients variable in the main scope needs to have a
JornD 48:39f84359998a 10 "*" in front of it. The coefficients can be printed using:
JornD 48:39f84359998a 11
JornD 48:39f84359998a 12 for ( int i = 0; i < 5; i++ ) {
JornD 48:39f84359998a 13 std::cout << *(coefficient_variable_name+i) << std::endl;
JornD 48:39f84359998a 14 }
JornD 48:39f84359998a 15
JornD 48:39f84359998a 16 */
JornD 48:39f84359998a 17
JornD 48:39f84359998a 18 void BuildController(ControllerSettings& StrucName, float P[], float Q[])
JornD 48:39f84359998a 19 {
JornD 48:39f84359998a 20
JornD 48:39f84359998a 21 StrucName.A = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 22 StrucName.B = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 23 StrucName.C = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 24 StrucName.D = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 25 StrucName.E = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);;
JornD 48:39f84359998a 26 }
JornD 41:7c4c41326cc6 27
JornD 40:82addb417220 28 void InitializeControllers(void)
JornD 15:95034d92bc76 29 {
JornD 43:9579a1afe9cb 30 //Define Controller structures, shorthand: Set_
JornD 47:adbb886ed3cd 31 ControllerSettings Set_ProcessEMG;
JornD 48:39f84359998a 32 ControllerSettings Set_Base; //Controller base
JornD 48:39f84359998a 33 ControllerSettings Set_EndAffector; //Controller end affector
JornD 15:95034d92bc76 34
JornD 48:39f84359998a 35 //Define Memory cells of the Input/Output, shorthand: Mem_
JornD 48:39f84359998a 36 MemoryIO Mem_ProcessEMG;
JornD 48:39f84359998a 37 MemoryIO Mem_Base;
JornD 48:39f84359998a 38 MemoryIO Mem_EndAffector;
JornD 48:39f84359998a 39
JornD 41:7c4c41326cc6 40
JornD 48:39f84359998a 41 //Build Controllers and define P and Q, the transfer function of the controller
JornD 48:39f84359998a 42 //--Build controller, EMG
JornD 48:39f84359998a 43 float PE[] = {1,2,3};
JornD 48:39f84359998a 44 float QE[] = {4,5,6};
JornD 56:58cbb056e4be 45 //BuildController(Set_ProcessEMG, PE, QE);
JornD 48:39f84359998a 46 //--Build controller, Base
JornD 48:39f84359998a 47 float PB[] = {1,2,3};
JornD 48:39f84359998a 48 float QB[] = {4,5,6};
JornD 56:58cbb056e4be 49 //BuildController(Set_Base, PB, QB);
JornD 48:39f84359998a 50 //--Build controller, End Affector
JornD 48:39f84359998a 51 float PA[] = {1,2,3};
JornD 48:39f84359998a 52 float QA[] = {4,5,6};
JornD 56:58cbb056e4be 53 //BuildController(Set_EndAffector, PA, QA);
JornD 56:58cbb056e4be 54
JornD 49:a9ed4f4cdef7 55 }
JornD 49:a9ed4f4cdef7 56