diff --git a/Src/freertos.c b/Src/freertos.c
index 13ba795..5bd5fc3 100644
--- a/Src/freertos.c
+++ b/Src/freertos.c
@@ -83,10 +83,10 @@ void MX_FREERTOS_Init(void) {
 //  osThreadDef(MMITask, vtMMIStartTask, osPriorityHigh, 0, 128);
 //  MMITaskHandle = osThreadCreate(osThread(MMITask), NULL);
 
-  osThreadDef(DataTransmitTask, vtDataTransmitStartTask, osPriorityHigh, 0, 128);
+  osThreadDef(DataTransmitTask, vtDataTransmitStartTask, osPriorityNormal, 0, 128);
   DataTransmitTaskHandle = osThreadCreate(osThread(DataTransmitTask), NULL);
 
-  osThreadDef(GyroTask, vtGyroStartTask, osPriorityNormal, 0, 128);
+  osThreadDef(GyroTask, vtGyroStartTask, osPriorityHigh, 0, 128);
   GyroTaskHandle = osThreadCreate(osThread(GyroTask), NULL);
 
   osThreadDef(ControlTask, vtControlStartTask, osPriorityNormal, 0, 128);
diff --git a/userApp/tControl.c b/userApp/tControl.c
index 7f561a1..875f387 100644
--- a/userApp/tControl.c
+++ b/userApp/tControl.c
@@ -13,6 +13,7 @@
 #include "tControlP.h"
 #include "tControlPID.h"
 #include "tControlLeadLag.h"
+#include <math.h>
 
 struct signalPackage seeSig;
 char inSignalWrite = 0;
@@ -23,16 +24,21 @@ void vtControlStartTask(void const * argument) {
 	xMotorPWMCommand_t xMotorCommandBuffer; /* Buffer contains motor PWM values, to be sent to motor task*/
 	xIMUDATA_t xIMUDataBuffer; /* Buffer contains IMU read outs, came in from gyro task*/
 	double dAnglePos, dAngleCorrectionConst = 0.4; /* max variation of motor base speed*/
-
+	double dAnglePosPrev = 0.0;
 	/* local constants  ------------------------------------------------- */
+
 //	const double dMotorTrustConsta = 9.1493e-6, dMotorTrustConstb = 6.7674e-4,
 //			dMotorTrustConstc = 5.48456e-4; /* motor constants derived from Mike Hammers paper*/
-	const uint16_t uiMotorBaseSpeed = 10; /* motor base speed, PWM value [40 ... 100];*/
+	const uint16_t uiMotorBaseSpeed = 9; /* motor base speed, PWM value [40 ... 100];*/
 	double tLast = 0.0;
 
+	double r = 0.0;
+	double dN = 0.0; // COUNT FOR REFERENCE SIGNAL
+	double dPeriod = 600; // Period length constant of reference signal
+
 	while (1) {
 		/* Reading IMU data from the Queue*/
-		osEvent evt = osMailGet(GyrodataQueueHandle, 100);
+		osEvent evt = osMailGet(GyrodataQueueHandle, 50);
 		if (evt.status == osEventMail) {
 			xIMUDATA_t *IMUData = (xIMUDATA_t *) evt.value.p;
 			xIMUDataBuffer.AccelX = IMUData->AccelX;
@@ -45,17 +51,26 @@ void vtControlStartTask(void const * argument) {
 		}
 
 		/* Normalize the measured angle to [-1...1]*/
+		r = 0 ; //6000*sin(dN/dPeriod);
+		dN++;
+		if (dN > 2*3.14*dPeriod) dN = 0; // Enforce bounds on dN
+
 		dAnglePos = (double) xIMUDataBuffer.AccelX;
 
+		dAnglePosPrev = dAnglePos;
+
+
+		dAnglePos = dAnglePos - r; // Create difference to reference signal
+
 		/* calculation of the control input */
 		double systemTime = (double) xTaskGetTickCount();
-		double dt = (systemTime - tLast)*0.001;
+		double dt = (systemTime - tLast); //*0.001;
 		tLast = systemTime;
 
 		double e = dLP(dAnglePos, dt);
-		double u = dLeadLag(dAnglePos, dt);
+		// double u = dLeadLag(dAnglePos, dt);
 		//double u = dP(dAnglePos);
-		//double u = dPID(dAnglePos,e,dt);
+		double u = dPID(dAnglePos,e,dt);
 		//double u = dLeadLag(e, dt);
 		//double u = dP(e);
 
@@ -63,8 +78,10 @@ void vtControlStartTask(void const * argument) {
 
 		/* signal transmission */
 		inSignalWrite = 1;
-		seeSig.e = e;
+		seeSig.y = dAnglePos;
 		seeSig.u = u;
+		seeSig.r = r;
+
 		inSignalWrite = 0;
 
 		/* Control starts here */
diff --git a/userApp/tControl.h b/userApp/tControl.h
index 76b5634..1ea5541 100644
--- a/userApp/tControl.h
+++ b/userApp/tControl.h
@@ -24,7 +24,8 @@ extern osMessageQId GyrodataQueueHandle;
 
 #pragma pack(push, 1) // exact fit - no padding
 struct signalPackage{
-	double e;
+	double r;
+	double y;
 	double u;
 };
 #pragma pack(pop) //back to whatever the previous packing mode was
diff --git a/userApp/tControlPID.c b/userApp/tControlPID.c
index 3e4c9a6..2c8af70 100644
--- a/userApp/tControlPID.c
+++ b/userApp/tControlPID.c
@@ -6,9 +6,9 @@
  */
 
 
-double kp = 0.00001;
-double Ti = 1000;
-double Td = 0.0;
+double kp = 0.000005;
+double Ti =  2000;
+double Td = 0.03;
 
 double e_prev = 0.0;
 double ef_prev = 0.0;
@@ -30,7 +30,7 @@ double dPID(double e, double ef, double dt) {
 	delta_uD = kp * Td / dt * (ef - 2 * ef_prev + ef_2prev);
 
 	// calculate PID output value
-	delta_u = delta_uP + delta_uI + delta_uD;
+	delta_u = delta_uP + delta_uD + delta_uI; // + delta_uD;
 	u = u_prev + delta_u;
 
 	// update values
diff --git a/userApp/tDataTransmit.c b/userApp/tDataTransmit.c
index 142d385..6a98b66 100644
--- a/userApp/tDataTransmit.c
+++ b/userApp/tDataTransmit.c
@@ -18,7 +18,6 @@
 struct dataPackage {
 	uint32_t systemTime;
 	struct signalPackage signals;
-	double testDouble;
 };
 #pragma pack(pop) //back to whatever the previous packing mode was
 
@@ -49,7 +48,6 @@ void vtDataTransmitStartTask(void const * argument) {
 		while (inSignalWrite) {
 		}
 		dataP.signals = seeSig;
-		dataP.testDouble = 2.71282;
 		memcpy(&uaBuffer[3], &dataP, sizeof(dataP));
 
 		/* checksum */ //TODO
diff --git a/userApp/tGyro.c b/userApp/tGyro.c
index e54c4fa..131a26d 100644
--- a/userApp/tGyro.c
+++ b/userApp/tGyro.c
@@ -27,41 +27,17 @@ void vtGyroStartTask(void const * argument)
 	  {
 		 /* read data from IMU */
 		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.AccelX, LSM6DS3_OUTX_H_XL, LSM6DS3_OUTX_L_XL);
-		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.AccelY, LSM6DS3_OUTY_H_XL, LSM6DS3_OUTY_L_XL);
-		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.AccelZ, LSM6DS3_OUTZ_H_XL, LSM6DS3_OUTZ_L_XL);
-		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.GyroX, LSM6DS3_OUTX_H_G, LSM6DS3_OUTX_L_G);
+//		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.AccelY, LSM6DS3_OUTY_H_XL, LSM6DS3_OUTY_L_XL);
+//		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.AccelZ, LSM6DS3_OUTZ_H_XL, LSM6DS3_OUTZ_L_XL);
+//		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.GyroX, LSM6DS3_OUTX_H_G, LSM6DS3_OUTX_L_G);
 		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.GyroY, LSM6DS3_OUTY_H_G, LSM6DS3_OUTY_L_G);
-		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.GyroZ, LSM6DS3_OUTZ_H_G, LSM6DS3_OUTZ_L_G);
+//		 vReadIMU(&hi2c1, &xIMUReceiveBuffer.GyroZ, LSM6DS3_OUTZ_H_G, LSM6DS3_OUTZ_L_G);
 
 		 /* send data to Queue */
 	     osMailPut(GyrodataQueueHandle, &xIMUReceiveBuffer);
 
-	     /* send data to the UART */
-	     if(SEND_GYRO_DATA_TO_USART){
-	    	 uint8_t ucUARTSendBuffer[18];
-
-	    	 /* clear send buffer */
-	    	 for(uint8_t i=0; i<14; i++)
-	    	 	 {ucUARTSendBuffer[i]=0; }
-
-	    	 /* setup buffer structure */
-	    	 ucUARTSendBuffer[7] = '\t';
-	    	 ucUARTSendBuffer[8] = '\t';
-	    	 ucUARTSendBuffer[17] = '\n';
-	    	 ucUARTSendBuffer[16] = '\r';
-
-	    	 /* load send Buffer with data*/
-	    	 itoa(xIMUReceiveBuffer.GyroY, ucUARTSendBuffer, 10);
-	    	 itoa(xIMUReceiveBuffer.AccelZ, &ucUARTSendBuffer[9], 10);
-
-	    	 /* send buffer to UART */
-	    	 HAL_UART_Transmit(& huart2, &ucUARTSendBuffer, sizeof(ucUARTSendBuffer), 100);
-	     	 }
-
-
-
 	     /* Delay to slow down this task. FixMe: change IMU readout to be controlled by interrupt */
-	     osDelay(1);
+	     osDelay(5);
 	  }
 }
 
@@ -72,7 +48,7 @@ void vGyroInitLSMD6DS3 (void)
 
 	/* Init gyroscope */
 	/* ODRG = Gyroscope output data rate selection. Default value: ODR_OFF */
-	/* FSG = Gyroscope full-scale selection. Default value: 245°/s */
+	/* FSG = Gyroscope full-scale selection. Default value: 245�/s */
 	uiI2C_Data[0] = LSM6DS3_CTRL2_G;
 	uiI2C_Data[1] = (LSM6D3_ODRG_104HZ | LSM6D3_FSG_245DPS);
 	HAL_I2C_Master_Transmit(&hi2c1, (uint16_t) (LSM6DS2_ADRESS), (uint8_t *) uiI2C_Data, 0x02, 100);
@@ -94,12 +70,19 @@ void vGyroInitLSMD6DS3 (void)
  */
 void vReadIMU(I2C_HandleTypeDef *pxI2CHandler, int16_t *pxDataValue, uint8_t I2C_HByte, uint8_t I2C_LByte)
 {
-	int8_t pcData;
-
-	HAL_I2C_Mem_Read(pxI2CHandler, LSM6DS2_ADRESS, I2C_HByte , 0x01, &pcData, 0x01, 100);
-	*pxDataValue = (int16_t) pcData<<8;
-	HAL_I2C_Mem_Read(pxI2CHandler, LSM6DS2_ADRESS, I2C_LByte , 0x01, &pcData, 0x01, 100);
-	*pxDataValue = *pxDataValue | (int16_t) pcData;
+	uint8_t isOk = 1;
+	HAL_StatusTypeDef status;
+	uint8_t pcData;
+	int16_t data;
+
+	status = HAL_I2C_Mem_Read(pxI2CHandler, LSM6DS2_ADRESS, I2C_HByte , 0x01, &pcData, 0x01, 100);
+	isOk &= status == HAL_OK;
+	data = pcData << 8;
+	status = HAL_I2C_Mem_Read(pxI2CHandler, LSM6DS2_ADRESS, I2C_LByte , 0x01, &pcData, 0x01, 100);
+	isOk &= status == HAL_OK;
+	data |= pcData;
+	if (isOk)
+	  *pxDataValue = data;
 }
 
 
diff --git a/userApp/tLowPass.c b/userApp/tLowPass.c
index c244cc2..893d7c0 100644
--- a/userApp/tLowPass.c
+++ b/userApp/tLowPass.c
@@ -9,7 +9,7 @@
 #include "tLowPass.h"
 
 double y0 = 0.0;
-double k = 0.01;
+double k = 0.4;
 
 
 double dLP(double e, double h){