diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/Drivers_/WheelEncoder.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/Drivers_/WheelEncoder.hpp
index 838d982258771c7f6dca39d6c2630a8ffe119d28..958ae129b61962a61e25d5c4efaa4fcc7b1c1b5b 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/Drivers_/WheelEncoder.hpp
+++ b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/Drivers_/WheelEncoder.hpp
@@ -31,7 +31,7 @@
 
 class WheelEncoder {
 public:
-	WheelEncoder() = delete;
+	WheelEncoder() = default;
 	WheelEncoder(TIM_HandleTypeDef& timer_handle);
 
 	uint32_t GetAbsoluteTicks();
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/SpeedAcquistion.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/SpeedAcquistion.hpp
deleted file mode 100644
index 7676082ef89a4dbbe98f5de0e3f3cfbec23e22b5..0000000000000000000000000000000000000000
--- a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/SpeedAcquistion.hpp
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * SpeedAcquistion.hpp
- *
- *  Created on: Mar 7, 2022
- *      Author: drobotti
- */
-
-#ifndef INC_DROBOTTI_SPEEDACQUISTION_HPP_
-#define INC_DROBOTTI_SPEEDACQUISTION_HPP_
-
-#include "Drivers_/VNH5019.hpp"
-#include "States/SpeedAckStates.hpp"
-#include "usart.h"
-
-struct RearWheelEncoderTicks{
-	RearWheelEncoderTicks() {}
-	RearWheelEncoderTicks(int32_t r, int32_t l) : right(r), left(l) {}
-	int32_t right;		// discrete counts
-	int32_t left;		// discrete counts
-};
-
-enum SpeedAckEvents{
-	evRunTest = 'R',
-	evStopTest = 'S',
-	evStartTest = 'A',
-	evSetTestLength = 'L',
-	evSetTestStart = 'O',
-	evConfigUpdated = 'U',
-	evConfigure = 'C',
-
-	evStep = 's',
-	evTestComplete = 'c'
-};
-
-class SpeedAcquistion{
-public:
-	SpeedAcquistion();
-	//explicit SpeedAcquistion(VNH5019 mot_right_channel, VNH5019 mot_left_channel, UART_HandleTypeDef output);
-	void Init(VNH5019 mot_right_channel, VNH5019 mot_left_channel, UART_HandleTypeDef& output){
-		_right_motor = mot_right_channel;
-		_left_motor = mot_left_channel;
-		_output_sink = output;
-	}
-
-	void HandleEvent(char ev){
-		switch (ev) {
-			case SpeedAckEvents::evRunTest:
-				HAL_UART_Transmit(&_output_sink, (uint8_t*)"\r\nevRunTest...\r\n", 16, 100);
-				_state->evRunTest(this);
-				break;
-			case SpeedAckEvents::evStopTest:
-				HAL_UART_Transmit(&_output_sink, (uint8_t*)"\r\nevStopTest...\r\n", 17, 100);
-				_state->evStopTest(this);
-				break;
-			case SpeedAckEvents::evStep:
-				HAL_UART_Transmit(&_output_sink, (uint8_t*)"\r\nevStep\r\n", 15, 100);
-				_state->evStep(this);
-				break;
-			case SpeedAckEvents::evTestComplete:
-				HAL_UART_Transmit(&_output_sink, (uint8_t*)"\r\nevTestComplete...\r\n", 17, 100);
-				_state->evTestComplete(this);
-			case SpeedAckEvents::evStartTest:
-				HAL_UART_Transmit(&_output_sink, (uint8_t*)"\r\nevStartTest...\r\n", 17, 100);
-				_state->evStartTest(this);
-				break;
-			default:
-				break;
-		}
-	}
-
-private:
-	VNH5019 _right_motor;
-	VNH5019 _left_motor;
-	UART_HandleTypeDef _output_sink; 	/* Communication link to host */
-
-	RearWheelEncoderTicks _total_ticks;
-
-	uint32_t _t_test_length; 	/* Seconds */
-
-	uint32_t _t_step_start; 	/* Seconds */
-	uint8_t _step_size_percent; /* 0 - 100 */
-
-	friend SpeedAckStates;
-	void ChangeState(SpeedAckStates& s){
-		_state = &s;
-	}
-	SpeedAckStates* _state;
-};
-
-
-#endif /* INC_DROBOTTI_SPEEDACQUISTION_HPP_ */
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/ImpulseResponseTest.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/ImpulseResponseTest.hpp
index e94c79506812b4efd13c307ffeb86414331966b1..28a76f102a409c2ddfe5e4720f0aeade31e223ee 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/ImpulseResponseTest.hpp
+++ b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/ImpulseResponseTest.hpp
@@ -9,7 +9,8 @@
 #define INC_DROBOTTI_STATES_IMPULSERESPONSETEST_HPP_
 
 #include "DRobotti/States/TestingStates.hpp"
-#include "usart.h"
+#include "DRobotti/Drivers_/VNH5019.hpp"
+#include "DRobotti/Drivers_/WheelEncoder.hpp"
 
 class ImpulseResponseTest : public TestingStates {
 public:
@@ -17,16 +18,19 @@ public:
 		static ImpulseResponseTest instance; // Static is initialized once
 		return instance;
 	}
+	void Init(VNH5019* m_r, VNH5019* m_l, uint32_t t_start, uint8_t step_size);
 
-	void evStep(Testing* ctx) override {
-		char msg[] = "In State ImpResponse, called evStep()";
-										HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
-	}
-
-
+	void evStep(Testing* ctx) override;
+	void evStopTest(Testing* ctx) override;
+	void evTestComplete(Testing* ctx) override;
 
 private:
 	ImpulseResponseTest(){}
+
+	VNH5019* _right_motor;
+	WheelEncoder* _right_encoder;
+	VNH5019* _left_motor;
+	WheelEncoder* _left_encoder;
 };
 
 
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAckStates.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAckStates.hpp
index 9601b185b6cb896a91ca80351e42da4c4015a4c4..1147d066466064dcb194947b44f690341b8195dd 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAckStates.hpp
+++ b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAckStates.hpp
@@ -38,6 +38,7 @@ public:
 	virtual void evStep(SpeedAcquistion* ctx){}
 	virtual void evTestComplete(SpeedAcquistion* ctx){}
 
+	void Init();
 protected:
 	SpeedAckStates();
 	void ChangeState(SpeedAcquistion* ctx, SpeedAckStates& s);
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAcquistion.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAcquistion.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd0da71ada7df3265eead467b10ee715a13473a6
--- /dev/null
+++ b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/SpeedAcquistion.hpp
@@ -0,0 +1,70 @@
+/*
+ * SpeedAcquistion.hpp
+ *
+ *  Created on: Mar 7, 2022
+ *      Author: drobotti
+ */
+
+#ifndef INC_DROBOTTI_STATES_SPEEDACQUISTION_HPP_
+#define INC_DROBOTTI_STATES_SPEEDACQUISTION_HPP_
+
+#include "DRobotti/States/SpeedAckStates.hpp"
+#include "usart.h"
+
+enum SpeedAckEvents{
+	evRunTest = 'R',
+	evStopTest = 'S',
+	evStartTest = 'A',
+	evSetTestLength = 'L',
+	evSetTestStart = 'O',
+	evConfigUpdated = 'U',
+	evConfigure = 'C',
+
+	evStep = 's',
+	evTestComplete = 'c'
+};
+
+class SpeedAcquistion{
+public:
+	SpeedAcquistion();
+	//explicit SpeedAcquistion(VNH5019 mot_right_channel, VNH5019 mot_left_channel, UART_HandleTypeDef output);
+	void Init();
+
+	void HandleEvent(char ev){
+		switch (ev) {
+			case SpeedAckEvents::evRunTest:
+				HAL_UART_Transmit(&huart2, (uint8_t*)"\r\nevRunTest...\r\n", 16, 100);
+				_state->evRunTest(this);
+				break;
+			case SpeedAckEvents::evStopTest:
+				HAL_UART_Transmit(&huart2, (uint8_t*)"\r\nevStopTest...\r\n", 17, 100);
+				_state->evStopTest(this);
+				break;
+			case SpeedAckEvents::evStep:
+				HAL_UART_Transmit(&huart2, (uint8_t*)"\r\nevStep\r\n", 15, 100);
+				_state->evStep(this);
+				break;
+			case SpeedAckEvents::evTestComplete:
+				HAL_UART_Transmit(&huart2, (uint8_t*)"\r\nevTestComplete...\r\n", 17, 100);
+				_state->evTestComplete(this);
+			case SpeedAckEvents::evStartTest:
+				HAL_UART_Transmit(&huart2, (uint8_t*)"\r\nevStartTest...\r\n", 17, 100);
+				_state->evStartTest(this);
+				break;
+			default:
+				break;
+		}
+	}
+
+private:
+	friend SpeedAckStates;
+	void ChangeState(SpeedAckStates& s){
+		_state = &s;
+	}
+	SpeedAckStates* _state;
+
+	//UART_HandleTypeDef _output_sink; 	/* Communication link to host */ // Does not make sense, either make a global endpoint (wrapping ST's interface) or use ST's interface directly
+};
+
+
+#endif /* INC_DROBOTTI_STATES_SPEEDACQUISTION_HPP_ */
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Stopped.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Stopped.hpp
index 04585a6c541466c0dc7a9c3b684c8e3999039cce..6f96e91caa5b50f1cabb8ab7369b0f9b276e2876 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Stopped.hpp
+++ b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Stopped.hpp
@@ -9,8 +9,7 @@
 #define INC_DROBOTTI_STATES_STOPPED_HPP_
 
 #include "DRobotti/States/TestingStates.hpp"
-#include "DRobotti/States/ImpulseResponseTest.hpp"
-#include "usart.h"
+
 
 class Stopped : public TestingStates {
 public:
@@ -19,18 +18,8 @@ public:
 		return instance;
 	}
 
-	void evStep(Testing* ctx) override {
-		char msg[] = "In State Stopped, called evStep()";
-										HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
-	}
-
-	void evStartTest(Testing* ctx) override{
-		char msg[] = "In State Stopped, called evStartTest()";
-										HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
-		ChangeState(ctx, ImpulseResponseTest::Instance());
-	}
-
-
+	void evStep(Testing* ctx) override;
+	void evStartTest(Testing* ctx) override;
 
 private:
 	Stopped(){}
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Testing.hpp b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Testing.hpp
index 136e0d3ef764524ccb57fee286447c6c4060e2f6..dda34e1eb51f137cbeba82527582c9cf27279270 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Testing.hpp
+++ b/SpeedControl_Data_Acquisition/Core/Inc/DRobotti/States/Testing.hpp
@@ -10,6 +10,28 @@
 
 #include "DRobotti/States/SpeedAckStates.hpp"
 #include "DRobotti/States/TestingStates.hpp"
+#include "DRobotti/Drivers_/VNH5019.hpp"
+#include "DRobotti/Drivers_/WheelEncoder.hpp"
+
+struct RearWheelEncoderTicks{
+	RearWheelEncoderTicks() {}
+	RearWheelEncoderTicks(int32_t r, int32_t l) : right(r), left(l) {}
+	int32_t right;		// discrete counts
+	int32_t left;		// discrete counts
+};
+
+struct testCfg{
+	testCfg(uint32_t t_cs, uint32_t t_tl, uint32_t t_ss, uint8_t ssp) : _t_current_step(t_cs),
+																		_t_test_length(t_tl),
+																		_t_step_start(t_ss),
+																		_step_size_percent(ssp)
+	{}
+
+	uint32_t _t_current_step;	/* Current step in the test run */
+	uint32_t _t_test_length; 	/* Total steps before test complete */
+	uint32_t _t_step_start; 	/* Step at which the control signal is started */
+	uint8_t _step_size_percent; /* 0 - 100 */
+};
 
 class Testing : public SpeedAckStates {
 public:
@@ -20,12 +42,11 @@ public:
 	void Init();
 	virtual ~Testing();
 
-	void evStep(SpeedAcquistion* ctx) override {_state->evStep(this);};
-	void evRunTest(SpeedAcquistion* ctx) override {};
-	void evStopTest(SpeedAcquistion* ctx) override {_state->evStopTest(this);};
-	void evTestComplete(SpeedAcquistion* ctx) override {_state->evTestComplete(this);};
-	void evStartTest(SpeedAcquistion* ctx) override {_state->evStartTest(this);};
-	void evConfigure(SpeedAcquistion* ctx) override {};
+	void evStep(SpeedAcquistion* ctx) override;
+	void evStopTest(SpeedAcquistion* ctx) override;
+	void evTestComplete(SpeedAcquistion* ctx) override;
+	void evStartTest(SpeedAcquistion* ctx) override;
+	void evConfigure(SpeedAcquistion* ctx) override;
 
 private:
 	Testing();
@@ -35,6 +56,15 @@ private:
 		_state = &s;
 	}
 	TestingStates* _state;
+
+	VNH5019 _right_motor;
+	WheelEncoder _right_encoder;
+	VNH5019 _left_motor;
+	WheelEncoder _left_encoder;
+
+	RearWheelEncoderTicks _total_ticks;
+
+	testCfg _testing_cfg;
 };
 
 
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/stm32f4xx_it.h b/SpeedControl_Data_Acquisition/Core/Inc/stm32f4xx_it.h
index 1805d69adefbd95fe45ced624e9a5b0681025f6b..dbaf99c2bf6c7f0cdcc70726cbbc0ee5d41412a3 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/stm32f4xx_it.h
+++ b/SpeedControl_Data_Acquisition/Core/Inc/stm32f4xx_it.h
@@ -58,6 +58,7 @@ void PendSV_Handler(void);
 void SysTick_Handler(void);
 void DMA1_Stream5_IRQHandler(void);
 void DMA1_Stream6_IRQHandler(void);
+void TIM1_UP_TIM10_IRQHandler(void);
 void TIM3_IRQHandler(void);
 void TIM4_IRQHandler(void);
 void USART2_IRQHandler(void);
diff --git a/SpeedControl_Data_Acquisition/Core/Inc/tim.h b/SpeedControl_Data_Acquisition/Core/Inc/tim.h
index ea4b378a4702310ec39af7775f65a8d8e654e3f9..9f9eb0b69e8d90f2d085aa8ab6336017ac04d0ce 100644
--- a/SpeedControl_Data_Acquisition/Core/Inc/tim.h
+++ b/SpeedControl_Data_Acquisition/Core/Inc/tim.h
@@ -36,6 +36,7 @@ extern TIM_HandleTypeDef htim3;
 extern TIM_HandleTypeDef htim4;
 extern TIM_HandleTypeDef htim5;
 extern TIM_HandleTypeDef htim7;
+extern TIM_HandleTypeDef htim10;
 
 /* USER CODE BEGIN Private defines */
 
@@ -46,6 +47,7 @@ void MX_TIM3_Init(void);
 void MX_TIM4_Init(void);
 void MX_TIM5_Init(void);
 void MX_TIM7_Init(void);
+void MX_TIM10_Init(void);
 
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
 
diff --git a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/SpeedAcquisition.cpp b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/SpeedAcquisition.cpp
deleted file mode 100644
index bb298c96d964dd4b72c35dc683d37433f02f8a6c..0000000000000000000000000000000000000000
--- a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/SpeedAcquisition.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * SpeedAcquisition.cpp
- *
- *  Created on: Mar 8, 2022
- *      Author: drobotti
- */
-
-#include "DRobotti/SpeedAcquistion.hpp"
-#include "DRobotti/States/Testing.hpp"
-
-SpeedAcquistion::SpeedAcquistion() : _left_motor(),
-									_right_motor(),
-									_output_sink(),
-									_step_size_percent(0),
-									_t_step_start(0),
-									_t_test_length(0),
-									_total_ticks(0,0),
-									_state(&Testing::Instance())
-{
-}
-
-
diff --git a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/ImpulseResponseTest.cpp b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/ImpulseResponseTest.cpp
index b353dd06814d383d4481d83e68d4ee01b9b8f289..ad7ebff3a78208cb71b9d7ff8f9c3a6d907e7dee 100644
--- a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/ImpulseResponseTest.cpp
+++ b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/ImpulseResponseTest.cpp
@@ -5,6 +5,28 @@
  *      Author: drobotti
  */
 
+#include "DRobotti/States/ImpulseResponseTest.hpp"
+#include "DRobotti/States/Stopped.hpp"
+#include "usart.h"
+#include "tim.h"
+#include "string.h"
 
+void ImpulseResponseTest::evStep(Testing* ctx){
+	char msg[] = "In State ImpResponse, called evStep()";
+									HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
+}
 
+void ImpulseResponseTest::evStopTest(Testing* ctx){
+	char msg[] = "In State ImpResponse, called evStopTest()";
+											HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
+	ChangeState(ctx, Stopped::Instance());
+	HAL_TIM_Base_Stop_IT(&htim10);
+}
+
+void ImpulseResponseTest::evTestComplete(Testing* ctx){
+	char msg[] = "In State ImpResponse, called evTestComplete()";
+											HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
+	ChangeState(ctx, Stopped::Instance());
+	HAL_TIM_Base_Stop_IT(&htim10);
+}
 
diff --git a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAckStates.cpp b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAckStates.cpp
index 3a97c84ed7e73dd641fac9599e983c8afdca36bb..1a3246bd3768bdcd79c598b4edac5eed91bd7467 100644
--- a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAckStates.cpp
+++ b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAckStates.cpp
@@ -5,8 +5,9 @@
  *      Author: drobotti
  */
 
+#include <DRobotti/States/SpeedAcquistion.hpp>
 #include "DRobotti/States/SpeedAckStates.hpp"
-#include "DRobotti/SpeedAcquistion.hpp"
+#include "DRobotti/States/Testing.hpp"
 
 SpeedAckStates::SpeedAckStates(){
 
@@ -16,6 +17,10 @@ SpeedAckStates::~SpeedAckStates(){
 
 }
 
+void Init(){
+	Testing::Instance().Init();
+}
+
 void SpeedAckStates::ChangeState(SpeedAcquistion* ctx, SpeedAckStates& s){
 	ctx->ChangeState(s);
 }
diff --git a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAcquisition.cpp b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAcquisition.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..00a2d4dfc3ca270c263ee2c3f31ae9dff3454afb
--- /dev/null
+++ b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/SpeedAcquisition.cpp
@@ -0,0 +1,17 @@
+/*
+ * SpeedAcquisition.cpp
+ *
+ *  Created on: Mar 8, 2022
+ *      Author: drobotti
+ */
+
+#include <DRobotti/States/SpeedAcquistion.hpp>
+#include "DRobotti/States/Testing.hpp"
+
+SpeedAcquistion::SpeedAcquistion() :_state(&Testing::Instance()) // Initial state
+{
+}
+
+void SpeedAcquistion::Init(){
+
+}
diff --git a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Stopped.cpp b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Stopped.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ac975986d35093ae45110845f01a348db25db5a
--- /dev/null
+++ b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Stopped.cpp
@@ -0,0 +1,25 @@
+/*
+ * Stopped.cpp
+ *
+ *  Created on: Mar 10, 2022
+ *      Author: drobotti
+ */
+
+#include "DRobotti/States/Stopped.hpp"
+#include "DRobotti/States/ImpulseResponseTest.hpp"
+#include "usart.h"
+#include "tim.h"
+#include "string.h"
+
+
+void Stopped::evStep(Testing* ctx){
+	char msg[] = "In State Stopped, called evStep()";
+									HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
+}
+
+void Stopped::evStartTest(Testing* ctx){
+	char msg[] = "In State Stopped, called evStartTest()";
+									HAL_UART_Transmit(&huart2, (uint8_t *) msg, strlen(msg), 100);
+	ChangeState(ctx, ImpulseResponseTest::Instance());
+	HAL_TIM_Base_Start_IT(&htim10);
+}
diff --git a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Testing.cpp b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Testing.cpp
index 45cc1282a6c7215ffa2f3e023a4296547c401a7c..a2edc4ba185b777df5b2715c62c07cb40b658ede 100644
--- a/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Testing.cpp
+++ b/SpeedControl_Data_Acquisition/Core/Src/DRobotti/States/Testing.cpp
@@ -7,13 +7,87 @@
 
 #include "DRobotti/States/Testing.hpp"
 #include "DRobotti/States/Stopped.hpp"
+#include "DRobotti/States/ImpulseResponseTest.hpp"
+#include "tim.h"
 
-Testing::Testing() : _state(&Stopped::Instance())
+Testing::Testing() : _state(&Stopped::Instance()),
+					 _testing_cfg(0, 5000, 2500, 50)
+//					 _t_current_step(0),
+//					 _t_test_length(5000),
+//					 _t_step_start(2500),
+//					 _step_size_percent(50)
 {
 
 
 }
 
+void Testing::Init(){
+	// Initialize HW peripherals used for dual motor driver board VNH5019 - STM32 HAL initialises its periphs
+	// runtime, therefore we do the same.
+	// Motor left:
+	PWMChannel pwm_gen_motor_left(htim4,
+								  TIM_CHANNEL_1); 	// Initialize the PWM channel of timer 4
+	GPIOPin in_a_1(GPIOA, GPIO_PIN_10);				// Direction pin
+	GPIOPin in_b_1(GPIOB, GPIO_PIN_5);				// Direction pin
+	GPIOPin enable_pin_1(GPIOB, GPIO_PIN_10);		// Enable pin
+
+	// Motor Right
+	PWMChannel pwm_gen_motor_right(htim3,
+								  TIM_CHANNEL_2);	// Initialize the PWM channel of timer 3
+	GPIOPin in_a_2(GPIOA, GPIO_PIN_8);				// Direction pin
+	GPIOPin in_b_2(GPIOA, GPIO_PIN_9);				// Direction pin
+	GPIOPin enable_pin_2(GPIOA, GPIO_PIN_6);		// Enable pin
+
+
+
+	// Create mdriver objects
+	_left_motor = VNH5019(VNH5019::MOTOR_FORWARD_ORIENTATION::COUNTERCLOCKWISE,
+									  pwm_gen_motor_left,
+									  in_a_1,
+									  in_b_1,
+									  enable_pin_1);
+	_left_encoder = WheelEncoder(htim2);
+
+	_right_motor = VNH5019(VNH5019::MOTOR_FORWARD_ORIENTATION::CLOCKWISE,
+									  pwm_gen_motor_right,
+									  in_a_2,
+									  in_b_2,
+									  enable_pin_2);
+	_right_encoder = WheelEncoder(htim5);
+
+	ImpulseResponseTest::Instance().Init(&_right_motor, &_left_motor, _testing_cfg._t_step_start, _testing_cfg._step_size_percent);
+}
+
+void Testing::evStep(SpeedAcquistion* ctx){
+	_state->evStep(this);
+
+	//	if(_t_current_step < _t_test_length){
+	//		_state->evStep(this);
+	//		_t_current_step++;
+	//	}
+	//	else{
+	//		_t_current_step = 0;
+	//		_state->evTestComplete(this);
+	//	}
+};
+
+
+void Testing::evStopTest(SpeedAcquistion* ctx){
+	_state->evStopTest(this);
+};
+
+void Testing::evTestComplete(SpeedAcquistion* ctx){
+	_state->evTestComplete(this);
+};
+
+void Testing::evStartTest(SpeedAcquistion* ctx){
+	_state->evStartTest(this);
+};
+
+void Testing::evConfigure(SpeedAcquistion* ctx){
+
+};
+
 
 
 Testing::~Testing(){}
diff --git a/SpeedControl_Data_Acquisition/Core/Src/main.cpp b/SpeedControl_Data_Acquisition/Core/Src/main.cpp
index 53ec1712b7daf7741c111020d2cbc0bdc7c73d3a..8c3bc59be5295093352634768584c89468ac471c 100644
--- a/SpeedControl_Data_Acquisition/Core/Src/main.cpp
+++ b/SpeedControl_Data_Acquisition/Core/Src/main.cpp
@@ -18,6 +18,7 @@
   */
 /* USER CODE END Header */
 /* Includes ------------------------------------------------------------------*/
+#include <DRobotti/States/SpeedAcquistion.hpp>
 #include "main.h"
 #include "dma.h"
 #include "tim.h"
@@ -31,7 +32,6 @@
 #include "DRobotti/RobotParameters.hpp"
 #include "DRobotti/Types.hpp"
 #include "DRobotti/Utils/PeripheralTypes.hpp"
-#include "DRobotti/SpeedAcquistion.hpp"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -101,46 +101,12 @@ int main(void)
   MX_TIM2_Init();
   MX_TIM5_Init();
   MX_TIM7_Init();
+  MX_TIM10_Init();
   /* USER CODE BEGIN 2 */
-
-  	// Initialize HW peripherals used for dual motor driver board VNH5019 - STM32 HAL initialises its periphs
-	// runtime, therefore we do the same.
-	// Motor left:
-	PWMChannel pwm_gen_motor_left(htim4,
-								  TIM_CHANNEL_1); 	// Initialize the PWM channel of timer 4
-	GPIOPin in_a_1(GPIOA, GPIO_PIN_10);				// Direction pin
-	GPIOPin in_b_1(GPIOB, GPIO_PIN_5);				// Direction pin
-	GPIOPin enable_pin_1(GPIOB, GPIO_PIN_10);		// Enable pin
-
-	// Motor Right
-	PWMChannel pwm_gen_motor_right(htim3,
-								  TIM_CHANNEL_2);	// Initialize the PWM channel of timer 3
-	GPIOPin in_a_2(GPIOA, GPIO_PIN_8);				// Direction pin
-	GPIOPin in_b_2(GPIOA, GPIO_PIN_9);				// Direction pin
-	GPIOPin enable_pin_2(GPIOA, GPIO_PIN_6);		// Enable pin
-
-
-
-	// Create mdriver objects
-	VNH5019 motor_left(VNH5019::MOTOR_FORWARD_ORIENTATION::COUNTERCLOCKWISE,
-									  pwm_gen_motor_left,
-									  in_a_1,
-									  in_b_1,
-									  enable_pin_1);
-	WheelEncoder encoder_left(htim2);
-
-	VNH5019 motor_right(VNH5019::MOTOR_FORWARD_ORIENTATION::CLOCKWISE,
-									  pwm_gen_motor_right,
-									  in_a_2,
-									  in_b_2,
-									  enable_pin_2);
-	WheelEncoder encoder_right(htim5);
-
-	SpeedAckProg.Init(motor_right, motor_left, huart2);
+	SpeedAckProg.Init();
 
 	HAL_UART_Transmit(&huart2, (uint8_t*)"\r\nStarted...\r\n", 15, 100);
-	HAL_UART_Receive_IT(&huart2, &byte, 1);
-
+	HAL_UART_Receive_IT(&huart2, &byte, 1); /* Start listening for a single byte ... */
   /* USER CODE END 2 */
 
   /* Infinite loop */
@@ -148,7 +114,7 @@ int main(void)
   while (1)
   {
     /* USER CODE END WHILE */
-	  /* Application should not reach this point */
+
     /* USER CODE BEGIN 3 */
   }
   /* USER CODE END 3 */
@@ -242,7 +208,9 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
     HAL_IncTick();
   }
   /* USER CODE BEGIN Callback 1 */
-
+  if(htim->Instance == TIM10){
+	  SpeedAckProg.HandleEvent('s'); // evStep
+  }
   /* USER CODE END Callback 1 */
 }
 
diff --git a/SpeedControl_Data_Acquisition/Core/Src/stm32f4xx_it.c b/SpeedControl_Data_Acquisition/Core/Src/stm32f4xx_it.c
index 04d3ac39bb4735904204337a3e48c66cf7771fa3..1b7a8c90251049140e99e83a048c0c6370566194 100644
--- a/SpeedControl_Data_Acquisition/Core/Src/stm32f4xx_it.c
+++ b/SpeedControl_Data_Acquisition/Core/Src/stm32f4xx_it.c
@@ -59,6 +59,7 @@
 extern TIM_HandleTypeDef htim3;
 extern TIM_HandleTypeDef htim4;
 extern TIM_HandleTypeDef htim7;
+extern TIM_HandleTypeDef htim10;
 extern DMA_HandleTypeDef hdma_usart2_rx;
 extern DMA_HandleTypeDef hdma_usart2_tx;
 extern UART_HandleTypeDef huart2;
@@ -234,6 +235,20 @@ void DMA1_Stream6_IRQHandler(void)
   /* USER CODE END DMA1_Stream6_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM1 update interrupt and TIM10 global interrupt.
+  */
+void TIM1_UP_TIM10_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 0 */
+
+  /* USER CODE END TIM1_UP_TIM10_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim10);
+  /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 1 */
+
+  /* USER CODE END TIM1_UP_TIM10_IRQn 1 */
+}
+
 /**
   * @brief This function handles TIM3 global interrupt.
   */
diff --git a/SpeedControl_Data_Acquisition/Core/Src/tim.c b/SpeedControl_Data_Acquisition/Core/Src/tim.c
index dff52ba2a3d4feb6ccfa8d9ebff885cafc352539..593be4240d27cb4451c647c39fba50c58dfc1344 100644
--- a/SpeedControl_Data_Acquisition/Core/Src/tim.c
+++ b/SpeedControl_Data_Acquisition/Core/Src/tim.c
@@ -29,6 +29,7 @@ TIM_HandleTypeDef htim3;
 TIM_HandleTypeDef htim4;
 TIM_HandleTypeDef htim5;
 TIM_HandleTypeDef htim7;
+TIM_HandleTypeDef htim10;
 
 /* TIM2 init function */
 void MX_TIM2_Init(void)
@@ -238,6 +239,32 @@ void MX_TIM7_Init(void)
 
   /* USER CODE END TIM7_Init 2 */
 
+}
+/* TIM10 init function */
+void MX_TIM10_Init(void)
+{
+
+  /* USER CODE BEGIN TIM10_Init 0 */
+
+  /* USER CODE END TIM10_Init 0 */
+
+  /* USER CODE BEGIN TIM10_Init 1 */
+
+  /* USER CODE END TIM10_Init 1 */
+  htim10.Instance = TIM10;
+  htim10.Init.Prescaler = 840 - 1;
+  htim10.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim10.Init.Period = 100 - 1;
+  htim10.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim10.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim10) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM10_Init 2 */
+
+  /* USER CODE END TIM10_Init 2 */
+
 }
 
 void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
@@ -355,6 +382,21 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM7_MspInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM10)
+  {
+  /* USER CODE BEGIN TIM10_MspInit 0 */
+
+  /* USER CODE END TIM10_MspInit 0 */
+    /* TIM10 clock enable */
+    __HAL_RCC_TIM10_CLK_ENABLE();
+
+    /* TIM10 interrupt Init */
+    HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+  /* USER CODE BEGIN TIM10_MspInit 1 */
+
+  /* USER CODE END TIM10_MspInit 1 */
+  }
 }
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
 {
@@ -497,6 +539,20 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM7_MspDeInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM10)
+  {
+  /* USER CODE BEGIN TIM10_MspDeInit 0 */
+
+  /* USER CODE END TIM10_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM10_CLK_DISABLE();
+
+    /* TIM10 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn);
+  /* USER CODE BEGIN TIM10_MspDeInit 1 */
+
+  /* USER CODE END TIM10_MspDeInit 1 */
+  }
 }
 
 /* USER CODE BEGIN 1 */
diff --git a/SpeedControl_Data_Acquisition/SpeedControl_Data_Acquisition.ioc b/SpeedControl_Data_Acquisition/SpeedControl_Data_Acquisition.ioc
index 28a70da818d146c5ca7cca2463bc99e5bcb95b7e..577935b039df785d8cdf2e4af83bfaa68832b301 100644
--- a/SpeedControl_Data_Acquisition/SpeedControl_Data_Acquisition.ioc
+++ b/SpeedControl_Data_Acquisition/SpeedControl_Data_Acquisition.ioc
@@ -2,6 +2,7 @@
 Mcu.Family=STM32F4
 ProjectManager.MainLocation=Core/Src
 PH0-OSC_IN.Locked=true
+VP_TIM10_VS_ClockSourceINT.Signal=TIM10_VS_ClockSourceINT
 PH0-OSC_IN.Signal=RCC_OSC_IN
 VP_SYS_VS_tim6.Mode=TIM6
 USART2.IPParameters=VirtualMode,BaudRate
@@ -46,13 +47,14 @@ Mcu.ThirdPartyNb=0
 RCC.SDIOFreq_Value=168000000
 Dma.USART2_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 RCC.HCLKFreq_Value=84000000
-Mcu.IPNb=10
+Mcu.IPNb=11
 RCC.I2SClocksFreq_Value=96000000
 TIM2.IPParameters=EncoderMode
 ProjectManager.PreviousToolchain=STM32CubeIDE
 RCC.APB2TimFreq_Value=84000000
 TIM3.Period=0x0FFF
 PB6.Signal=S_TIM4_CH1
+VP_TIM10_VS_ClockSourceINT.Mode=Enable_Timer
 RCC.SPDIFRXFreq_Value=168000000
 RCC.VcooutputI2S=96000000
 PC7.Signal=S_TIM3_CH2
@@ -77,6 +79,7 @@ NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:true\:false
 PB2.Signal=GPIO_Output
 PA8.Locked=true
+Mcu.IP10=USART2
 NVIC.SysTick_IRQn=true\:0\:0\:true\:false\:true\:true\:true
 ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.26.1
 MxDb.Version=DB.6.0.21
@@ -133,6 +136,7 @@ TIM3.Pulse-PWM\ Generation2\ CH2=0x0
 RCC.SAIBFreq_Value=96000000
 NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true
 Dma.Request0=USART2_RX
+TIM10.IPParameters=Period,Prescaler
 NVIC.TIM7_IRQn=true\:0\:0\:false\:false\:true\:true\:true
 ProjectManager.CustomerFirmwarePackage=
 PC14-OSC32_IN.Locked=true
@@ -142,12 +146,13 @@ PA3.Signal=USART2_RX
 PA2.Locked=true
 PB3.GPIO_Label=SWO
 TIM4.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
+TIM10.Prescaler=840 - 1
 PC15-OSC32_OUT.Mode=LSE-External-Oscillator
 SH.S_TIM4_CH1.ConfNb=1
 Dma.USART2_TX.1.Priority=DMA_PRIORITY_VERY_HIGH
 ProjectManager.ProjectFileName=SpeedControl_Data_Acquisition.ioc
 TIM5.IPParameters=EncoderMode
-Mcu.PinsNb=27
+Mcu.PinsNb=28
 Dma.USART2_TX.1.FIFOMode=DMA_FIFOMODE_DISABLE
 ProjectManager.NoMain=false
 PC13.Locked=true
@@ -196,6 +201,7 @@ board=NUCLEO-F446RE
 RCC.VCOOutputFreq_Value=336000000
 ProjectManager.LastFirmware=false
 NVIC.TIM3_IRQn=true\:0\:0\:false\:false\:true\:true\:true
+TIM10.Period=100 - 1
 Dma.USART2_TX.1.Direction=DMA_MEMORY_TO_PERIPH
 VP_SYS_VS_tim6.Signal=SYS_VS_tim6
 RCC.APB2Freq_Value=84000000
@@ -216,6 +222,7 @@ Mcu.Name=STM32F446R(C-E)Tx
 Dma.USART2_RX.0.MemInc=DMA_MINC_ENABLE
 RCC.PLLI2SQCLKFreq_Value=96000000
 Mcu.Pin26=VP_TIM7_VS_ClockSourceINT
+Mcu.Pin27=VP_TIM10_VS_ClockSourceINT
 RCC.RTCHSEDivFreq_Value=4000000
 PA2.Signal=USART2_TX
 PA13.GPIO_Label=TMS
@@ -224,7 +231,7 @@ ProjectManager.UnderRoot=true
 Mcu.Pin25=VP_SYS_VS_tim6
 PB2.Locked=true
 Mcu.IP8=TIM7
-Mcu.IP9=USART2
+Mcu.IP9=TIM10
 Mcu.IP6=TIM4
 RCC.VCOSAIInputFreq_Value=1000000
 Mcu.IP7=TIM5
@@ -256,6 +263,7 @@ Mcu.Pin16=PA9
 Mcu.Pin13=PB10
 Mcu.Pin14=PC7
 Mcu.Pin19=PA14
+NVIC.TIM1_UP_TIM10_IRQn=true\:0\:0\:false\:false\:true\:true\:true
 ProjectManager.ComputerToolchain=false
 Mcu.Pin17=PA10
 RCC.HSI_VALUE=16000000