#include "controllers.h"

/**
 * \brief No regulation.
 */
inline void zero_controller(struct rpi_state* state){
	return;
}

/*
 * \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;
	}
}