diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c
index 662768c..e097c42 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c
+++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c
@@ -14,11 +14,11 @@
 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;
-	}
+	//if (motor->motor_state.fault)
+	//{
+		//motor->motor_state.previousstate = motor->motor_state.currentstate;
+		//motor->motor_state.currentstate = MOTOR_FAULT;
+	//}
 	
 	switch (motor->motor_state.currentstate)
 	{
@@ -189,9 +189,16 @@ void exec_commutation(BLDCMotor_t* const motor)
 	// ----------------------------------------------------------------------
 	//tic_port(DEBUG_2_PORT);
 	//motor->motor_status.currentHallPattern = readM1Hall();
-	volatile uint8_t currentHall = 1;
-	currentHall = motor->readHall();
+	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
@@ -265,9 +272,9 @@ void calculate_motor_speed(BLDCMotor_t* const motor)
 {	
 	//tic_port(DEBUG_2_PORT);
 	volatile uint32_t temp_rpm = 0;
-	hri_tccount32_read_CC_reg(TC2, 0); /* Read CC0 but throw away)*/
-	volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC2, 1);
-	if((period_after_capture > UINT32_MAX)||(period_after_capture == 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);
@@ -290,14 +297,12 @@ void calculate_motor_speed(BLDCMotor_t* const motor)
 		temp_rpm = temp_rpm;
 	}
 		
-	//motor->motor_status.calc_rpm = (int16_t)temp_rpm;
-	//toc_port(DEBUG_2_PORT);
+	motor->motor_status.calc_rpm = (int16_t)temp_rpm;
 	
-	//#ifdef AVERAGE_SPEED_MEASURE
-		//if(temp_rpm==0)
-		//#define n_SAMPLE 8
-		//static uint32_t speed_average = 0;
-		//static uint8_t count = 1;
+	#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)
@@ -318,7 +323,7 @@ void calculate_motor_speed(BLDCMotor_t* const motor)
 			//return;
 		//}
 		//else count++;
-	//#endif
+	#endif
 	
 	motor->motor_status.calc_rpm = (int16_t)temp_rpm;
 	//toc_port(DEBUG_2_PORT);	
@@ -445,6 +450,7 @@ volatile uint8_t readHallSensorM2(void)
 	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;
@@ -458,7 +464,7 @@ volatile uint8_t readHallSensorM2(void)
 	//return ((a << 2)	|
 			//(b << 1)	|
 			//(c << 0));
-	return motor_read;
+	
 }
 
 // ----------------------------------------------------------------------
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h
index 2254838..c917e17 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h
@@ -29,7 +29,8 @@
 #define INVALID_HALL_0              (0)
 #define INVALID_HALL_7              (7)
 
-#define AVERAGE_SPEED_MEASURE	1
+//#define AVERAGE_SPEED_MEASURE	
+#define n_SAMPLE 8
 
 // ----------------------------------------------------------------------
 // 	ADC Parameters
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h
index c650168..229a9c2 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h
@@ -24,6 +24,29 @@ void adc_sram_dma_callback(struct _dma_resource *adc_dma_res)
 	Motor2.timerflags.adc_readings_ready_tic = true;
 }
 
+// ----------------------------------------------------------------------
+// 	EtherCAT Cycle Timer - 1kHz
+// ----------------------------------------------------------------------
+void TC2_Handler(void)
+{
+	if (TC2->COUNT32.INTFLAG.bit.OVF == 0x01) {
+		TC2->COUNT32.INTFLAG.bit.OVF = 0x01;
+		Motor1.motor_status.calc_rpm = 0;
+	}
+}
+
+// ----------------------------------------------------------------------
+//
+// ----------------------------------------------------------------------
+void TC4_Handler(void)
+{
+	if (TC4->COUNT32.INTFLAG.bit.OVF == 0x01) {
+		TC4->COUNT32.INTFLAG.bit.OVF = 0x01;
+		Motor2.motor_status.calc_rpm = 0;	
+	}
+}
+
+
 
 
 #endif /* INTERRUPTS_H_ */
\ No newline at end of file
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c
index 3bf90e9..a91aa00 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/main.c
+++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c
@@ -79,8 +79,10 @@ void One_ms_timer_init(void)
 
 void enable_NVIC_IRQ(void)
 {
-	//NVIC_EnableIRQ(TC7_IRQn);	 // TC7: TC_ECAT
-	//NVIC_EnableIRQ(TC0_IRQn);	 // TC0: TC_SPEED
+	//hri_tc_clear_INTEN_OVF_bit(TC2);
+	//hri_tc_clear_INTEN_OVF_bit(TC4);
+	NVIC_EnableIRQ(TC2_IRQn);	 // TC2: M1_Speed_Timer
+	NVIC_EnableIRQ(TC4_IRQn);	 // TC4: M2_Speed_Timer
 	NVIC_EnableIRQ(DMAC_0_IRQn);
 	NVIC_EnableIRQ(TCC1_0_IRQn);
 	NVIC_EnableIRQ(TCC1_0_IRQn);
@@ -175,9 +177,6 @@ int main(void)
 	//speed_timer_init();
 	enable_NVIC_IRQ();
 	
-
-
-	
 	/* Replace with your application code */
 	while (1) {
 		//delay_ms(100);
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
index a688f0d..668eaa3 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h
@@ -71,109 +71,48 @@
 **	W7  |	M1_0	| CC0 ||	M2_0   | CC0 |
 */
 
-/* CCW from Back - works best in forward diraction*/
-//static const uint16_t COMMUTATION_PATTERN[] = {
-	//0x003F,		// (0) invalid state
-	//0x053D,		// (1)
-	//0x1437,		// (2)
-	//0x113D,		// (3)
-	//0x111F,		// (4)
-	//0x141F,		// (5)
-	//0x0537,		// (6)
-	//0x003F,		// (7) invalid state
-	//0x003F,		// (8) invalid state
-	//0x0537,		// (9)
-	//0x141F,		// (10)
-	//0x111F,		// (11)
-	//0x113D,		// (12)
-	//0x1437,		// (13)
-	//0x053D,		// (14)
-	//0x003F		// (15) invalid state
-//};
-
-////* CW From Back */
-//static const uint16_t COMMUTATION_PATTERN[16] = {
-	//0x003F,		// (0) invalid state
-	//0x0537,		// (1)
-	//0x141F,		// (2)
-	//0x111F,		// (3)
-	//0x113D,		// (4)
-	//0x1437,		// (5)
-	//0x053D,		// (6)
-	//0x003F,		// (7) invalid state
-	//0x003F,		// (8) invalid state
-	//0x053D,		// (9)
-	//0x1437,		// (10)
-	//0x113D,		// (11)
-	//0x111F,		// (12)
-	//0x141F,		// (13)
-	//0x0537,		// (14)
-	//0x003F		// (15) invalid state
-//};
-
-////* CW From Back */
-//static const uint16_t COMMUTATION_PATTERN[16] = {
-	//0x003F,		// (0) invalid state
-	//0x0537,		// (1)
-	//0x141F,		// (2)
-	//0x111F,		// (3)
-	//0x113D,		// (4)
-	//0x1437,		// (5)
-	//0x053D,		// (6)
-	//0x003F,		// (7) invalid state
-	//0x003F,		// (8) invalid state
-	//0x0537,		// (9)
-	//0x053D,		// (10)
-	//0x1437,		// (11)
-	//0x113D,		// (12)
-	//0x111F,		// (13)
-	//0x141F,		// (14)
-	//0x003F		// (15) invalid state
-//};
-//
-
 #define DISABLE_PATTERN		0x003F
 
-////* A&C phase swapped */
-static const uint16_t COMMUTATION_PATTERN[16] = {
-	0x003F,		// (0) invalid state
-	0x1437,					// (1)
-	0x053D,					// (2)
-	0x113D,					// (3)
-	0x111F,					// (4)
-	0x0537,					// (5)
-	0x141F,					// (6)
-	0x003F,		// (7) invalid state
-	0x003F,		// (8) invalid state
-	0x141F,					// (9)
-	0x0537,					// (10)
-	0x111F,					// (11)
-	0x113D,					// (12)
-	0x053D,					// (13)
-	0x1437,					// (14)
-	0x003F,			// (15) invalid state
-};
-
-//////* A&C phase swapped and modified to match currents */
+//////* A&C phase swapped */
 //static const uint16_t COMMUTATION_PATTERN[16] = {
 	//0x003F,		// (0) invalid state
-	//0x141F,		// (1)
-	//0x0537,		// (2)
-	//0x111F,		// (3)
-	//0x113D,		// (4)
-	//0x053D,		// (5)
-	//0x1437,		// (6)
+	//0x1437,					// (1)
+	//0x053D,					// (2)
+	//0x113D,					// (3)
+	//0x111F,					// (4)
+	//0x0537,					// (5)
+	//0x141F,					// (6)
 	//0x003F,		// (7) invalid state
 	//0x003F,		// (8) invalid state
-	//0x1437,		// (9)
-	//0x053D,		// (10)
-	//0x113D,		// (11)
-	//0x111F,		// (12)
-	//0x0537,		// (13)
-	//0x141F,		// (14)
-	//0x003F		// (15) invalid state
+	//0x141F,					// (9)
+	//0x0537,					// (10)
+	//0x111F,					// (11)
+	//0x113D,					// (12)
+	//0x053D,					// (13)
+	//0x1437,					// (14)
+	//0x003F,			// (15) invalid state
 //};
 
+////* A&C phase swapped and modified to match currents */
+static const uint16_t COMMUTATION_PATTERN[16] = {
+	0x003F,		// (0) invalid state
+	0x141F,		// (1)
+	0x0537,		// (2)
+	0x111F,		// (3)
+	0x113D,		// (4)
+	0x053D,		// (5)
+	0x1437,		// (6)
+	0x003F,		// (7) invalid state
+	0x003F,		// (8) invalid state
+	0x1437,		// (9)
+	0x053D,		// (10)
+	0x113D,		// (11)
+	0x111F,		// (12)
+	0x0537,		// (13)
+	0x141F,		// (14)
+	0x003F		// (15) invalid state
+};
+
 // ----------------------------------------------------------------------
 // 	Motor Physical Parameters
 // ----------------------------------------------------------------------
diff --git a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
index 7c24574..3bbb186 100644
--- a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
+++ b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h
@@ -59,9 +59,9 @@ typedef enum
 
 typedef enum
 {
-	MOTOR_NOFAULT			  = 0,
-	MOTOR_HALLSENSORINVALID	  = 1,
-	MOTOR_DRIVER_OVER_CURRENT = 2,
+	MOTOR_NOFAULT			  = 0xE1,
+	MOTOR_HALLSENSORINVALID	  = 0xE2,
+	MOTOR_DRIVER_OVER_CURRENT = 0xE3,
 } MOTOR_FAULTS_t;
 
 typedef struct MOTOR_STATE
diff --git a/Twincat/MotorData/.vs/MotorData/v15/.suo b/Twincat/MotorData/.vs/MotorData/v15/.suo
index c74cc05..02fe545 100644
Binary files a/Twincat/MotorData/.vs/MotorData/v15/.suo and b/Twincat/MotorData/.vs/MotorData/v15/.suo differ