Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@62:44f5c366110b, 2019-10-16 (annotated)

Committer:
JornD
Date:
Wed Oct 16 10:17:23 2019 +0000
Branch:
Branch2
Revision:
62:44f5c366110b
Parent:
61:4c7de1e2f9fe
Child:
64:5a6bf0cd1c50
WORKING - Fixed mistake in controller ;

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 void BuildController(ControllerSettings& StrucName, float P[], float Q[])
JornD 48:39f84359998a 18 {
JornD 48:39f84359998a 19
JornD 48:39f84359998a 20 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 21 StrucName.B = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 22 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 23 StrucName.D = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 24 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 25 }
JornD 41:7c4c41326cc6 26
JornD 40:82addb417220 27 void InitializeControllers(void)
JornD 15:95034d92bc76 28 {
JornD 43:9579a1afe9cb 29 //Define Controller structures, shorthand: Set_
JornD 61:4c7de1e2f9fe 30 ControllerSettings Set_LPFEMG;
JornD 61:4c7de1e2f9fe 31 ControllerSettings Set_NOTEMG;
JornD 61:4c7de1e2f9fe 32 //--
JornD 48:39f84359998a 33 ControllerSettings Set_Base; //Controller base
JornD 48:39f84359998a 34 ControllerSettings Set_EndAffector; //Controller end affector
JornD 15:95034d92bc76 35
JornD 48:39f84359998a 36 //Define Memory cells of the Input/Output, shorthand: Mem_
JornD 61:4c7de1e2f9fe 37 MemoryIO Mem_LPFEMG;
JornD 61:4c7de1e2f9fe 38 MemoryIO Mem_NOTEMG;
JornD 61:4c7de1e2f9fe 39 //--
JornD 48:39f84359998a 40 MemoryIO Mem_Base;
JornD 48:39f84359998a 41 MemoryIO Mem_EndAffector;
JornD 48:39f84359998a 42
JornD 41:7c4c41326cc6 43
JornD 48:39f84359998a 44 //Build Controllers and define P and Q, the transfer function of the controller
JornD 61:4c7de1e2f9fe 45 //--Build LPF controller, EMG
JornD 61:4c7de1e2f9fe 46 float PL[] = {1,2,3};
JornD 61:4c7de1e2f9fe 47 float QL[] = {4,5,6};
JornD 61:4c7de1e2f9fe 48 BuildController(Set_LPFEMG, PL, QL);
JornD 61:4c7de1e2f9fe 49 //--Build Notch filter controller, EMG
JornD 61:4c7de1e2f9fe 50 float PN[] = {1,2,3};
JornD 61:4c7de1e2f9fe 51 float QN[] = {4,5,6};
JornD 61:4c7de1e2f9fe 52 BuildController(Set_NOTEMG, PN, QN);
JornD 48:39f84359998a 53 //--Build controller, Base
JornD 48:39f84359998a 54 float PB[] = {1,2,3};
JornD 48:39f84359998a 55 float QB[] = {4,5,6};
JornD 58:897b67113094 56 BuildController(Set_Base, PB, QB);
JornD 48:39f84359998a 57 //--Build controller, End Affector
JornD 61:4c7de1e2f9fe 58 float PE[] = {1,2,3};
JornD 61:4c7de1e2f9fe 59 float QE[] = {4,5,6};
JornD 61:4c7de1e2f9fe 60 BuildController(Set_EndAffector, PE, QE);
JornD 49:a9ed4f4cdef7 61 }
JornD 49:a9ed4f4cdef7 62
JornD 58:897b67113094 63 float Biquad(ControllerSettings& StrucName, MemoryIO& MemName, float X)
JornD 58:897b67113094 64 {
JornD 58:897b67113094 65 //Difference equation which implements the digital biquad filter
JornD 62:44f5c366110b 66 float Y = StrucName.A * X + StrucName.B * MemName.Xm + StrucName.C * MemName.Xmm - StrucName.D * MemName.Ym - StrucName.E * MemName.Ymm;
JornD 58:897b67113094 67
JornD 61:4c7de1e2f9fe 68 MemName.ShiftValues(X,Y);
JornD 61:4c7de1e2f9fe 69
JornD 58:897b67113094 70 return Y;
JornD 58:897b67113094 71 }
JornD 58:897b67113094 72