/*
 * user_code.c
 *
 *  Created on: 22 θών. 2020 γ.
 *      Author: andre
 */


/*
 * user_code.c
 *
 *  Created on: 21 θών. 2020 γ.
 *      Author: andre
 */
#include "motorcontrol.h"
# include "stdlib.h"
#include "stm32g4xx_ll_adc.h"

#define SPEED_MAX	MAX_APPLICATION_SPEED_RPM
#define SPEED_MIN	OBS_MINIMUM_SPEED_RPM

#define POT_MAX		4000
#define POT_START	200
#define POT_STOP	50


#define POT_SPEED_ARRAY_SIZE	4
#define POT_SPEED_DELTA	10

int32_t POT_SPEED;
int32_t POT_SPEED_ARRAY[POT_SPEED_ARRAY_SIZE];
int32_t POT_SPEED_AVERAGE;
int32_t POT_SPEED_INDEX;


RegConv_t PotentiometerConv;
uint8_t PotentiometerHandle;

void Potentiometer_Init() {
	uint8_t i;
	for (i=0; i<POT_SPEED_ARRAY_SIZE; i++) {
		POT_SPEED_ARRAY[i] = 0;
	}
	POT_SPEED_INDEX=0;
	POT_SPEED = 0;

	PotentiometerConv.regADC = ADC2;
	PotentiometerConv.channel = 8;
	PotentiometerConv.samplingTime = LL_ADC_SAMPLINGTIME_6CYCLES_5;
	PotentiometerHandle = RCM_RegisterRegConv(&PotentiometerConv);
}

void Potentiometer_Process() {
	uint8_t i;
	uint16_t ADC_VALUE;

	State_t motorState = MC_GetSTMStateMotor1();
	if(RCM_GetUserConvState() == RCM_USERCONV_IDLE)
	{
		RCM_RequestUserConv(PotentiometerHandle);
	}
	else if (RCM_GetUserConvState() == RCM_USERCONV_EOC)
	{
		ADC_VALUE = RCM_GetUserConv()>>4;

		POT_SPEED_ARRAY[POT_SPEED_INDEX] = SPEED_MIN + ( (SPEED_MAX-SPEED_MIN) * ((int32_t)ADC_VALUE - POT_STOP) / (POT_MAX-POT_STOP) ) ;
		POT_SPEED_INDEX++;
		if (POT_SPEED_INDEX>=POT_SPEED_ARRAY_SIZE) {
			POT_SPEED_INDEX=0;
		}
		POT_SPEED_AVERAGE = 0;
		for (i=0; i<POT_SPEED_ARRAY_SIZE; i++) {
			POT_SPEED_AVERAGE = POT_SPEED_AVERAGE + POT_SPEED_ARRAY[i];
		}
		POT_SPEED_AVERAGE = POT_SPEED_AVERAGE/POT_SPEED_ARRAY_SIZE;

		if ( abs(POT_SPEED_AVERAGE - POT_SPEED) > POT_SPEED_DELTA) {
			POT_SPEED = POT_SPEED_AVERAGE;
			MC_ProgramSpeedRampMotor1((int16_t)(POT_SPEED/6), 100);
		}

		if( ADC_VALUE > POT_START ) {
			if (motorState == IDLE) {
			  MC_StartMotor1();
			}
		}
		else {
			if( ADC_VALUE < POT_STOP ) {
				if (motorState == RUN) {
			  	  MC_StopMotor1();
				}
			}
		}

	}

}
