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?
User | Revision | Line number | New 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 | } |