DP
Dependencies: FastAnalogIn mbed-rtos mbed
Fork of dipl_prace_v10 by
control.cpp@1:28d74f044818, 2015-04-28 (annotated)
- Committer:
- romankrej
- Date:
- Tue Apr 28 18:48:50 2015 +0000
- Revision:
- 1:28d74f044818
- Parent:
- 0:f3b355df6f26
DP
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 | 1:28d74f044818 | 6 | //K[0] = 10.0; K[1] = 2.4; K[2] = -0.15; K[3] = -0.25; |
romankrej | 0:f3b355df6f26 | 7 | q[0] = 24; |
romankrej | 0:f3b355df6f26 | 8 | q[1] = -39.999; |
romankrej | 0:f3b355df6f26 | 9 | q[2] = 16; |
romankrej | 0:f3b355df6f26 | 10 | e[0] = 0; |
romankrej | 0:f3b355df6f26 | 11 | e[1] = 0; |
romankrej | 0:f3b355df6f26 | 12 | e[2] = 0; |
romankrej | 0:f3b355df6f26 | 13 | u_old = 0; |
romankrej | 0:f3b355df6f26 | 14 | } |
romankrej | 0:f3b355df6f26 | 15 | |
romankrej | 0:f3b355df6f26 | 16 | void cControl::setCurrent() { |
romankrej | 0:f3b355df6f26 | 17 | float temp = 0; |
romankrej | 0:f3b355df6f26 | 18 | if(programMode == RUNNING) { |
romankrej | 0:f3b355df6f26 | 19 | /* STATE FEEDBACK */ |
romankrej | 0:f3b355df6f26 | 20 | if(controller == ST_FEEDBACK) { |
romankrej | 0:f3b355df6f26 | 21 | temp = -K[0]*states.phi1 - K[1]*states.omega1 - K[2]*states.phi2 - K[3]*states.omega2; |
romankrej | 0:f3b355df6f26 | 22 | |
romankrej | 0:f3b355df6f26 | 23 | if(temp > 10.0) |
romankrej | 0:f3b355df6f26 | 24 | states.current = 10.0; |
romankrej | 0:f3b355df6f26 | 25 | |
romankrej | 0:f3b355df6f26 | 26 | else { |
romankrej | 0:f3b355df6f26 | 27 | |
romankrej | 0:f3b355df6f26 | 28 | if(temp < -10.0) |
romankrej | 0:f3b355df6f26 | 29 | states.current = -10.0; |
romankrej | 0:f3b355df6f26 | 30 | |
romankrej | 0:f3b355df6f26 | 31 | else |
romankrej | 0:f3b355df6f26 | 32 | states.current = temp; |
romankrej | 0:f3b355df6f26 | 33 | } |
romankrej | 0:f3b355df6f26 | 34 | } |
romankrej | 0:f3b355df6f26 | 35 | /* PID CONTROLLER */ |
romankrej | 0:f3b355df6f26 | 36 | else { |
romankrej | 0:f3b355df6f26 | 37 | float temp; |
romankrej | 0:f3b355df6f26 | 38 | e[0] = -states.phi1; |
romankrej | 0:f3b355df6f26 | 39 | temp = u_old + q[0]*e[0] + q[1]*e[1] + q[2]*e[2]; |
romankrej | 0:f3b355df6f26 | 40 | if(temp > 10.0) |
romankrej | 0:f3b355df6f26 | 41 | states.current = 10.0; |
romankrej | 0:f3b355df6f26 | 42 | else { |
romankrej | 0:f3b355df6f26 | 43 | if(temp < -10.0) |
romankrej | 0:f3b355df6f26 | 44 | states.current = -10.0; |
romankrej | 0:f3b355df6f26 | 45 | else |
romankrej | 0:f3b355df6f26 | 46 | states.current = temp; |
romankrej | 0:f3b355df6f26 | 47 | } |
romankrej | 0:f3b355df6f26 | 48 | |
romankrej | 0:f3b355df6f26 | 49 | e[1] = e[0]; |
romankrej | 0:f3b355df6f26 | 50 | e[2] = e[1]; |
romankrej | 0:f3b355df6f26 | 51 | u_old = states.current; |
romankrej | 0:f3b355df6f26 | 52 | } |
romankrej | 0:f3b355df6f26 | 53 | } |
romankrej | 0:f3b355df6f26 | 54 | else |
romankrej | 0:f3b355df6f26 | 55 | states.current = 0.0; |
romankrej | 0:f3b355df6f26 | 56 | } |