Roman Krejci / Mbed 2 deprecated dipl_prace_v10

DP

Dependencies:   FastAnalogIn mbed-rtos mbed

control.cpp@0:f3b355df6f26, 2015-04-26 (annotated)

Committer:
romankrej
Date:
Sun Apr 26 13:14:02 2015 +0000
Revision:
0:f3b355df6f26
Diplomova prace 2015

Who changed what in which revision?

UserRevisionLine numberNew contents of line
romankrej 0:f3b355df6f26 1 #include "control.h"
romankrej 0:f3b355df6f26 2 #include "threads.h"
romankrej 0:f3b355df6f26 3
romankrej 0:f3b355df6f26 4 cControl::cControl() {
romankrej 0:f3b355df6f26 5 K[0] = 9.6; K[1] = 2.3; K[2] = -0.12; K[3] = -0.2;
romankrej 0:f3b355df6f26 6 q[0] = 24;
romankrej 0:f3b355df6f26 7 q[1] = -39.999;
romankrej 0:f3b355df6f26 8 q[2] = 16;
romankrej 0:f3b355df6f26 9 e[0] = 0;
romankrej 0:f3b355df6f26 10 e[1] = 0;
romankrej 0:f3b355df6f26 11 e[2] = 0;
romankrej 0:f3b355df6f26 12 u_old = 0;
romankrej 0:f3b355df6f26 13 }
romankrej 0:f3b355df6f26 14
romankrej 0:f3b355df6f26 15 void cControl::setCurrent() {
romankrej 0:f3b355df6f26 16 float temp = 0;
romankrej 0:f3b355df6f26 17 if(programMode == RUNNING) {
romankrej 0:f3b355df6f26 18 /* STATE FEEDBACK */
romankrej 0:f3b355df6f26 19 if(controller == ST_FEEDBACK) {
romankrej 0:f3b355df6f26 20 temp = -K[0]*states.phi1 - K[1]*states.omega1 - K[2]*states.phi2 - K[3]*states.omega2;
romankrej 0:f3b355df6f26 21
romankrej 0:f3b355df6f26 22 if(temp > 10.0)
romankrej 0:f3b355df6f26 23 states.current = 10.0;
romankrej 0:f3b355df6f26 24
romankrej 0:f3b355df6f26 25 else {
romankrej 0:f3b355df6f26 26
romankrej 0:f3b355df6f26 27 if(temp < -10.0)
romankrej 0:f3b355df6f26 28 states.current = -10.0;
romankrej 0:f3b355df6f26 29
romankrej 0:f3b355df6f26 30 else
romankrej 0:f3b355df6f26 31 states.current = temp;
romankrej 0:f3b355df6f26 32 }
romankrej 0:f3b355df6f26 33 }
romankrej 0:f3b355df6f26 34 /* PID CONTROLLER */
romankrej 0:f3b355df6f26 35 else {
romankrej 0:f3b355df6f26 36 float temp;
romankrej 0:f3b355df6f26 37 e[0] = -states.phi1;
romankrej 0:f3b355df6f26 38 temp = u_old + q[0]*e[0] + q[1]*e[1] + q[2]*e[2];
romankrej 0:f3b355df6f26 39 if(temp > 10.0)
romankrej 0:f3b355df6f26 40 states.current = 10.0;
romankrej 0:f3b355df6f26 41 else {
romankrej 0:f3b355df6f26 42 if(temp < -10.0)
romankrej 0:f3b355df6f26 43 states.current = -10.0;
romankrej 0:f3b355df6f26 44 else
romankrej 0:f3b355df6f26 45 states.current = temp;
romankrej 0:f3b355df6f26 46 }
romankrej 0:f3b355df6f26 47
romankrej 0:f3b355df6f26 48 e[1] = e[0];
romankrej 0:f3b355df6f26 49 e[2] = e[1];
romankrej 0:f3b355df6f26 50 u_old = states.current;
romankrej 0:f3b355df6f26 51 }
romankrej 0:f3b355df6f26 52 }
romankrej 0:f3b355df6f26 53 else
romankrej 0:f3b355df6f26 54 states.current = 0.0;
romankrej 0:f3b355df6f26 55 }