TEB programma
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
controller.cpp@49:a9ed4f4cdef7, 2019-10-15 (annotated)
- Committer:
- JornD
- Date:
- Tue Oct 15 11:34:02 2019 +0000
- Revision:
- 49:a9ed4f4cdef7
- Parent:
- 48:39f84359998a
- Child:
- 56:58cbb056e4be
Cleaned the files and commented some stuff
Who changed what in which revision?
| User | Revision | Line number | New 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 | 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 | 47:adbb886ed3cd | 30 | ControllerSettings Set_ProcessEMG; |
| JornD | 48:39f84359998a | 31 | ControllerSettings Set_Base; //Controller base |
| JornD | 48:39f84359998a | 32 | ControllerSettings Set_EndAffector; //Controller end affector |
| JornD | 15:95034d92bc76 | 33 | |
| JornD | 48:39f84359998a | 34 | //Define Memory cells of the Input/Output, shorthand: Mem_ |
| JornD | 48:39f84359998a | 35 | MemoryIO Mem_ProcessEMG; |
| JornD | 48:39f84359998a | 36 | MemoryIO Mem_Base; |
| JornD | 48:39f84359998a | 37 | MemoryIO Mem_EndAffector; |
| JornD | 48:39f84359998a | 38 | |
| JornD | 41:7c4c41326cc6 | 39 | |
| JornD | 48:39f84359998a | 40 | //Build Controllers and define P and Q, the transfer function of the controller |
| JornD | 48:39f84359998a | 41 | //--Build controller, EMG |
| JornD | 48:39f84359998a | 42 | float PE[] = {1,2,3}; |
| JornD | 48:39f84359998a | 43 | float QE[] = {4,5,6}; |
| JornD | 48:39f84359998a | 44 | BuildController(Set_ProcessEMG, PE, QE); |
| JornD | 48:39f84359998a | 45 | //--Build controller, Base |
| JornD | 48:39f84359998a | 46 | float PB[] = {1,2,3}; |
| JornD | 48:39f84359998a | 47 | float QB[] = {4,5,6}; |
| JornD | 48:39f84359998a | 48 | BuildController(Set_Base, PB, QB); |
| JornD | 48:39f84359998a | 49 | //--Build controller, End Affector |
| JornD | 48:39f84359998a | 50 | float PA[] = {1,2,3}; |
| JornD | 48:39f84359998a | 51 | float QA[] = {4,5,6}; |
| JornD | 48:39f84359998a | 52 | BuildController(Set_EndAffector, PA, QA); |
| JornD | 49:a9ed4f4cdef7 | 53 | } |
| JornD | 49:a9ed4f4cdef7 | 54 |