DP
Dependencies: FastAnalogIn mbed-rtos mbed
Fork of dipl_prace_v10 by
control.cpp
- Committer:
- romankrej
- Date:
- 2015-04-26
- Revision:
- 0:f3b355df6f26
- Child:
- 1:28d74f044818
File content as of revision 0:f3b355df6f26:
#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; }