/*
 * bldc.c
 *
 * Created: 31/07/2021 14:56:17
 *  Author: Nick-XMG
 */ 

#include "bldc.h"

#include "statemachine.h"
#include "utilities.h"
#include "Ethercat_SlaveDef.h"

void motor_StateMachine(BLDCMotor_t* const motor)
{
	
	//if (motor->motor_state.fault)
	//{
		//motor->motor_state.previousstate = motor->motor_state.currentstate;
		//motor->motor_state.currentstate = MOTOR_FAULT;
	//}
	
	switch (motor->motor_state.currentstate)
	{
		case MOTOR_INIT: ;
			volatile uint8_t currentHall = motor->readHall();
			if((currentHall > 6) || (currentHall < 1) )  {
				motor->motor_state.fault = MOTOR_HALLSENSORINVALID;
				motor->motor_state.currentstate = MOTOR_FAULT;
				break;	
			} 
			motor->motor_state.previousstate = motor->motor_state.currentstate;
			motor->motor_state.currentstate = MOTOR_IDLE;
		break;
		case  MOTOR_IDLE:
			hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw,  DISABLE_PATTERN);
			motor->motor_state.previousstate = motor->motor_state.currentstate;
			motor->motor_state.currentstate = MOTOR_OPEN_LOOP_STATE;
		break;
		case MOTOR_OPEN_LOOP_STATE:
			BLDC_runOpenLoop(motor, *M1_Desired_dc);
			calculate_motor_speed(motor);
			motor->motor_state.previousstate = motor->motor_state.currentstate;
		break;
		case MOTOR_PVI_CTRL_STATE:	
			switch (Motor1.regulation_loop_count) {
				case 0: /* PWM FREQ / 25 - 1kHz */
				Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag
				BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
				BLDC_runPosCntl(&Motor2, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
				
				case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */

				//calculate_motor_speed(motor);
				//calculate_motor_speed();
		
				BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
				BLDC_runSpeedCntl(&Motor2, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
	
				default: /* PWM FREQ - 25kHz */
				select_active_phase(motor);
				//select_active_phase(&Motor2, Motor2.motor_status.currentHallPattern);
		
				BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
				BLDC_runCurrentCntl(&Motor2, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
		
				break;
			} // end switch(regulation_loop_count)
			if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
			else Motor1.regulation_loop_count++;
		break;
	} //end switch (motor->motor_state.currentstate)
	
	// ----------------------------------------------------------------------
	// Run Commutation
	// ----------------------------------------------------------------------
	
}

void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t * const motor_param)
{
	// ----------------------------------------------------------------------
	//  Assign Motor Parameters:
	// ----------------------------------------------------------------------
	motor->motor_param						 = motor_param;
	
	// ----------------------------------------------------------------------
	// Initialize State Machine:
	// ----------------------------------------------------------------------
	motor->motor_state.currentstate			 = 	MOTOR_INIT;
	motor->motor_state.previousstate		 =	MOTOR_INIT;
	motor->motor_state.fault				 =	MOTOR_NOFAULT;			
	// ----------------------------------------------------------------------
	//  Initialize Status Struct:
	// ----------------------------------------------------------------------
	motor->motor_status.actualDirection		 = 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;
	
	// ----------------------------------------------------------------------
	//  Initialize Phase Current Struct:
	// ----------------------------------------------------------------------
	motor->Iphase_pu.A						 = 0;
	motor->Iphase_pu.B						 = 0;
	motor->Iphase_pu.C						 = 0;
	motor->Iphase_pu.Bus					 = 0;
		
	// ----------------------------------------------------------------------
	//  Initialize Timers:
	// ----------------------------------------------------------------------
	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->timerflags.adc_readings_ready_tic = false;
	
	// ----------------------------------------------------------------------
	//  Current Sensors Calibration offsets:
	// ----------------------------------------------------------------------
	motor->Voffset_lsb.A					 = 0;
	motor->Voffset_lsb.B					 = 0;
	
	// ----------------------------------------------------------------------
	//  Regulation Loop Counter
	// ----------------------------------------------------------------------
	motor->regulation_loop_count			 = 0;

	// ----------------------------------------------------------------------
	//  Initialize Bus Voltage:
	// ----------------------------------------------------------------------
	motor->VdcBus_pu						 = DEVICE_DC_VOLTAGE_V;
	motor->VoneByDcBus_pu					 = 1.0f/DEVICE_DC_VOLTAGE_V;
	
	// ----------------------------------------------------------------------
	//  Initialize Setpoints Struct:
	// ----------------------------------------------------------------------
	motor->motor_setpoints.desiredDirection	 = 0;
	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;
		
	//// ------------------------------------------------------------------------------
	//// Initialize PI current control 
	//// ------------------------------------------------------------------------------
	PI_objectInit(&motor->controllers.Pi_Idc);
	float32_t motorLs_H   = motor_param->motor_LQ_H	  * 2.0f;
	float32_t motorRs_OHM = motor_param->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);
	motor->controllers.Pi_Idc.Ki = PI_calcKi(motorRs_OHM, motorLs_H, DEVICE_ISR_PERIOD_Sec);
	
	//// ------------------------------------------------------------------------------
	//// Initialize PD Vel control 
	//// ------------------------------------------------------------------------------
	PID_objectInit(&motor->controllers.Pid_Speed);
		
	/* 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_param->motor_Max_Current_IDC_A);
	motor->controllers.Pid_Speed.OutMin_pu = -(motor_param->motor_Max_Current_IDC_A);
	
	//// ------------------------------------------------------------------------------
	////  Initialize PI Position control
	//// ------------------------------------------------------------------------------
	PI_objectInit(&motor->controllers.Pi_Pos);
	motor->controllers.Pi_Pos.Kp = 40.0f;
	motor->controllers.Pi_Pos.Ki = 0.0f;	
	motor->controllers.Pi_Pos.OutMax_pu =  (motor_param->motor_Max_Spd_RPM);
	motor->controllers.Pi_Pos.OutMin_pu = -(motor_param->motor_Max_Spd_RPM);
}

void exec_commutation(BLDCMotor_t* const motor)
{
	
	// ----------------------------------------------------------------------
	// Read Motor Hall Sensors
	// ----------------------------------------------------------------------
	//tic_port(DEBUG_2_PORT);
	//motor->motor_status.currentHallPattern = readM1Hall();
	volatile uint8_t currentHall = motor->readHall();
	motor->motor_status.currentHallPattern = currentHall;
		
	if ((currentHall == INVALID_HALL_0)||(currentHall == INVALID_HALL_7))
	{
		hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw,  DISABLE_PATTERN);
		motor->motor_state.currentstate == MOTOR_FAULT;
		motor->motor_state.fault == MOTOR_HALLSENSORINVALID;
		return;
	}
																		   
	// ----------------------------------------------------------------------
	// Set Pattern Buffers
	// ----------------------------------------------------------------------
	volatile uint16_t temp_M1	= COMMUTATION_PATTERN[currentHall + 
													  motor->motor_setpoints.directionOffset];

	hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw,  temp_M1);
	
	// ----------------------------------------------------------------------
	// Set Calculated Duty Cycle
	// ----------------------------------------------------------------------
	hri_tcc_write_CCBUF_CCBUF_bf(motor->motor_param->pwm_desc->device.hw, 0, motor->motor_status.duty_cycle);

	volatile uint8_t cur_comm_step = MOTOR_COMMUTATION_STEPS[currentHall];
	motor->motor_status.cur_comm_step = cur_comm_step;
	volatile int8_t step_change = (cur_comm_step - motor->motor_status.prev_comm_step);
	
	switch(step_change)
	{
		case 1: case -5:
			motor->motor_status.Num_Steps += 1;
			motor->motor_status.actualDirection = CW;
		//Motor1.motor_setpoints.directionOffset = DIRECTION_CW_OFFSET;
		break;
		case -1: case 5:
			motor->motor_status.Num_Steps -= 1;
			motor->motor_status.actualDirection = CCW;
		//Motor1.motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET;
		break;
		default:
		// do nothing
		break;
	}
	motor->motor_status.prev_comm_step = cur_comm_step;
	//toc_port(DEBUG_2_PORT);
}

//// ------------------------------------------------------------------------------
// Current Selection Based on Hall State
// Direction":
// CW  -> Always positive current
// CCW -> Always negative current
//// ------------------------------------------------------------------------------
void select_active_phase(BLDCMotor_t* const Motor)
{
	uint8_t hall_state = Motor->motor_status.currentHallPattern;
	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 calculate_motor_speed(BLDCMotor_t* const motor)
{	
	//tic_port(DEBUG_2_PORT);
	volatile uint32_t temp_rpm = 0;
	hri_tccount32_read_CC_reg(motor->motor_param->speedtimer_hw, 0); /* Read CC0 but throw away)*/
	volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(motor->motor_param->speedtimer_hw, 1);
	if((period_after_capture >= UINT32_MAX)||(period_after_capture == 0)||(period_after_capture > 600000))  {
		motor->motor_status.calc_rpm = 0;
	} else {
		////uint32_t test = (SPEEDFACTOR, period_after_capture);
		////temp_rpm = SPEEDFACTOR / period_after_capture;
		////tic_port(DEBUG_3_PORT);
		temp_rpm = HZ_TO_RPM / (period_after_capture * 42);
		//temp_rpm = HZ_TO_RPM * period_after_capture;
		////toc_port(DEBUG_3_PORT);
		
		//uint32_t muti = DEVICE_SPEEDTC_FREQUENCY_Hz*60;
		//temp_rpm = (muti) / (period_after_capture*42);
	}
	
	if(motor->motor_status.actualDirection == CCW) /* Changed from CCW */
	{
		//*motor_speed = -1* Motor1.calc_rpm;
		temp_rpm = -1 * temp_rpm;
	} else { 
		//*motor_speed = Motor1.calc_rpm;
		temp_rpm = temp_rpm;
	}
		
	motor->motor_status.calc_rpm = (int16_t)temp_rpm;
	
	#ifdef AVERAGE_SPEED_MEASURE
		
		//volatile uint32_t speed_average = 0;
		//volatile uint8_t count = 1;
		//// To avoid noise an average is realized on 8 samples
		//speed_average += temp_rpm;
		//if(count >= n_SAMPLE)
		//{
			//count = 1;
			//motor->motor_status.calc_rpm = (speed_average >> 3); // divide by 8
			////Motor1.motor_status.calc_rpm = (speed_average); // divide by 8
			//speed_average = 0;
			////*Spare_byte1 = motorState.actualDirection;
			//if(motor->motor_status.actualDirection == CCW) /* Changed from CCW */
			//{
				////*motor_speed = -1* Motor1.calc_rpm;
				//motor->motor_status.calc_rpm = -1* motor->motor_status.calc_rpm;
			//} else { 
				////*motor_speed = Motor1.calc_rpm;
				//motor->motor_status.calc_rpm = motor->motor_status.calc_rpm;
			//}
			//return;
		//}
		//else count++;
	#endif
	
	motor->motor_status.calc_rpm = (int16_t)temp_rpm;
	//toc_port(DEBUG_2_PORT);	
}


//------------------------------------------------------------------------------
// pi current control
//------------------------------------------------------------------------------
void BLDC_runCurrentCntl(BLDCMotor_t *motor, const float32_t curfbk, const float32_t 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->motor_param->motor_Max_Current_IDC_A, 
														motor->motor_param->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 float32_t duty_pu = f_abs((motor->controllers.Pi_Idc.Out_pu * motor->VoneByDcBus_pu));
	motor->motor_status.duty_cycle = (uint16_t)f_clamp(duty_pu * (float32_t)MAX_PWM, 0.0f, (float32_t)MAX_PWM);
	// 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, const float32_t speedfbk, const float32_t speedRef)
{
	//tic_port(DEBUG_3_PORT);
	motor->controllers.Pid_Speed.Fbk_pu = speedfbk; 
	motor->controllers.Pid_Speed.Ref_pu = f_clamp(speedRef, -motor->motor_param->motor_Max_Spd_RPM, 
															 motor->motor_param->motor_Max_Spd_RPM);	
	
	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 float32_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;
	} else 	{
		/* Pu in Current (Amps) */
		motor->controllers.Pid_Speed.OutMax_pu =  (motor->motor_param->motor_Max_Current_IDC_A);
		motor->controllers.Pid_Speed.OutMin_pu = -(motor->motor_param->motor_Max_Current_IDC_A);
		PID_run_parallel(&motor->controllers.Pid_Speed);
		motor->controllers.Pi_Idc.Ref_pu = motor->controllers.Pid_Speed.Out_pu;
	}
	//toc_port(DEBUG_3_PORT);
}

//// ------------------------------------------------------------------------------
// Position Control:Input in Hall step units (int16_t)
//------------------------------------------------------------------------------
void BLDC_runPosCntl(BLDCMotor_t *motor, const int16_t posfbk, const int16_t posRef)
{
	/* Output Pu in RPM */
	motor->controllers.Pi_Pos.OutMax_pu = motor->motor_param->motor_Max_Spd_RPM;
	motor->controllers.Pi_Pos.OutMin_pu = -motor->motor_param->motor_Max_Spd_RPM;
	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;
}

void BLDC_runOpenLoop(BLDCMotor_t *motor, int16_t duty)
{
	if (duty < 0){
		motor->motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET;
	} else {
		motor->motor_setpoints.directionOffset = DIRECTION_CW_OFFSET;
	}
	motor->motor_status.duty_cycle = u_clamp(abs(duty), 0, MAX_PWM); 
}

volatile uint8_t readHallSensorM1(void)
{
	
	volatile uint8_t motor_read = 0;
	motor_read = (motor_read & M1_HALL_A_MASK) | (uint8_t)((PORT->Group[M1_HALL_A_GROUP].IN.reg & M1_HALL_A_PORT)>>(M1_HALL_A_LSR));
	motor_read = (motor_read & M1_HALL_B_MASK) | (uint8_t)((PORT->Group[M1_HALL_B_GROUP].IN.reg & M1_HALL_B_PORT)>>(M1_HALL_B_LSR));
	motor_read = (motor_read & M1_HALL_C_MASK) | (uint8_t)((PORT->Group[M1_HALL_C_GROUP].IN.reg & M1_HALL_C_PORT)>>(M1_HALL_C_LSR));
	return motor_read;
	
	//volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN);
	//volatile uint8_t b = gpio_get_pin_level(M1_HALL_B_PIN);
	//volatile uint8_t c = gpio_get_pin_level(M1_HALL_C_PIN);
	//return ((a << 2)	|
			//(b << 1)	|
			//(c << 0));
	
}

volatile uint8_t readHallSensorM2(void)
{
	
	volatile uint8_t motor_read = 0;
	motor_read = (motor_read & M2_HALL_A_MASK) | (uint8_t)((PORT->Group[M2_HALL_A_GROUP].IN.reg & M2_HALL_A_PORT)>>(M2_HALL_A_LSR)); 
	motor_read = (motor_read & M2_HALL_B_MASK) | (uint8_t)((PORT->Group[M2_HALL_B_GROUP].IN.reg & M2_HALL_B_PORT)>>(M2_HALL_B_LSR)); 
	motor_read = (motor_read & M2_HALL_C_MASK) | (uint8_t)((PORT->Group[M2_HALL_C_GROUP].IN.reg & M2_HALL_C_PORT)>>(M2_HALL_C_LSR)); 
	
	return motor_read;
	//if(((motor_read == INVALID_HALL_0) || (motor_read == INVALID_HALL_7))) {
		//Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID;
		//Motor2.motor_state.currentstate = MOTOR_FAULT;
		////applicationStatus.currentstate = APP_FAULT;
	//}
		
	//volatile uint8_t a = gpio_get_pin_level(M2_HALL_A_PIN);
	//volatile uint8_t b = gpio_get_pin_level(M2_HALL_B_PIN);
	//volatile uint8_t c = gpio_get_pin_level(M2_HALL_C_PIN);	
	//volatile uint8_t motor_read2 = ((a << 2)|(b << 1)|(c << 0));
	//return ((a << 2)	|
			//(b << 1)	|
			//(c << 0));
	
}

// ----------------------------------------------------------------------
// 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(BLDCMotor_t *motor1, BLDCMotor_t *motor2)
{
	uint32_t phase_A_zero_current_offset_temp = 0;
	uint32_t phase_B_zero_current_offset_temp = 0;
	volatile uint16_t zero_current_offset_temp[2] = {0,0};
	uint8_t samples = 32;
	uint8_t i;
	
	// ------------------------- Motor 1 ---------------------------------
	adc_sync_enable_channel(&ADC_1, 9);
	//adc_sync_enable_channel(&ADC_1, 0);
	
	/* Single ended */
	//ADC1->INPUTCTRL.reg = 0x1809;
	/* Differential */
	ADC1->INPUTCTRL.reg = 0x0089;
	while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
	
	for (i=0; i<samples; i++)
	{
		volatile uint16_t zero_current_offset_temp[2] = {0,0};
		
		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[0]	= ADC1->RESULT.reg; /* Read the value. */
		
		phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
	}
	
	/* Set Motor Variables */
	motor1->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
	
	/* Single ended */
	//ADC1->INPUTCTRL.reg = 0x1808;
	/* Differential */
	ADC1->INPUTCTRL.reg = 0x0088;
	while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
	
	for (i=0; i<samples; i++)
	{
		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_B_zero_current_offset_temp += zero_current_offset_temp[1];
	}
	
	
	/* Set Motor Variables */
	motor1->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
	adc_sync_disable_channel(&ADC_1, 8);
	
	adc_sync_enable_channel(&ADC_1, 7);
	//adc_sync_enable_channel(&ADC_1, 0);
	
	phase_A_zero_current_offset_temp = 0;
	phase_B_zero_current_offset_temp = 0;
	
	
	/* Single ended */
	//ADC1->INPUTCTRL.reg = 0x1807;
	/* Differential */
	ADC1->INPUTCTRL.reg = 0x0087;
	while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
	
	for (i=0; i<samples; i++)
	{
		volatile uint16_t zero_current_offset_temp[2] = {0,0};
		
		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[0]	= ADC1->RESULT.reg; /* Read the value. */
		
		phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
	}
	
	/* Set Motor Variables */
	motor2->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
	
	/* Single ended */
	//ADC1->INPUTCTRL.reg = 0x1806;
	/* Differential */
	ADC1->INPUTCTRL.reg = 0x0086;
	while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
	
	for (i=0; i<samples; i++)
	{
		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_B_zero_current_offset_temp += zero_current_offset_temp[1];
	}
	
	/* Set Motor Variables */
	motor2->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
	adc_sync_disable_channel(&ADC_1, 6);
	//adc_sync_disable_channel(&ADC_1, 0);
}