/*
 * position.c
 *
 *    Author: Andrey Koryagin https://blog.avislab.com/
 */

#include "u_foc.h"

#define POSITION_MAX 4095

extern volatile SettingsStruct Settings;

PIDstruct PositionPID;

int32_t POSITION_TASK = 0;

int32_t Position_Calc () {
	int32_t pos;
	pos = (int32_t)MC_ADC_GetRegularValue(ADC_POT) - POSITION_TASK;
	//pos = POSITION_TASK - (int32_t)MC_ADC_GetRegularValue(ADC_POT);
	if (pos > POSITION_MAX/2) {
		pos = pos - POSITION_MAX;
	}
	if (pos < -POSITION_MAX/2) {
		pos = POSITION_MAX + pos;
	}
	return pos;
}

void Position_Init () {
	PositionPID.kp = 50000;
	PositionPID.ki = 50000;
	PositionPID.kd = 0;
	PositionPID.max = Settings.TorqueMax;
	PositionPID.min = -Settings.TorqueMax;
	//PositionPID.max = Settings.RpmMax;
	//PositionPID.min = -Settings.RpmMax;

	PositionPID.task = 0;
	PositionPID.input = Position_Calc();

	MC_SetMode(MC_MODE_TORQUE);
	MC_SetTorque(0);
}

void Position_Process () {
	PositionPID.input = Position_Calc();
	/*
	int16_t speed = -(int16_t)PI_Compute(&PositionPID);
	if ((-200 < speed) & (speed < 200) ) {
		speed=0;
	}
	*/
	//MC_SetSpeed(speed);
	int16_t torque = -(int16_t)PI_Compute(&PositionPID);
	MC_SetTorque(torque);
}

void Position_Set (uint16_t position) {
	if (position > POSITION_MAX) {
		position = POSITION_MAX;
	}

	POSITION_TASK = (int32_t)position;
}

uint16_t Position_Get () {
	return (uint16_t)POSITION_TASK;
}

uint16_t Position_Get_Real () {
	return (uint16_t) MC_ADC_GetRegularValue(ADC_POT);
}