/*
 * bldc.c
 *
 * Created: 10/03/2021 14:53:19
 *  Author: Nick-XMG
 */ 


#include "bldc.h"
#include "utilities.h"
// ----------------------------------------------------------------------
// 	header files
// ----------------------------------------------------------------------
#include "arm_math.h"
#include "statemachine.h"



//// ------------------------------------------------------------------------------
// Current Selection Based on Hall State
// Direction":
// CW  -> Always positive current
// CCW -> Always negative current
void select_active_phase(volatile BLDCMotor_t *Motor, volatile uint8_t hall_state)
{
	volatile float32_t phase_current = 0;
	switch(hall_state)
	{
		case 0b001:	case 0b011:
		phase_current = Motor->Iphase_pu.C;
		break;
		
		case 0b010:	case 0b110:
		phase_current = Motor->Iphase_pu.B;
		break;
				
		case 0b100: case 0b101: 
		phase_current = Motor->Iphase_pu.A;
		break;
				
		default : 
		//phase_current = 0;	// Invalid hall code
		break;
	}
	
	Motor->Iphase_pu.Bus = phase_current;
}

void BldcInitStruct(BLDCMotor_t *motor)
{
	// ----------------------------------------------------------------------
	//  Initialize all voltage and current objects, variables and helpers:
	motor->motor_status.actualDirection		= 0;
	motor->motor_setpoints.desiredDirection	= 0;
	motor->motor_status.duty_cycle			= 0;
	motor->motor_status.calc_rpm			= 0;
	motor->motor_status.Num_Steps			= 0;
	motor->motor_status.cur_comm_step		= 0;
	motor->motor_status.currentHallPattern	= 1;
	motor->motor_status.nextHallPattern		= 3;
	motor->motor_setpoints.directionOffset	= 0;
		
	//motor->hall_state = 1;
	//motor->dir_hall_code  = 1;
	
	motor->Iphase_pu.A						= 0;
	motor->Iphase_pu.B						= 0;
	motor->Iphase_pu.C						= 0;
	motor->Iphase_pu.Bus					= 0;
	motor->Voffset_lsb.A					= 0;
	motor->Voffset_lsb.B					= 0;
	motor->timerflags.pwm_cycle_tic			= false;
	motor->timerflags.control_loop_tic		= false;
	motor->timerflags.motor_telemetry_flag	= false;
	motor->timerflags.control_loop_tic		= false;
	motor->regulation_loop_count			= 0;

	
	motor->VdcBus_pu = DEVICE_DC_VOLTAGE_V;
	motor->VoneByDcBus_pu = 1.0f/DEVICE_DC_VOLTAGE_V;
	motor->motor_commutation_Pattern = COMMUTATION_PATTERN_M1;
	
	motor->motor_setpoints.desired_torque	= 0.0;
	motor->motor_setpoints.desired_speed	= 0;
	motor->motor_setpoints.desired_position = 0;
	motor->motor_setpoints.max_current		= 0.0;
	motor->motor_setpoints.max_torque		= 0.0;
	motor->motor_setpoints.max_velocity		= 0;
	
	// ------------------------------------------------------------------------------
	// Function
	// ------------------------------------------------------------------------------
	//motor->ReadHall = HallFunc;
	
	//// ------------------------------------------------------------------------------
	////  pi current control init
	PI_objectInit(&motor->controllers.Pi_Idc);
	float motorLs_H   = MOTOR_LQ_H   * 2.0f;
	float motorRs_OHM = MOTOR_RS_OHM * 2.0f;
	motor->controllers.Pi_Idc.Kp = PI_calcKp(motorLs_H, DEVICE_SHUNT_CURRENT_A, DEVICE_DC_VOLTAGE_V, DEVICE_ISR_PERIOD_Sec);
	//Pi_Idc.Ki = 0;
	motor->controllers.Pi_Idc.Ki = PI_calcKi(motorRs_OHM, motorLs_H, DEVICE_ISR_PERIOD_Sec);
	
	//// ------------------------------------------------------------------------------
	////  pi velocity control init
	PID_objectInit(&motor->controllers.Pid_Speed);
	
	/* V only Control Gains */
	//motor->controllers.Pid_Speed.Kp = 0.005f;
	//motor->controllers.Pid_Speed.Ki = 0.01f;
	
	/* VI Control Gains */
	//motor->controllers.Pid_Speed.Kp = 0.0003f;
	//motor->controllers.Pid_Speed.Ki = 0.001f;
	motor->controllers.Pid_Speed.Kp = 0.0005f;
	motor->controllers.Pid_Speed.Ki = 0.0f;
	motor->controllers.Pid_Speed.Kd = 0.0001f;
	
	
	//motor->controllers.Pid_Speed.Ki = 0.0001f;
	motor->controllers.Pid_Speed.OutMax_pu = (MOTOR_MAX_CURRENT_IDC_A);
	motor->controllers.Pid_Speed.OutMin_pu = -(MOTOR_MAX_CURRENT_IDC_A);
	
	//// ------------------------------------------------------------------------------
	////  pi position control init
	PI_objectInit(&motor->controllers.Pi_Pos);
	motor->controllers.Pi_Pos.Kp = 40.0f;
	motor->controllers.Pi_Pos.Ki = 0.0f;

	//motor->controllers.Pi_Pos.Ki = 0.00015f;
	//Pi_Pos.Kp = 500000;
	//Pi_Pos.Kp = 0;
	
	motor->controllers.Pi_Pos.OutMax_pu = MOTOR_MAX_SPD_RPM;
	motor->controllers.Pi_Pos.OutMin_pu = -MOTOR_MAX_SPD_RPM;
}

void BldcInitFunctions()
{
	Motor1.ReadHall = readM1Hall;
	Motor2.ReadHall = readM2Hall;
	Motor3.ReadHall = readM3Hall;
	Motor1.DisableMotor = DisableM1GateDrivers;
	Motor2.DisableMotor = DisableM2GateDrivers;
	Motor3.DisableMotor = DisableM3GateDrivers;
	Motor1.SetDutyCycle = SetM1DutyCycle;
	Motor2.SetDutyCycle = SetM2DutyCycle;
	Motor3.SetDutyCycle = SetM3DutyCycle;
}

// ----------------------------------------------------------------------
// Before Power up, Measure and average offset voltage for Current Sensor
// This voltage is subtracted from all reading for Bi-Directional Current
// Measurement
// ----------------------------------------------------------------------
void read_zero_current_offset_value(void)
{
	uint32_t phase_A_zero_current_offset_temp = 0;
	uint32_t phase_B_zero_current_offset_temp = 0;
	uint8_t samples = 16;
	int16_t i;
	
	adc_async_enable_channel(&ADC_0, 0);
	adc_async_enable_channel(&ADC_1, 0);
	
	for (i=0; i<samples; i++)
	{
		volatile uint16_t zero_current_offset_temp[2] = {0,0};
			
		while (ADC0->STATUS.bit.ADCBUSY) {};			/* Wait for bus synchronization. */
		ADC0->SWTRIG.bit.START = true;					/* Start the ADC using a software trigger. */
		while (ADC0->INTFLAG.bit.RESRDY == 0);			/* Wait for the result ready flag to be set. */
		ADC0->INTFLAG.reg = ADC_INTFLAG_RESRDY;			/* Clear the flag. */
		zero_current_offset_temp[0]	= ADC0->RESULT.reg; /* Read the value. */
		
		while (ADC1->STATUS.bit.ADCBUSY) {};			/* Wait for bus synchronization. */
		ADC1->SWTRIG.bit.START = true;					/* Start the ADC using a software trigger. */
		while (ADC1->INTFLAG.bit.RESRDY == 0);			/* Wait for the result ready flag to be set. */
		ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY;			/* Clear the flag. */
		zero_current_offset_temp[1]	= ADC1->RESULT.reg; /* Read the value. */
			
		phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
		phase_B_zero_current_offset_temp += zero_current_offset_temp[1];
		samples--;
	}
	//Motor1.Iphase_pu
	Motor1.Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
	Motor1.Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
	adc_async_disable_channel(&ADC_0, 0);
	adc_async_disable_channel(&ADC_1, 0);
}

//uint8_t get_dir_hall_code(void)
//{
	//return ((Motor1.desiredDirection	 << 3)	|
			//(_gpio_get_level(HALL_A_PIN) << 2)	|
			//(_gpio_get_level(HALL_B_PIN) << 1)	|
			//(_gpio_get_level(HALL_C_PIN) << 0));
//}
//
//uint8_t HALLPatternGet(void)
//{
	//
	//
	//volatile uint8_t motor_read = 0;
	//motor_read = (motor_read & HALL_A_MASK) | (uint8_t)((PORT->Group[HALL_A_GROUP].IN.reg & HALL_A_PORT)>>(HALL_A_LSR)); 
	//motor_read = (motor_read & HALL_B_MASK) | (uint8_t)((PORT->Group[HALL_B_GROUP].IN.reg & HALL_B_PORT)>>(HALL_B_LSR)); 
	//motor_read = (motor_read & HALL_C_MASK) | (uint8_t)((PORT->Group[HALL_C_GROUP].IN.reg & HALL_C_PORT)>>(HALL_C_LSR)); 
	//
	//return motor_read;
	//
	////return ((gpio_get_pin_level(HALL_A_PIN) << 2)|
			////(gpio_get_pin_level(HALL_B_PIN) << 1)|
			////(gpio_get_pin_level(HALL_C_PIN) << 0));
//}


// ----------------------------------------------------------------------
// Hall Reading Functions
// ----------------------------------------------------------------------

uint8_t readM1Hall(void)
{
	volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN);
	volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN);
	volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN);
	
	return ((a << 2)	|
			(b << 1)	|
			(c << 0));
}

uint8_t readM2Hall(void)
{
	volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN);
	volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN);
	volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN);
	
	return ((a << 2)	|
			(b << 1)	|
			(c << 0));
}

uint8_t readM3Hall(void)
{
	volatile uint8_t a = gpio_get_pin_level(HALL_A_PIN);
	volatile uint8_t b = gpio_get_pin_level(HALL_B_PIN);
	volatile uint8_t c = gpio_get_pin_level(HALL_C_PIN);
	
	return ((a << 2)	|
			(b << 1)	|
			(c << 0));
	
}

// ----------------------------------------------------------------------
// Disable Functions
// ----------------------------------------------------------------------

void DisableM1GateDrivers(BLDCMotor_t *motor)
{
	hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]);
}

void DisableM2GateDrivers(BLDCMotor_t *motor)
{
	hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]);
}

void DisableM3GateDrivers(BLDCMotor_t *motor)
{
	
}

void SetM1DutyCycle(uint16_t duty)
{
	TCC1->CCBUF[1].reg = duty;
}

void SetM2DutyCycle(uint16_t duty)
{
	TCC0->CCBUF[0].reg = duty;
	TCC0->CCBUF[1].reg = duty;
	TCC0->CCBUF[2].reg = duty;
	TCC0->CCBUF[3].reg = duty;
}

void SetM3DutyCycle(uint16_t duty)
{
	TCC0->CCBUF[4].reg = duty;
	TCC0->CCBUF[5].reg = duty;
	TCC1->CCBUF[0].reg = duty;
}

/** 
**	____|___TCC0_________ ||____TCC1_________| 
**	W0	|	M2_0	| CC0 ||  __M3_2__ | CC0 |
**	W1	|	M2_1	| CC1 ||	  -	   | CC1 |
**	W2	|	M2_2	| CC2 ||	M1_0   | CC1 |
**	W3	|	M2_3	| CC3 ||	M1_1   | CC1 |
**	W4	| __M3_0__	| CC4 ||	M1_2   | CC1 |
**	W5	| __M3_1__	| CC5 ||	M1_3   | CC1 |
**	W6	|	M2_4	| CC0 ||	M1_4   | CC1 |
**	W7  |	M2_5	| CC1 ||	M1_5   | CC1 |
*/

void exec_commutation(void)
{
	// ----------------------------------------------------------------------
	// Read Motor Hall Sensors
	// ----------------------------------------------------------------------
	Motor1.motor_status.currentHallPattern = Motor1.ReadHall();
	Motor2.motor_status.currentHallPattern = Motor1.ReadHall();
	Motor3.motor_status.currentHallPattern = Motor1.ReadHall();
	
	// ----------------------------------------------------------------------
	// Multi Motor Register Masking
	// ----------------------------------------------------------------------
	volatile uint16_t temp_M1	= COMMUTATION_PATTERN_M1[Motor1.motor_status.currentHallPattern];
	volatile uint16_t temp_M2	= COMMUTATION_PATTERN_M2[Motor2.motor_status.currentHallPattern];
	volatile uint16_t temp_M3_tcc1_des	= COMMUTATION_PATTERN_M1[Motor3.motor_status.currentHallPattern] & m3_TCC1_mask;
	volatile uint16_t temp_M3_tcc0_des	= COMMUTATION_PATTERN_M2[Motor3.motor_status.currentHallPattern] & m3_TCC0_mask;
	/* Zero target bits */	
	temp_M1 &= m3_TCC1_inv_mask;
	temp_M2 &= m3_TCC0_inv_mask;
	/* Set Desired bits */
	temp_M1 |= temp_M3_tcc1_des;
	temp_M2 |= temp_M3_tcc0_des;
	
	// ----------------------------------------------------------------------
	// Set Pattern Buffers
	// ----------------------------------------------------------------------
	TCC0->PATTBUF.reg = (uint16_t)temp_M2;
	TCC1->PATTBUF.reg = (uint16_t)temp_M1;
	
	// ----------------------------------------------------------------------
	// Set Remaining GPIO lines responsible for M3 Commutation 
	// ----------------------------------------------------------------------
	///* GPIO En Pin Setting for M3 */
	switch(Motor3.motor_status.currentHallPattern)                    							 	
	{
		// REG_PORT_OUTSET0 = Port A
		// REG_PORT_OUTSET1 = Port B
		case 1: case 6: case 9: case 14:
			REG_PORT_OUTCLR0 = (1 << GPIO_PIN(M3_3));
			REG_PORT_OUTSET1 = (1 << GPIO_PIN(M3_4))|(1 << GPIO_PIN(M3_5));
			break;
		case 2: case 5: case 10: case 13:
			REG_PORT_OUTSET0 = (1 << GPIO_PIN(M3_3));
			REG_PORT_OUTSET1 = (1 << GPIO_PIN(M3_4));
			REG_PORT_OUTCLR1 = (1 << GPIO_PIN(M3_5));
			break;
		case 3: case 4: case 11: case 12:
			REG_PORT_OUTSET0 = (1 << GPIO_PIN(M3_3));
			REG_PORT_OUTCLR1 = (1 << GPIO_PIN(M3_4));
			REG_PORT_OUTSET1 = (1 << GPIO_PIN(M3_5));
			break;
		default:
			/*hall error - should not get here */
			break;
	}
	
	
	// 	TCC1->PATTBUF.reg = COMMUTATION_PATTERN_M1[(Motor1.motor_status.currentHallPattern + 
	// 												Motor1.motor_setpoints.directionOffset)];
	// ----------------------------------------------------------------------
	// Set Calculated Duty Cycles 
	// ----------------------------------------------------------------------
	Motor1.SetDutyCycle((uint16_t)Motor1.motor_status.duty_cycle);
	Motor2.SetDutyCycle((uint16_t)Motor1.motor_status.duty_cycle+1);
	Motor3.SetDutyCycle((uint16_t)Motor1.motor_status.duty_cycle+2);
												
	//TCC1->CCBUF->reg = (uint16_t)Motor1.motor_status.duty_cycle;
	//hri_tcc_write_PATTBUF_reg(TCC1, (COMMUTATION_PATTERN[(Motor1.currentHallPattern + Motor1.directionOffset)]));
	
	//hri_tcc_write_CCBUF_CCBUF_bf(TCC1, 0,  150);
	//hri_tcc_write_CCBUF_CCBUF_bf(TCC1, 0,  Motor1.duty_cycle);
	
	
	//if ((Motor1.currentHallPattern != INVALID_HALL_0) &&  (Motor1.currentHallPattern != INVALID_HALL_7))
	//{	
	//	hri_tcc_write_PATTBUF_reg(TCC1, COMMUTATION_PATTERN[(Motor1.currentHallPattern + Motor1.directionOffset)]);
	//} else {
		//hall error
	//	return;
	//}
		
	//hri_tcc_write_CCBUF_reg(TCC1, 0, Motor1.duty_cycle);
	
	/* Update the next pattern in the pattern array */
	//Motor1.nextHallPattern = HALL_PATTERN_ARRAY[(Motor1.currentHallPattern+Motor1.directionOffset)];
	//}

	//volatile uint8_t curHallState = hallCode & 0x07;  //ABC format
	//volatile uint8_t curHallState = get_hall_state();  //ABC format
	Motor1.motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[Motor1.motor_status.currentHallPattern];
	volatile int8_t step_change = Motor1.motor_status.cur_comm_step - Motor1.motor_status.prev_comm_step; 
	
	switch(step_change)
	{
		case 1:
		case -5:
			Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps+1;
			Motor1.motor_status.actualDirection = CW;
			//Motor1.directionOffset = DIRECTION_CW_OFFSET;
		break;
		case -1:
		case 5:
			Motor1.motor_status.Num_Steps = Motor1.motor_status.Num_Steps-1;
			Motor1.motor_status.actualDirection = CCW;
			//Motor1.directionOffset = DIRECTION_CCW_OFFSET;
		break;
		default: 
		// do nothing
		break;
	}
	Motor1.motor_status.prev_comm_step = Motor1.motor_status.cur_comm_step;
	//calculate_motor_speed();
	//toc(DEBUG_3);
	//toc(DEBUG_4);
	//CRITICAL_SECTION_LEAVE();
}

void calculate_motor_speed(void)
{	
	volatile uint32_t temp_rpm = 0;
	hri_tccount32_read_CC_reg(TC0, 0); /* Read CC0 but throw away)*/
	volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC0, 1);
	if((period_after_capture > UINT32_MAX)|| (period_after_capture > 10712046)) {
		Motor1.motor_status.calc_rpm = 0;
	} else {
		//uint32_t test = (SPEEDFACTOR, period_after_capture);
		//temp_rpm = SPEEDFACTOR / period_after_capture;
		uint32_t muti = DEVICE_SPEEDTC_FREQUENCY_Hz*60;
		temp_rpm = (muti) / (period_after_capture*MOTOR_COMUTATION_STATES);
	}
	
	#ifdef AVERAGE_SPEED_MEASURE
		// To avoid noise an average is realized on 8 samples
		speed_average += temp_rpm;
		if(count >= n_SAMPLE)
		{
			count = 1;
			Motor1.motor_status.calc_rpm = (speed_average >> 3); // divide by 32
			speed_average = 0;
			//*Spare_byte1 = motorState.actualDirection;
			if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */
			{
				//*motor_speed = -1* Motor1.calc_rpm;
				Motor1.motor_status.calc_rpm = -1* Motor1.motor_status.calc_rpm;
			} else { 
				//*motor_speed = Motor1.calc_rpm;
				Motor1.motor_status.calc_rpm = Motor1.motor_status.calc_rpm;
			}
			return;
		}
		else count++;
	#else
		Motor1.motor_status.calc_rpm = (int16_t)temp_rpm;
	#endif	
}

void DisableMotor(BLDCMotor_t *motor)
{
	hri_tcc_write_PATTBUF_reg(motor->hw, motor->motor_commutation_Pattern[0]);
	
}

//------------------------------------------------------------------------------
// pi current control
// Current Feedback
void BLDC_runCurrentCntl(BLDCMotor_t *motor, volatile float curfbk, volatile float curRef)
{
	
	motor->controllers.Pi_Idc.Fbk_pu = f_clamp(curfbk, -DEVICE_SHUNT_CURRENT_A, DEVICE_SHUNT_CURRENT_A);	// Clamped to max current sensor readingspeedfbk;
	motor->controllers.Pi_Idc.Ref_pu = f_clamp(curRef, -MOTOR_MAX_CURRENT_IDC_A, MOTOR_MAX_CURRENT_IDC_A); // Clamp desired to Motor Max Current i_ref_clamped;
	
	//*Spare_1 = (int16_t)(motor->controllers.Pi_Idc.Ref_pu * 1000);  // Send Req Current to TC

	motor->controllers.Pi_Idc.OutMax_pu = motor->VdcBus_pu;
	motor->controllers.Pi_Idc.OutMin_pu = -motor->VdcBus_pu;
	PI_run_series(&motor->controllers.Pi_Idc);
	
	if(motor->controllers.Pi_Idc.Out_pu > 0) {
		motor->motor_setpoints.desiredDirection = 0; 
		motor->motor_setpoints.directionOffset = 0;
	} else {
		motor->motor_setpoints.desiredDirection = 1;
		motor->motor_setpoints.directionOffset = 8;
	}

	volatile float duty_pu = f_abs((motor->controllers.Pi_Idc.Out_pu * motor->VoneByDcBus_pu));
	volatile uint32_t duty_cycle = f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM);
	motor->motor_status.duty_cycle = (uint16_t)duty_cycle;
	// Remove Low duty cycle values
	//if(duty_cycle < 80.0) motor->duty_cycle = (uint16_t)0;
	//else motor->duty_cycle = (uint16_t)duty_cycle;
	
}

//// ------------------------------------------------------------------------------
// Speed Control: Input in RPM units (int16_t)
void BLDC_runSpeedCntl(BLDCMotor_t *motor, volatile float speedfbk, volatile float speedRef)
{
	
	motor->controllers.Pid_Speed.Fbk_pu = speedfbk; 
	motor->controllers.Pid_Speed.Ref_pu = f_clamp(speedRef, -MAX_VEL, MAX_VEL);	// Convert Speed Ref to Q16 Format
	
	if (applicationStatus.currentstate == MOTOR_V_CTRL_STATE)
	{
		/* Output Pu in Volts (PWM %) */
		motor->controllers.Pid_Speed.OutMax_pu = motor->VdcBus_pu;
		motor->controllers.Pid_Speed.OutMin_pu = -motor->VdcBus_pu;
		
		PID_run_parallel(&motor->controllers.Pid_Speed);
		
		if(motor->controllers.Pid_Speed.Out_pu > 0) {
			motor->motor_setpoints.desiredDirection = 0; 
			motor->motor_setpoints.directionOffset = 0;
		} else {
			motor->motor_setpoints.desiredDirection = 1;
			motor->motor_setpoints.directionOffset = 8;
		}
		/* Process unit is Volts */
		volatile float32_t duty_pu = f_abs((motor->controllers.Pid_Speed.Out_pu * motor->VoneByDcBus_pu));
		volatile duty_cycle = f_clamp(duty_pu *  (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM);
		motor->motor_status.duty_cycle = (uint16_t)duty_cycle;
	} else 	{
		/* Pu in Current (Amps) */
		motor->controllers.Pid_Speed.OutMax_pu = (MOTOR_MAX_CURRENT_IDC_A);
		motor->controllers.Pid_Speed.OutMin_pu = -(MOTOR_MAX_CURRENT_IDC_A);
		PID_run_parallel(&motor->controllers.Pid_Speed);
		motor->controllers.Pi_Idc.Ref_pu = motor->controllers.Pid_Speed.Out_pu;
	}

}

//// ------------------------------------------------------------------------------
// Position Control:Input in Hall step units (int16_t)
void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef)
{
	/* Output Pu in RPM */
	motor->controllers.Pi_Pos.OutMax_pu = MAX_VEL;
	motor->controllers.Pi_Pos.OutMin_pu = -MAX_VEL;
	motor->controllers.Pi_Pos.Fbk_pu = posfbk;	
	motor->controllers.Pi_Pos.Ref_pu = posRef;	
	PI_run_series(&motor->controllers.Pi_Pos);
	motor->controllers.Pid_Speed.Ref_pu = motor->controllers.Pi_Pos.Out_pu;
}
