Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@64:5a6bf0cd1c50, 2019-10-16 (annotated)

Committer:
JornD
Date:
Wed Oct 16 10:58:23 2019 +0000
Branch:
Branch2
Revision:
64:5a6bf0cd1c50
Parent:
62:44f5c366110b 
WORKING - Moved structures and constants to "global.h";

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 64:5a6bf0cd1c50 3 #include "global.h"
JornD 43:9579a1afe9cb 4
JornD 48:39f84359998a 5 float K = 2/Ts;
JornD 48:39f84359998a 6
JornD 48:39f84359998a 7 /*
JornD 48:39f84359998a 8 To call this function, the coefficients variable in the main scope needs to have a
JornD 48:39f84359998a 9 "*" in front of it. The coefficients can be printed using:
JornD 48:39f84359998a 10
JornD 48:39f84359998a 11 for ( int i = 0; i < 5; i++ ) {
JornD 48:39f84359998a 12 std::cout << *(coefficient_variable_name+i) << std::endl;
JornD 48:39f84359998a 13 }
JornD 48:39f84359998a 14 */
JornD 48:39f84359998a 15
JornD 48:39f84359998a 16 void BuildController(ControllerSettings& StrucName, float P[], float Q[])
JornD 48:39f84359998a 17 {
JornD 48:39f84359998a 18
JornD 48:39f84359998a 19 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 20 StrucName.B = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 21 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 22 StrucName.D = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 48:39f84359998a 23 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 24 }
JornD 41:7c4c41326cc6 25
JornD 40:82addb417220 26 void InitializeControllers(void)
JornD 15:95034d92bc76 27 {
JornD 48:39f84359998a 28 //Build Controllers and define P and Q, the transfer function of the controller
JornD 61:4c7de1e2f9fe 29 //--Build LPF controller, EMG
JornD 61:4c7de1e2f9fe 30 float PL[] = {1,2,3};
JornD 61:4c7de1e2f9fe 31 float QL[] = {4,5,6};
JornD 61:4c7de1e2f9fe 32 BuildController(Set_LPFEMG, PL, QL);
JornD 61:4c7de1e2f9fe 33 //--Build Notch filter controller, EMG
JornD 61:4c7de1e2f9fe 34 float PN[] = {1,2,3};
JornD 61:4c7de1e2f9fe 35 float QN[] = {4,5,6};
JornD 61:4c7de1e2f9fe 36 BuildController(Set_NOTEMG, PN, QN);
JornD 48:39f84359998a 37 //--Build controller, Base
JornD 48:39f84359998a 38 float PB[] = {1,2,3};
JornD 48:39f84359998a 39 float QB[] = {4,5,6};
JornD 58:897b67113094 40 BuildController(Set_Base, PB, QB);
JornD 48:39f84359998a 41 //--Build controller, End Affector
JornD 61:4c7de1e2f9fe 42 float PE[] = {1,2,3};
JornD 61:4c7de1e2f9fe 43 float QE[] = {4,5,6};
JornD 61:4c7de1e2f9fe 44 BuildController(Set_EndAffector, PE, QE);
JornD 49:a9ed4f4cdef7 45 }
JornD 49:a9ed4f4cdef7 46
JornD 58:897b67113094 47 float Biquad(ControllerSettings& StrucName, MemoryIO& MemName, float X)
JornD 58:897b67113094 48 {
JornD 58:897b67113094 49 //Difference equation which implements the digital biquad filter
JornD 62:44f5c366110b 50 float Y = StrucName.A * X + StrucName.B * MemName.Xm + StrucName.C * MemName.Xmm - StrucName.D * MemName.Ym - StrucName.E * MemName.Ymm;
JornD 58:897b67113094 51
JornD 61:4c7de1e2f9fe 52 MemName.ShiftValues(X,Y);
JornD 61:4c7de1e2f9fe 53
JornD 58:897b67113094 54 return Y;
JornD 58:897b67113094 55 }
JornD 58:897b67113094 56