Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@47:adbb886ed3cd, 2019-10-14 (annotated)

Committer:
JornD
Date:
Mon Oct 14 14:50:15 2019 +0000
Revision:
47:adbb886ed3cd
Parent:
43:9579a1afe9cb
Child:
48:39f84359998a
Implemented Rens' code;

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 43:9579a1afe9cb 3
JornD 47:adbb886ed3cd 4 float * Filt(float P[], float Q[])
JornD 41:7c4c41326cc6 5 {
JornD 47:adbb886ed3cd 6 extern float Ts;
JornD 47:adbb886ed3cd 7 float K = 2/Ts;
JornD 47:adbb886ed3cd 8 static float coefficients[5];
JornD 47:adbb886ed3cd 9 coefficients[0] = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 47:adbb886ed3cd 10 coefficients[1] = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 47:adbb886ed3cd 11 coefficients[2] = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 47:adbb886ed3cd 12 coefficients[3] = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 47:adbb886ed3cd 13 coefficients[4] = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
JornD 47:adbb886ed3cd 14 return coefficients;
JornD 47:adbb886ed3cd 15 }
JornD 41:7c4c41326cc6 16
JornD 40:82addb417220 17 void InitializeControllers(void)
JornD 15:95034d92bc76 18 {
JornD 43:9579a1afe9cb 19 //Define Controller structures, shorthand: Set_
JornD 47:adbb886ed3cd 20 ControllerSettings Set_ProcessEMG;
JornD 41:7c4c41326cc6 21
JornD 43:9579a1afe9cb 22 //Define Memory cells, shorthand: Mem_
JornD 40:82addb417220 23 MemoryIO MemOne;
JornD 15:95034d92bc76 24
JornD 41:7c4c41326cc6 25 //Write controller setting values to structures
JornD 47:adbb886ed3cd 26 float *CoeffOne[];
JornD 47:adbb886ed3cd 27 float CoeffOne = Filt(P[],Q[]);
JornD 41:7c4c41326cc6 28
JornD 43:9579a1afe9cb 29 SetOne.A = CoeffOne[0];
JornD 43:9579a1afe9cb 30 SetOne.B = CoeffOne[1];
JornD 43:9579a1afe9cb 31 SetOne.C = CoeffOne[2];
JornD 43:9579a1afe9cb 32 SetOne.D = CoeffOne[3];
JornD 43:9579a1afe9cb 33 SetOne.E = CoeffOne[4];
JornD 41:7c4c41326cc6 34
JornD 15:95034d92bc76 35 }