Jorn Dokter / Mbed 2 deprecated TEB_branch2

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@55:f764f223c56a, 2019-10-15 (annotated)

Committer:
Rdeb36
Date:
Tue Oct 15 12:19:22 2019 +0000
Revision:
55:f764f223c56a
Parent:
48:39f84359998a 
Digital Biquad filter, without updating memory (code Jorn)

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