Roman Krejci / Mbed 2 deprecated DP_28_4_2015

DP

Dependencies:   FastAnalogIn mbed-rtos mbed

Fork of dipl_prace_v10 by Roman Krejci

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?

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 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 }