diff --git a/workspace/TS100/Core/BSP/Miniware/Setup.c b/workspace/TS100/Core/BSP/Miniware/Setup.c
index 9f50feec..fdaeb139 100644
--- a/workspace/TS100/Core/BSP/Miniware/Setup.c
+++ b/workspace/TS100/Core/BSP/Miniware/Setup.c
@@ -30,7 +30,7 @@ static void MX_TIM2_Init(void);
 static void MX_DMA_Init(void);
 static void MX_GPIO_Init(void);
 static void MX_ADC2_Init(void);
-
+#define SWD_ENABLE
 void Setup_HAL() {
 	SystemClock_Config();
 
diff --git a/workspace/TS100/Core/BSP/Miniware/flash.c b/workspace/TS100/Core/BSP/Miniware/flash.c
index 05f2b3d0..7559f23e 100644
--- a/workspace/TS100/Core/BSP/Miniware/flash.c
+++ b/workspace/TS100/Core/BSP/Miniware/flash.c
@@ -23,7 +23,7 @@ uint8_t flash_save_buffer(const uint8_t *buffer, const uint16_t length) {
 	__HAL_FLASH_CLEAR_FLAG(
 			FLASH_FLAG_EOP | FLASH_FLAG_WRPERR | FLASH_FLAG_PGERR | FLASH_FLAG_BSY);
 	HAL_FLASH_Unlock();
-	HAL_Delay(10);
+	HAL_Delay(1);
 	resetWatchdog();
 	HAL_FLASHEx_Erase(&pEraseInit, &failingAddress);
 	//^ Erase the page of flash (1024 bytes on this stm32)
diff --git a/workspace/TS100/Core/BSP/Miniware/fusb302b.cpp b/workspace/TS100/Core/BSP/Miniware/fusb302b.cpp
index 7ccfc644..d7d7416f 100644
--- a/workspace/TS100/Core/BSP/Miniware/fusb302b.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/fusb302b.cpp
@@ -144,12 +144,12 @@ void fusb_send_hardrst() {
 
 void fusb_setup() {
 	GPIO_InitTypeDef GPIO_InitStruct;
-	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
 	GPIO_InitStruct.Pin = GPIO_PIN_9;
 	GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
 	GPIO_InitStruct.Pull = GPIO_PULLUP;
 	HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-	HAL_NVIC_SetPriority(EXTI9_5_IRQn, 12, 0);
+	HAL_NVIC_SetPriority(EXTI9_5_IRQn, 10, 0);
 	HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
 
 	if (!I2CBB::lock2()) {
diff --git a/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp b/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
index c5f16b4c..d0743771 100644
--- a/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
@@ -12,11 +12,10 @@
 
 // Initialisation to be performed with scheduler active
 void postRToSInit() {
-
-	/* Init the IPC objects */
-	FRToSI2C::FRToSInit();
 #ifdef POW_PD
-	//Spawn all of the USB-C processors
-	fusb302_start_processing();
+	if (usb_pd_detect() == true) {
+		//Spawn all of the USB-C processors
+		fusb302_start_processing();
+	}
 #endif
 }
diff --git a/workspace/TS100/Core/BSP/Miniware/preRTOS.cpp b/workspace/TS100/Core/BSP/Miniware/preRTOS.cpp
index 6b0614b9..67b04f49 100644
--- a/workspace/TS100/Core/BSP/Miniware/preRTOS.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/preRTOS.cpp
@@ -17,13 +17,9 @@ void preRToSInit() {
 	 */
 	HAL_Init();
 	Setup_HAL();  // Setup all the HAL objects
-	FRToSI2C::init();
-	HAL_Delay(50);
-	HAL_GPIO_WritePin(OLED_RESET_GPIO_Port, OLED_RESET_Pin, GPIO_PIN_SET);
-	HAL_Delay(50);
 #ifdef I2C_SOFT
 	I2CBB::init();
-
 #endif
-
+	/* Init the IPC objects */
+	FRToSI2C::FRToSInit();
 }
diff --git a/workspace/TS100/Core/Drivers/FUSB302/fusbpd.cpp b/workspace/TS100/Core/Drivers/FUSB302/fusbpd.cpp
index f6bb58e1..b48c42fa 100644
--- a/workspace/TS100/Core/Drivers/FUSB302/fusbpd.cpp
+++ b/workspace/TS100/Core/Drivers/FUSB302/fusbpd.cpp
@@ -17,22 +17,13 @@
 #include "int_n.h"
 #include "hard_reset.h"
 
-
-
 void fusb302_start_processing() {
 	/* Initialize the FUSB302B */
-	resetWatchdog();
 	fusb_setup();
-	resetWatchdog();
-	/* Create the policy engine thread. */
-	PolicyEngine::init();
-
-	/* Create the protocol layer threads. */
-	ProtocolReceive::init();
-	ProtocolTransmit::init();
 	ResetHandler::init();
-	resetWatchdog();
-	/* Create the INT_N thread. */
+	PolicyEngine::init();
+	ProtocolTransmit::init();
+	ProtocolReceive::init();
 	InterruptHandler::init();
 }
 #endif
diff --git a/workspace/TS100/Core/Drivers/FUSB302/int_n.cpp b/workspace/TS100/Core/Drivers/FUSB302/int_n.cpp
index 52d7701b..1206928e 100644
--- a/workspace/TS100/Core/Drivers/FUSB302/int_n.cpp
+++ b/workspace/TS100/Core/Drivers/FUSB302/int_n.cpp
@@ -28,7 +28,7 @@
 #include "task.h"
 #include "BSP.h"
 
-osThreadId InterruptHandler::TaskHandle=NULL;
+osThreadId InterruptHandler::TaskHandle = NULL;
 uint32_t InterruptHandler::TaskBuffer[InterruptHandler::TaskStackSize];
 osStaticThreadDef_t InterruptHandler::TaskControlBlock;
 
@@ -41,7 +41,7 @@ void InterruptHandler::init() {
 void InterruptHandler::Thread(const void *arg) {
 	(void) arg;
 	union fusb_status status;
-	volatile uint32_t events;
+	uint32_t events;
 	bool notifSent = false;
 	while (true) {
 		/* If the INT_N line is low */
diff --git a/workspace/TS100/Core/Src/main.cpp b/workspace/TS100/Core/Src/main.cpp
index 89d2bc45..9c9da2dc 100644
--- a/workspace/TS100/Core/Src/main.cpp
+++ b/workspace/TS100/Core/Src/main.cpp
@@ -12,8 +12,6 @@
 #include "Settings.h"
 #include "cmsis_os.h"
 uint8_t PCBVersion = 0;
-// File local variables
-bool usb_pd_available = false;
 bool settingsWereReset = false;
 // FreeRTOS variables
 
@@ -32,44 +30,17 @@ uint32_t MOVTaskBuffer[MOVTaskStackSize];
 osStaticThreadDef_t MOVTaskControlBlock;
 
 // End FreeRTOS
-
 // Main sets up the hardware then hands over to the FreeRTOS kernel
 int main(void) {
 	preRToSInit();
-
 	setTipX10Watts(0);  // force tip off
 	resetWatchdog();
-	OLED::initialize();  // start up the LCD
 	OLED::setFont(0);    // default to bigger font
 	// Testing for which accelerometer is mounted
-	resetWatchdog();
-	usb_pd_available = usb_pd_detect();
-	resetWatchdog();
 	settingsWereReset = restoreSettings();  // load the settings from flash
-#ifdef ACCEL_MMA
-	if (MMA8652FC::detect()) {
-		PCBVersion = 1;
-		MMA8652FC::initalize();  // this sets up the I2C registers
-	} else
-#endif
-#ifdef ACCEL_LIS
-	if (LIS2DH12::detect()) {
-		PCBVersion = 2;
-		// Setup the ST Accelerometer
-		LIS2DH12::initalize();  // startup the accelerometer
-	} else
-#endif
-	{
-		PCBVersion = 3;
-		systemSettings.SleepTime = 0;
-		systemSettings.ShutdownTime = 0;  // No accel -> disable sleep
-		systemSettings.sensitivity = 0;
-	}
 	resetWatchdog();
-
 	/* Create the thread(s) */
 	/* definition and creation of GUITask */
-
 	osThreadStaticDef(GUITask, startGUITask, osPriorityBelowNormal, 0,
 			GUITaskStackSize, GUITaskBuffer, &GUITaskControlBlock);
 	GUITaskHandle = osThreadCreate(osThread(GUITask), NULL);
diff --git a/workspace/TS100/Core/Threads/GUIThread.cpp b/workspace/TS100/Core/Threads/GUIThread.cpp
index afeceeeb..77a22f9d 100644
--- a/workspace/TS100/Core/Threads/GUIThread.cpp
+++ b/workspace/TS100/Core/Threads/GUIThread.cpp
@@ -632,6 +632,7 @@ void showDebugMenu(void) {
 uint8_t idleScreenBGF[sizeof(idleScreenBG)];
 /* StartGUITask function */
 void startGUITask(void const *argument __unused) {
+	OLED::initialize();  // start up the LCD
 
 	uint8_t tempWarningState = 0;
 	bool buttonLockout = false;
diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp
index 4ca5e54b..4ef12c21 100644
--- a/workspace/TS100/Core/Threads/MOVThread.cpp
+++ b/workspace/TS100/Core/Threads/MOVThread.cpp
@@ -23,8 +23,28 @@
 uint8_t accelInit = 0;
 uint32_t lastMovementTime = 0;
 void startMOVTask(void const *argument __unused) {
-	OLED::setRotation(systemSettings.OrientationMode & 1);
+#ifdef ACCEL_MMA
+	if (MMA8652FC::detect()) {
+		PCBVersion = 1;
+		MMA8652FC::initalize();  // this sets up the I2C registers
+	} else
+#endif
+#ifdef ACCEL_LIS
+	if (LIS2DH12::detect()) {
+		PCBVersion = 2;
+		// Setup the ST Accelerometer
+		LIS2DH12::initalize();  // startup the accelerometer
+	} else
+#endif
+	{
+		PCBVersion = 3;
+		systemSettings.SleepTime = 0;
+		systemSettings.ShutdownTime = 0;  // No accel -> disable sleep
+		systemSettings.sensitivity = 0;
+	}
 	postRToSInit();
+	OLED::setRotation(systemSettings.OrientationMode & 1);
+
 	lastMovementTime = 0;
 	int16_t datax[MOVFilter] = { 0 };
 	int16_t datay[MOVFilter] = { 0 };