diff --git a/Makefile b/Makefile
index 65251acf..ec813218 100644
--- a/Makefile
+++ b/Makefile
@@ -13,7 +13,7 @@ TARGET = ODriveFirmware
 # debug build?
 DEBUG = 1
 # optimization
-OPT = -Og -ffast-math
+OPT = -O0 -ffast-math -u _printf_float -u _scanf_float
 
 #######################################
 # pathes
@@ -120,9 +120,9 @@ C_INCLUDES += -IDrivers/CMSIS/Include
 C_INCLUDES += -IInc
 C_INCLUDES += -IMotorControl
 # compile gcc flags
-ASFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -u _printf_float -u _scanf_float
-CFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -u _printf_float -u _scanf_float
-CXXFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -u _printf_float -u _scanf_float
+ASFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
+CFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
+CXXFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
 ifeq ($(DEBUG), 1)
 CFLAGS += -g -gdwarf-2
 CXXFLAGS += -g -gdwarf-2
@@ -157,10 +157,10 @@ vpath %.cpp $(sort $(dir $(CPP_SOURCES)))
 OBJECTS += $(addprefix $(BUILD_DIR)/,$(notdir $(ASM_SOURCES:.s=.o)))
 vpath %.s $(sort $(dir $(ASM_SOURCES)))
 
-$(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR) 
+$(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR)
 	@$(CC) -c $(CFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.c=.lst)) $< -o $@
 
-$(BUILD_DIR)/%.o: %.cpp Makefile | $(BUILD_DIR) 
+$(BUILD_DIR)/%.o: %.cpp Makefile | $(BUILD_DIR)
 	@$(CXX) -c $(CXXFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.cpp=.lst)) $< -o $@
 
 $(BUILD_DIR)/%.o: %.s Makefile | $(BUILD_DIR)
diff --git a/MotorControl/axis.cpp b/MotorControl/axis.cpp
index 7e524cda..4044fa9a 100644
--- a/MotorControl/axis.cpp
+++ b/MotorControl/axis.cpp
@@ -1,5 +1,6 @@
 
-#include "axis.hpp"
+#include "axis.h"
+#include "commands.h"
 
 //TODO: goal of refactor is to kick this out completely
 extern "C" {
diff --git a/MotorControl/axis.hpp b/MotorControl/axis.h
similarity index 100%
rename from MotorControl/axis.hpp
rename to MotorControl/axis.h
diff --git a/MotorControl/commands.c b/MotorControl/commands.c
index 76de2d98..36854447 100644
--- a/MotorControl/commands.c
+++ b/MotorControl/commands.c
@@ -1,9 +1,9 @@
 /* Includes ------------------------------------------------------------------*/
 #include <cmsis_os.h>
-#include <commands.h>
-#include <usart.h>
-#include <gpio.h>
-#include <freertos_vars.h>
+#include "commands.h"
+#include "usart.h"
+#include "gpio.h"
+#include "freertos_vars.h"
 
 extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
 
@@ -23,7 +23,7 @@ static const GpioMode_t gpio_mode = GPIO_MODE_UART;     //GPIO 1,2 is UART Tx,Rx
 
 // variables exposed to usb/serial interface via set/get/monitor
 // Note: this will be depricated soon
-float* const exposed_floats[] = {
+float* exposed_floats[] = {
     &vbus_voltage, // ro
     NULL, //&elec_rad_per_enc, // ro
     &motors[0].pos_setpoint, // rw
@@ -84,7 +84,7 @@ float* const exposed_floats[] = {
     &motors[1].encoder.pll_ki, // rw
 };
 
-int* const exposed_ints[] = {
+int* exposed_ints[] = {
     (int*)&motors[0].control_mode, // rw
     &motors[0].encoder.encoder_offset, // rw
     &motors[0].encoder.encoder_state, // ro
@@ -95,7 +95,7 @@ int* const exposed_ints[] = {
     &motors[1].error, // rw
 };
 
-bool* const exposed_bools[] = {
+bool* exposed_bools[] = {
     &motors[0].thread_ready, // ro
     //For now these are written by Axis::SetupLegacyMappings
     NULL, // &motors[0].enable_control, // rw
@@ -107,7 +107,7 @@ bool* const exposed_bools[] = {
     NULL, // &motors[1].calibration_ok, // ro
 };
 
-uint16_t* const exposed_uint16[] = {
+uint16_t* exposed_uint16[] = {
     &motors[0].control_deadline, // rw
     &motors[0].last_cpu_time, // ro
     &motors[1].control_deadline, // rw
diff --git a/MotorControl/commands.h b/MotorControl/commands.h
index 26cdd9e0..7ef5a1a8 100644
--- a/MotorControl/commands.h
+++ b/MotorControl/commands.h
@@ -2,7 +2,7 @@
 #define COMMANDS_H
 
 /* Includes ------------------------------------------------------------------*/
-#include <low_level.h>
+#include "low_level.h"
 /* Exported types ------------------------------------------------------------*/
 
 typedef enum {
diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h
index e5190f5d..fb20c24b 100644
--- a/MotorControl/low_level.h
+++ b/MotorControl/low_level.h
@@ -169,9 +169,6 @@ void init_motor_control();
 
 bool motor_calibration(Motor_t* motor);
 
-// void motor_thread(void const * argument);
-void motor_parse_cmd(uint8_t* buffer, int len);
-
 
 //// Old private:
 // Utility
@@ -194,10 +191,10 @@ void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude);
 void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q);
 // Main motor control
 void update_rotor(Motor_t* motor);
-bool using_encoder(Motor_t* motor)
-bool using_sensorless(Motor_t* motor)
-float get_rotor_phase(Motor_t* motor)
-float get_pll_vel(Motor_t* motor)
+bool using_encoder(Motor_t* motor);
+bool using_sensorless(Motor_t* motor);
+float get_rotor_phase(Motor_t* motor);
+float get_pll_vel(Motor_t* motor);
 bool spin_up_sensorless(Motor_t* motor);
 void update_brake_current(float brake_current);
 void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta);