DP
Dependencies: FastAnalogIn mbed-rtos mbed
Fork of dipl_prace_v10 by
Diff: control.cpp
- Revision:
- 0:f3b355df6f26
- Child:
- 1:28d74f044818
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/control.cpp Sun Apr 26 13:14:02 2015 +0000 @@ -0,0 +1,55 @@ +#include "control.h" +#include "threads.h" + +cControl::cControl() { + K[0] = 9.6; K[1] = 2.3; K[2] = -0.12; K[3] = -0.2; + q[0] = 24; + q[1] = -39.999; + q[2] = 16; + e[0] = 0; + e[1] = 0; + e[2] = 0; + u_old = 0; +} + +void cControl::setCurrent() { + float temp = 0; + if(programMode == RUNNING) { + /* STATE FEEDBACK */ + if(controller == ST_FEEDBACK) { + temp = -K[0]*states.phi1 - K[1]*states.omega1 - K[2]*states.phi2 - K[3]*states.omega2; + + if(temp > 10.0) + states.current = 10.0; + + else { + + if(temp < -10.0) + states.current = -10.0; + + else + states.current = temp; + } + } + /* PID CONTROLLER */ + else { + float temp; + e[0] = -states.phi1; + temp = u_old + q[0]*e[0] + q[1]*e[1] + q[2]*e[2]; + if(temp > 10.0) + states.current = 10.0; + else { + if(temp < -10.0) + states.current = -10.0; + else + states.current = temp; + } + + e[1] = e[0]; + e[2] = e[1]; + u_old = states.current; + } + } + else + states.current = 0.0; +} \ No newline at end of file