Controllers moved to separate file.

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / controllers.c

diff --git a/pmsm-control/test_sw/controllers.c b/pmsm-control/test_sw/controllers.c
new file mode 100644 (file)
index 0000000..059ade6
--- /dev/null
+++ b/pmsm-control/test_sw/controllers.c
@@ -0,0 +1,40 @@
+
+#include "controllers.h"
+
+/*
+ * \brief
+ * Very simple PID regulator.
+ * Now only with P-part so that the error doesnt go to zero.
+ * TODO: add anti-wind up and I and D parts
+ */
+void pos_pid(struct rpi_state* state){
+       int duty_tmp;
+       duty_tmp = PID_P*(state->desired_pos - (int32_t)state->spi_dat->pozice);
+       if (duty_tmp>MAX_DUTY){
+               state->duty=MAX_DUTY;
+       }else if (duty_tmp<-MAX_DUTY){
+               state->duty=-MAX_DUTY;
+       }else{
+               state->duty = duty_tmp;
+       }
+}
+
+/*
+ * \brief
+ * Very simple PID regulator.
+ * FIXME: make better
+ */
+void spd_pid(struct rpi_state* state){
+       int duty_tmp;
+       int error;
+       error=state->desired_spd - state->speed;
+       state->spd_err_sum+=error;
+       duty_tmp = PID_P_S*error+PID_I_S*state->spd_err_sum;
+       if (duty_tmp>MAX_DUTY){
+               state->duty=MAX_DUTY;
+       }else if (duty_tmp<-MAX_DUTY){
+               state->duty=-MAX_DUTY;
+       }else{
+               state->duty = duty_tmp;
+       }
+}