X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/b98f70ee3eed3af0cbbc9d5869ac1e7626534ef4..34c6632a92d19c06bbdef9dcc4eb754b21ade81d:/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
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;
+	}
+}