# HG changeset patch
# User Ethan Zonca <ez@ethanzonca.com>
# Date 2017-07-24 19:31:42
# Node ID e81c09ec6423cfd8dd4bc568952af5db23c48f5d
# Parent  39ea1ee3d5af63c880582159ebd17dbb7ada9ccb
# Parent  953c718ee4cff581992e643116860ab2014bd425

Merge

diff --git a/.cproject b/.cproject
--- a/.cproject
+++ b/.cproject
@@ -17,33 +17,33 @@
 				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="${cross_rm} -rf" description="" id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1538442532" name="Debug" parent="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug">
 					<folderInfo id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1538442532." name="/" resourcePath="">
 						<toolChain id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.elf.debug.1849706153" name="Cross ARM GCC" superClass="ilg.gnuarmeclipse.managedbuild.cross.toolchain.elf.debug">
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.1898847267" name="Optimization Level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level" value="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.debug" valueType="enumerated"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength.1803427167" name="Message length (-fmessage-length=0)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar.1920457501" name="'char' is signed (-fsigned-char)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections.321897226" name="Function sections (-ffunction-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.datasections.1782401255" name="Data sections (-fdata-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.datasections" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.407850642" name="Debug level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level" value="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.max" valueType="enumerated"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format.485776751" name="Debug format" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family.1154107227" name="ARM family" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.mcpu.cortex-m4" valueType="enumerated"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn.1932872609" name="Enable all common warnings (-Wall)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.extrawarn.747789546" name="Enable extra warnings (-Wextra)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.extrawarn" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.freestanding.1360639253" name="Assume freestanding environment (-ffreestanding)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.freestanding" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.nomoveloopinvariants.1265121449" name="Disable loop invariant move (-fno-move-loop-invariants)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.nomoveloopinvariants" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto.2018930176" name="Link-time optimizer (-flto)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name.149408563" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name" value="GNU Tools for ARM Embedded Processors" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.929516986" name="Architecture" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.architecture" value="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.arm" valueType="enumerated"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset.1562917164" name="Instruction set" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset.thumb" valueType="enumerated"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.prefix.392227972" name="Prefix" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.prefix" value="arm-none-eabi-" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.c.2121007191" name="C compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.c" value="gcc" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.cpp.1652498155" name="C++ compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.cpp" value="g++" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.ar.544972784" name="Archiver" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.ar" value="ar" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.objcopy.1005994348" name="Hex/Bin converter" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.objcopy" value="objcopy" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.objdump.1204092853" name="Listing generator" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.objdump" value="objdump" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.size.894265291" name="Size command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.size" value="size" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.make.886060440" name="Build command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.make" value="make" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.rm.153446908" name="Remove command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.rm" value="rm" valueType="string"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash.2133363735" name="Create flash image" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash" value="true" valueType="boolean"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize.737083294" name="Print size" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.1898847267" name="Optimization Level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level" useByScannerDiscovery="true" value="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.size" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength.1803427167" name="Message length (-fmessage-length=0)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar.1920457501" name="'char' is signed (-fsigned-char)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.signedchar" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections.321897226" name="Function sections (-ffunction-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.functionsections" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.datasections.1782401255" name="Data sections (-fdata-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.datasections" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.407850642" name="Debug level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level" useByScannerDiscovery="true" value="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.level.max" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format.485776751" name="Debug format" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format" useByScannerDiscovery="true"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family.1154107227" name="ARM family" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.mcpu.cortex-m4" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn.1932872609" name="Enable all common warnings (-Wall)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.allwarn" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.extrawarn.747789546" name="Enable extra warnings (-Wextra)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.extrawarn" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.freestanding.1360639253" name="Assume freestanding environment (-ffreestanding)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.freestanding" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.nomoveloopinvariants.1265121449" name="Disable loop invariant move (-fno-move-loop-invariants)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.nomoveloopinvariants" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto.2018930176" name="Link-time optimizer (-flto)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.lto" useByScannerDiscovery="true" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name.149408563" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name" useByScannerDiscovery="false" value="GNU Tools for ARM Embedded Processors" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.929516986" name="Architecture" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.architecture" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.arm" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset.1562917164" name="Instruction set" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset.thumb" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.prefix.392227972" name="Prefix" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.prefix" useByScannerDiscovery="false" value="arm-none-eabi-" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.c.2121007191" name="C compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.c" useByScannerDiscovery="false" value="gcc" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.cpp.1652498155" name="C++ compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.cpp" useByScannerDiscovery="false" value="g++" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.ar.544972784" name="Archiver" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.ar" useByScannerDiscovery="false" value="ar" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.objcopy.1005994348" name="Hex/Bin converter" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.objcopy" useByScannerDiscovery="false" value="objcopy" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.objdump.1204092853" name="Listing generator" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.objdump" useByScannerDiscovery="false" value="objdump" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.size.894265291" name="Size command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.size" useByScannerDiscovery="false" value="size" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.make.886060440" name="Build command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.make" useByScannerDiscovery="false" value="make" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.rm.153446908" name="Remove command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.rm" useByScannerDiscovery="false" value="rm" valueType="string"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash.2133363735" name="Create flash image" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize.737083294" name="Print size" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.1641379360" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
 							<builder buildPath="${workspace_loc:/S3467-MiniBuoy-BT-Sensor-Software}/Debug" id="ilg.gnuarmeclipse.managedbuild.cross.builder.1145700756" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder">
 								<outputEntries>
@@ -52,16 +52,18 @@
 								</outputEntries>
 							</builder>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.1885536253" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.1335843010" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" value="true" valueType="boolean"/>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths.119815874" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths" valueType="includePath">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.1335843010" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths.119815874" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/CMSIS/Include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/lib&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/CMSIS/Device/ST/STM32F3xx/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/inc/system&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/ldscripts/STM32F302K8_FLASH.ld}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/STM32F3xx_HAL_Driver/Inc}&quot;"/>
 								</option>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs.2115290648" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs" valueType="definedSymbols">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs.2115290648" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="STM32F302x8"/>
 								</option>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.1515695785" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
@@ -70,7 +72,9 @@
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths.1718280814" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/CMSIS/Include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/lib&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/CMSIS/Device/ST/STM32F3xx/Include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/inc/system&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/ldscripts/STM32F302K8_FLASH.ld}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/STM32F3xx_HAL_Driver/Inc}&quot;"/>
@@ -84,6 +88,8 @@
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths.1649547949" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/CMSIS/Include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/lib&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/inc/system&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/drivers/CMSIS/Device/ST/STM32F3xx/Include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/ldscripts/STM32F302K8_FLASH.ld}&quot;"/>
@@ -116,14 +122,14 @@
 								</inputType>
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.linker.226210550" name="Cross ARM C++ Linker" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.linker">
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.gcsections.1592260261" name="Remove unused sections (-Xlinker --gc-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.gcsections" value="true" valueType="boolean"/>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.paths.1684527273" name="Library search path (-L)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.paths"/>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile.2056115086" name="Script files (-T)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile" valueType="stringList">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.gcsections.1592260261" name="Remove unused sections (-Xlinker --gc-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.gcsections" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.paths.1684527273" name="Library search path (-L)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.paths" useByScannerDiscovery="false"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile.2056115086" name="Script files (-T)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile" useByScannerDiscovery="false" valueType="stringList">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/ldscripts/STM32F302K8_FLASH.ld}&quot;"/>
 								</option>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.nostart.337377349" name="Do not use standard start files (-nostartfiles)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.nostart" value="false" valueType="boolean"/>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.usenewlibnano.444480985" name="Use newlib-nano (--specs=nano.specs)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.usenewlibnano" value="true" valueType="boolean"/>
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.useprintffloat.472439722" name="Use float with nano printf (-u _printf_float)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.useprintffloat" value="true" valueType="boolean"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.nostart.337377349" name="Do not use standard start files (-nostartfiles)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.nostart" useByScannerDiscovery="false" value="false" valueType="boolean"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.usenewlibnano.444480985" name="Use newlib-nano (--specs=nano.specs)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.usenewlibnano" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.useprintffloat.472439722" name="Use float with nano printf (-u _printf_float)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.useprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.linker.input.490763083" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -139,7 +145,7 @@
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.createlisting.wide.1198581919" name="Wide lines (--wide|-w)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.createlisting.wide" value="true" valueType="boolean"/>
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.printsize.343129499" name="Cross ARM GNU Print Size" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.printsize">
-								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.printsize.format.1881178891" name="Size format" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.printsize.format"/>
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.printsize.format.1881178891" name="Size format" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.printsize.format" useByScannerDiscovery="false"/>
 							</tool>
 						</toolChain>
 					</folderInfo>
@@ -221,6 +227,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.rm.153446908.2047149393" name="Remove command" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.rm.153446908"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash.2133363735.798650567" name="Create flash image" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash.2133363735"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize.737083294.931072004" name="Print size" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.printsize.737083294"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.896192688" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.1885536253">
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.1392942558" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
 							</tool>
diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1039910129683149700" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-796171634606229038" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1105339482864355034" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-786907217906447192" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/inc/buttons.h b/inc/buttons.h
deleted file mode 100644
--- a/inc/buttons.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * buttons.h
- *
- *  Created on: Oct 22, 2016
- *      Author: Nicholas Orlando
- */
-
-#ifndef BUTTONS_H_
-#define BUTTONS_H_
-
-enum _btn_state_id
-{
-	NOT_PRESSED = 0,
-	PRESSED = 1,
-	HELD_SHORT = 2,
-	HELD_LONG = 3,
-};
-
-void freaking_debounce(void);
-
-#endif /* BUTTONS_H_ */
diff --git a/inc/config.h b/inc/config.h
--- a/inc/config.h
+++ b/inc/config.h
@@ -2,6 +2,52 @@
 #define CONFIG_H
 
 
+//////////////////////////////////////////////////////
+// Sensor Type
+//////////////////////////////////////////////////////
+//#define MAX31855_TC_SENSOR
+//#define MAX31865_RTD_SENSOR
+#define MAX_WHATEVERTHENEWONEIS
+
+
+//////////////////////////////////////////////////////
+// USB
+//////////////////////////////////////////////////////
+
+// Virtual serial port transmit rate
+#define VCP_TX_FREQ 1000
+
+// Solid-state relay maximum on-time
+#define SSR_PERIOD 200
+
+// Interval of PID calculations
+#define PID_PERIOD 120
+
+
+//////////////////////////////////////////////////////
+// Other settings
+//////////////////////////////////////////////////////
+
+// Add bootloader option to top of idle screen menu
+#define BOOTLOADER_SHORTCUT
+
+
+//////////////////////////////////////////////////////
+// Default Settings
+//////////////////////////////////////////////////////
+
+#define DEFAULT_BOOT_TO_BREW 0
+#define DEFAULT_TEMP_UNITS TEMP_UNITS_FAHRENHEIT
+#define DEFAULT_WINDUP_GUARD 10
+#define DEFAULT_K_P 10
+#define DEFAULT_K_I 1
+#define DEFAULT_K_D 1
+#define DEFAULT_TEMP_OFFSET 0
+#define DEFAULT_IGNORE_ERROR 0
+#define DEFAULT_SETPOINT_BREW 70
+#define DEFAULT_SETPOINT_STEAM 70
+#define DEFAULT_HYSTERESIS 1
+
 
 //////////////////////////////////////////////////////
 // Watchdog Settings
@@ -10,7 +56,6 @@
 
 
 
-
 // Internal macros
 #define hal_init HAL_Init
 #define MAKE32(by3,by2,by1,by0) ((uint32_t)((((((by3<<8)+by2)<<8)+by1)<<8)+by0))
diff --git a/inc/display.h b/inc/display.h
new file mode 100644
--- /dev/null
+++ b/inc/display.h
@@ -0,0 +1,14 @@
+#ifndef DISPLAY_H
+#define DISPLAY_H
+
+#include "states.h"
+
+#ifdef MAX31865_RTD_SENSOR
+#include "max31865.h"
+#endif
+
+void display_startup_screen(void);
+void display_1hz(void);
+void display_process(void);
+
+#endif
diff --git a/inc/dma.h b/inc/dma.h
deleted file mode 100644
--- a/inc/dma.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * dma.h
- *
- *  Created on: Aug 9, 2016
- *      Author: Nicholas Orlando
- */
-
-#ifndef DMA_H_
-#define DMA_H_
-
-/* Includes ------------------------------------------------------------------*/
-#include "stm32f3xx_hal.h"
-
-/* DMA memory to memory transfer handles -------------------------------------*/
-
-/* USER CODE BEGIN Includes */
-
-/* USER CODE END Includes */
-
-/* USER CODE BEGIN Private defines */
-
-/* USER CODE END Private defines */
-
-void MX_DMA_Init(void);
-
-/* USER CODE BEGIN Prototypes */
-
-/* USER CODE END Prototypes */
-
-#endif /* DMA_H_ */
diff --git a/inc/pid.h b/inc/pid.h
new file mode 100644
--- /dev/null
+++ b/inc/pid.h
@@ -0,0 +1,17 @@
+#ifndef PIDS_H
+#define PIDS_H
+
+#include "stm32f3xx_hal.h"
+#include "states.h"
+
+typedef struct {
+	int16_t last_pid_temp;
+	uint8_t last_pid_temp_frac;
+	int32_t i_state;
+} pid_state_t;
+
+void pid_init(void);
+float pid_process(void);
+int16_t pid_update(void);
+
+#endif
diff --git a/inc/pwmout.h b/inc/pwmout.h
new file mode 100644
--- /dev/null
+++ b/inc/pwmout.h
@@ -0,0 +1,15 @@
+#ifndef PWMOUT_H
+#define PWMOUT_H
+
+
+#define SSR_PIN GPIO_PIN_7
+#define SSR_GPIO_Port GPIOB
+#define SSR SSR_GPIO_Port, SSR_PIN
+
+
+
+void pwmout_init(void);
+void pwmout_process(float duty);
+
+
+#endif
diff --git a/inc/states.h b/inc/states.h
new file mode 100644
--- /dev/null
+++ b/inc/states.h
@@ -0,0 +1,104 @@
+#ifndef STATES_H
+#define STATES_H
+
+#include "stm32f3xx_hal.h"
+#include "config.h"
+
+typedef struct {
+    float temp;
+    uint8_t state_resume;
+    uint8_t state;
+    float setpoint;
+    uint8_t pid_enabled;
+    uint8_t error_code;
+} therm_status_t;
+
+typedef union
+{
+     struct {
+        uint32_t boottobrew;
+        uint32_t temp_units;
+        uint32_t windup_guard;
+        uint32_t k_p;
+        uint32_t k_i;
+        uint32_t k_d;
+        int32_t temp_offset;
+        uint32_t ignore_error;
+        int32_t setpoint_brew;
+        int32_t setpoint_steam;
+        uint32_t control_mode;
+        uint32_t sensor_type;
+        uint32_t plant_type;
+        uint32_t hysteresis;
+    } val;
+
+    uint16_t data[128];
+} therm_settings_t;
+
+enum tempunits {
+    TEMP_UNITS_CELSIUS = 0,
+    TEMP_UNITS_FAHRENHEIT,
+};
+
+enum state {
+    STATE_IDLE = 0,
+
+	STATE_SETSENSORTYPE,
+    STATE_SETMODE,
+    STATE_SETPLANTTYPE,
+    STATE_SETHYSTERESIS,
+    STATE_SETP,
+    STATE_SETI,
+    STATE_SETD,
+    STATE_SETSTEPS,
+    STATE_SETWINDUP,
+    STATE_SETBOOTTOBREW,
+    STATE_SETUNITS,
+    STATE_SETTEMPOFFSET,
+
+    STATE_PREHEAT,
+    STATE_MAINTAIN,
+
+    STATE_TC_ERROR,
+	STATE_RESET,
+};
+
+enum control_mode {
+    MODE_PID = 0,
+    MODE_THERMOSTAT,
+};
+
+enum plant_type {
+    PLANT_HEATER = 0,
+    PLANT_COOLER,
+};
+
+enum GOTO_MODE {
+	#ifdef BOOTLOADER_SHORTCUT
+	MODE_BOOTLOADER,
+	#endif
+	MODE_HEAT,
+	MODE_SETUP,
+	MODE_RESET,
+	MODE_SIZE,
+};
+
+enum RESET_MODE {
+	RESET_REBOOT = 0,
+	RESET_DEFAULTS,
+	RESET_BOOTLOADER,
+	RESET_EXIT,
+	RESET_SIZE,
+};
+
+enum SENSOR_TYPE {
+	SENSOR_NTC = 0,
+	SENSOR_TC_K,
+	SENSOR_TC_E,
+	SENSOR_TC_N,
+	SENSOR_TC_R,
+	SENSOR_TC_S,
+	SENSOR_TC_T,
+};
+
+#endif
diff --git a/inc/error.h b/inc/system/error.h
rename from inc/error.h
rename to inc/system/error.h
diff --git a/inc/flash.h b/inc/system/flash.h
rename from inc/flash.h
rename to inc/system/flash.h
--- a/inc/flash.h
+++ b/inc/system/flash.h
@@ -1,23 +1,19 @@
 #ifndef flash_h
 #define flash_h
 
+#include "states.h"
+
 #define FLASH_MAGIC_LOC 511
 #define FLASH_MAGIC_VALUE 0xbeef
 
 
-typedef union
-{
-	struct _values {
-		uint16_t can_id;
-		uint16_t tx_period;
-	} values;
-
-	uint16_t data[128];
-} settings_t;
 
 void flash_init(void);
 void flash_savesettings(void);
 void flash_restoresettings(void);
-settings_t* flash_getsettings(void);
+therm_settings_t* flash_getsettings(void);
+
+therm_status_t* runtime_status(void);
+
 
 #endif // flash_h
diff --git a/inc/gpio.h b/inc/system/gpio.h
rename from inc/gpio.h
rename to inc/system/gpio.h
--- a/inc/gpio.h
+++ b/inc/system/gpio.h
@@ -7,12 +7,19 @@
 // Define button push
 #define SW_D_Pin GPIO_PIN_4
 #define SW_D_GPIO_Port GPIOA
+#define SW_RIGHT SW_D_GPIO_Port, SW_D_Pin
+
 #define SW_B_Pin GPIO_PIN_5
 #define SW_B_GPIO_Port GPIOA
+#define SW_UP SW_B_GPIO_Port, SW_B_Pin
+
 #define SW_A_Pin GPIO_PIN_6
 #define SW_A_GPIO_Port GPIOA
+#define SW_LEFT SW_A_GPIO_Port, SW_A_Pin
+
 #define SW_C_Pin GPIO_PIN_7
 #define SW_C_GPIO_Port GPIOA
+#define SW_DOWN SW_C_GPIO_Port, SW_C_Pin
 
 #define SW_BTN_Pin GPIO_PIN_0
 #define SW_BTN_GPIO_Port GPIOB
@@ -22,6 +29,10 @@
 #define LED_GPIO_Port GPIOB
 #define LED LED_GPIO_Port, LED_PIN
 
+
+void user_input(uint16_t* to_modify);
+void user_input_signed(int32_t* to_modify);
+
 void gpio_init(void);
 void gpio_led_blueblink(uint8_t num_blinks);
 
diff --git a/inc/interrupts.h b/inc/system/interrupts.h
rename from inc/interrupts.h
rename to inc/system/interrupts.h
diff --git a/inc/stm32f3xx_hal_conf.h b/inc/system/stm32f3xx_hal_conf.h
rename from inc/stm32f3xx_hal_conf.h
rename to inc/system/stm32f3xx_hal_conf.h
diff --git a/inc/system/stringhelpers.h b/inc/system/stringhelpers.h
new file mode 100644
--- /dev/null
+++ b/inc/system/stringhelpers.h
@@ -0,0 +1,7 @@
+#ifndef STRINGHELPERS_H 
+#define STRINGHELPERS_H
+
+char* zitoa(int16_t i, char b[]);
+char* itoa_fp(int16_t i, uint8_t frac, char b[]);
+
+#endif
diff --git a/inc/system.h b/inc/system/system.h
rename from inc/system.h
rename to inc/system/system.h
diff --git a/inc/watchdog.h b/inc/system/watchdog.h
rename from inc/watchdog.h
rename to inc/system/watchdog.h
diff --git a/inc/tempsense.h b/inc/tempsense.h
new file mode 100644
--- /dev/null
+++ b/inc/tempsense.h
@@ -0,0 +1,12 @@
+#ifndef TEMPSENSE_H
+#define TEMPSENSE_H
+
+
+#define TEMPSENSE_MAX_CS_PIN GPIO_PIN_15
+#define TEMPSENSE_MAX_CS_PORT GPIOA
+
+
+void tempsense_init(void);
+float tempsense_readtemp(void);
+
+#endif
diff --git a/inc/thermostat.h b/inc/thermostat.h
new file mode 100644
--- /dev/null
+++ b/inc/thermostat.h
@@ -0,0 +1,10 @@
+#ifndef THERMOSTAT_H
+#define THERMOSTAT_H
+
+#include "stm32f3xx_hal.h"
+
+
+float thermostat_process(void);
+
+
+#endif
diff --git a/lib/max31856/max31856.c b/lib/max31856/max31856.c
new file mode 100644
--- /dev/null
+++ b/lib/max31856/max31856.c
@@ -0,0 +1,148 @@
+//
+// MAX31856: Driver to configure and read temperature from the MAX31856 thermoouple-to-Digital IC
+//
+
+#include "max31856.h"
+#include "states.h"
+#include "error.h"
+
+// Private variables
+static float temp_latest = 0.0;
+static float temp_avg = 0.0;
+
+static SPI_HandleTypeDef* spiport;
+static GPIO_TypeDef* csport;
+static uint32_t cspin;
+
+
+// Private prototypes
+static void __cs_assert(void);
+static void __cs_deassert(void);
+static void __write_reg(uint8_t reg, uint8_t data);
+static void __read_reg(uint8_t reg, uint8_t* rxbuf, uint8_t len);
+
+
+// Initialize the MAX31856 driver
+void max31856_init(SPI_HandleTypeDef* spi_port, GPIO_TypeDef* cs_port, uint32_t cs_pin, uint32_t sensor_type)
+{
+	// Set CS pin references
+	csport = cs_port;
+	cspin = cs_pin;
+
+	// Set SPI port reference
+	spiport = spi_port;
+
+	// Configure the CS pin for output
+	GPIO_InitTypeDef GPIO_InitStruct;
+	GPIO_InitStruct.Pin = cs_pin;
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+	HAL_GPIO_Init(cs_port, &GPIO_InitStruct);
+
+	// MAX31856
+	// - set to continuous conversion mode
+	// - probably no filtering, we'll do that on this side of things
+	// - set up to read the typical open/short faults but not the high/low alarms
+
+
+	// TODO: Enable open/short detection
+	// Enables auto conv
+	__write_reg(MAX31856_CR0_REG, MAX31856_CR0_OCFAULT0 | MAX31856_CR0_AUTOCONVERT);
+
+	// Averaging set to 1 sample, TC type set to K
+	__write_reg(MAX31856_CR1_REG, MAX31856_TCTYPE_K);
+
+	// sensor type - could we just mask bits off? maybe optimize the enum for this
+}
+
+
+// Pull reading from the MAX31856 IC
+void max31856_process(void)
+{
+	uint8_t tempbuf[4];
+	__read_reg(MAX31856_LTCBH_REG, tempbuf, 3);
+
+
+	int32_t temp24 = tempbuf[0] << 16 | tempbuf[1] << 8 | tempbuf[0];
+	if (temp24 & 0x800000) {
+		temp24 |= 0xFF000000;  // fix sign
+	}
+
+	temp24 >>= 5;  // bottom 5 bits are unused
+
+	float tempfloat = temp24;
+	tempfloat *= 0.0078125;
+
+	return tempfloat;
+
+
+
+
+	// Read temperature from the MAX31856 (approx 10hz optimally)
+	uint8_t data[] = {0,0,0,0};
+	HAL_SPI_Transmit(spiport, data, 1, 100);
+
+}
+
+
+// Return latest temperature reading (unaveraged, deg C)
+float max31856_latest_temp(void)
+{
+	return temp_latest;
+}
+
+
+// Return average temperature reading (deg C)
+float max31856_avg_temp(void)
+{
+	return temp_latest;
+}
+
+
+static void __write_reg(uint8_t reg, uint8_t data)
+{
+	// Set write bit
+	reg |= MAX31856_WRITE_BIT;
+
+	uint8_t outarr[2] = {reg, data};
+
+	// Assert the bus
+	__cs_assert();
+
+	// Write data
+	HAL_SPI_Transmit(spiport, outarr, 2, 100);
+
+	// Release the bus
+	__cs_deassert();
+}
+
+static void __read_reg(uint8_t reg, uint8_t* rxbuf, uint8_t len)
+{
+	// Transmit buffer only uses first item for reg addr
+	uint8_t regarr[1] = {reg};
+
+
+	// Assert the bus
+	__cs_assert();
+
+	// Send address
+	HAL_SPI_Transmit(spiport, regarr, 1, 100);
+
+	// Receive data
+	HAL_SPI_Receive(spiport, rxbuf, len, 100);
+
+	// Release bus
+	__cs_deassert();
+
+}
+
+static void __cs_assert(void)
+{
+	HAL_GPIO_WritePin(csport, cspin, 0);
+}
+
+static void __cs_deassert(void)
+{
+	HAL_GPIO_WritePin(csport, cspin, 1);
+}
diff --git a/lib/max31856/max31856.h b/lib/max31856/max31856.h
new file mode 100644
--- /dev/null
+++ b/lib/max31856/max31856.h
@@ -0,0 +1,64 @@
+#ifndef MAX31856_H
+#define MAX31856_H
+
+#include "stm32f3xx_hal.h"
+
+void max31856_init(SPI_HandleTypeDef* spi_port, GPIO_TypeDef* cs_port, uint32_t cs_pin, uint32_t sensor_type);
+void max31856_process(void);
+float max31856_latest_temp(void);
+float max31856_avg_temp(void);
+
+// Thanks to Adafruit:
+#define MAX31856_CR0_REG           0x00
+#define MAX31856_CR0_AUTOCONVERT   0x80
+#define MAX31856_CR0_1SHOT         0x40
+#define MAX31856_CR0_OCFAULT1      0x20
+#define MAX31856_CR0_OCFAULT0      0x10
+#define MAX31856_CR0_CJ            0x08
+#define MAX31856_CR0_FAULT         0x04
+#define MAX31856_CR0_FAULTCLR      0x02
+
+#define MAX31856_CR1_REG           0x01
+#define MAX31856_MASK_REG          0x02
+#define MAX31856_CJHF_REG          0x03
+#define MAX31856_CJLF_REG          0x04
+#define MAX31856_LTHFTH_REG        0x05
+#define MAX31856_LTHFTL_REG        0x06
+#define MAX31856_LTLFTH_REG        0x07
+#define MAX31856_LTLFTL_REG        0x08
+#define MAX31856_CJTO_REG          0x09
+#define MAX31856_CJTH_REG          0x0A
+#define MAX31856_CJTL_REG          0x0B
+#define MAX31856_LTCBH_REG         0x0C
+#define MAX31856_LTCBM_REG         0x0D
+#define MAX31856_LTCBL_REG         0x0E
+#define MAX31856_SR_REG            0x0F
+
+#define MAX31856_FAULT_CJRANGE     0x80
+#define MAX31856_FAULT_TCRANGE     0x40
+#define MAX31856_FAULT_CJHIGH      0x20
+#define MAX31856_FAULT_CJLOW       0x10
+#define MAX31856_FAULT_TCHIGH      0x08
+#define MAX31856_FAULT_TCLOW       0x04
+#define MAX31856_FAULT_OVUV        0x02
+#define MAX31856_FAULT_OPEN        0x01
+
+
+#define MAX31856_WRITE_BIT		   0x80
+
+typedef enum
+{
+  MAX31856_TCTYPE_B  = 0b0000,
+  MAX31856_TCTYPE_E  = 0b0001,
+  MAX31856_TCTYPE_J  = 0b0010,
+  MAX31856_TCTYPE_K  = 0b0011,
+  MAX31856_TCTYPE_N  = 0b0100,
+  MAX31856_TCTYPE_R  = 0b0101,
+  MAX31856_TCTYPE_S  = 0b0110,
+  MAX31856_TCTYPE_T  = 0b0111,
+  MAX31856_VMODE_G8  = 0b1000,
+  MAX31856_VMODE_G32 = 0b1100,
+} max31856_thermocoupletype_t;
+
+
+#endif
diff --git a/src/ssd1306.c b/lib/ssd1306/ssd1306.c
rename from src/ssd1306.c
rename to lib/ssd1306/ssd1306.c
--- a/src/ssd1306.c
+++ b/lib/ssd1306/ssd1306.c
@@ -5,7 +5,7 @@
 SPI_HandleTypeDef hspi1;
 
 
-
+// SPI handle accessor
 SPI_HandleTypeDef* spi_get()
 {
     return &hspi1;
@@ -33,80 +33,79 @@ static void WriteData(unsigned char data
 // Initialize OLED
 void ssd1306_init(void)
 {
-    __SPI3_CLK_ENABLE();
-    __GPIOA_CLK_ENABLE();
-    __GPIOB_CLK_ENABLE();
-    GPIO_InitTypeDef GPIO_InitStruct;
+	__SPI3_CLK_ENABLE();
+	__GPIOA_CLK_ENABLE();
+	__GPIOB_CLK_ENABLE();
+	GPIO_InitTypeDef GPIO_InitStruct;
 
-    /*Configure GPIO pins : OLED_CS_Pin OLED_RESET_Pin OLED_DC_Pin */
-    GPIO_InitStruct.Pin = OLED_CS_Pin|OLED_RESET_Pin|OLED_DC_Pin;
-    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    HAL_GPIO_Init(OLED_DC_GPIO_Port, &GPIO_InitStruct);
+	/*Configure GPIO pins : OLED_CS_Pin OLED_RESET_Pin OLED_DC_Pin */
+	GPIO_InitStruct.Pin = OLED_CS_Pin|OLED_RESET_Pin|OLED_DC_Pin;
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+	HAL_GPIO_Init(OLED_DC_GPIO_Port, &GPIO_InitStruct);
 
 
 	// Set up MOSI/MISO/SCK
-    GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5;
-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+	GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5;
+	GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+	GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
+	HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
 
 	// Set up SPI port for OLED
-    hspi1.Instance = SPI3;
-    hspi1.Init.Mode = SPI_MODE_MASTER;
-    hspi1.Init.Direction = SPI_DIRECTION_2LINES;
-    hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
-    hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
-    hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
-    hspi1.Init.NSS = SPI_NSS_SOFT;
-    hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
-    hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
-    hspi1.Init.TIMode = SPI_TIMODE_DISABLED;
-    hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
-    hspi1.Init.NSSPMode = SPI_NSS_PULSE_ENABLED;
-    HAL_SPI_Init(&hspi1);
-
+	hspi1.Instance = SPI3;
+	hspi1.Init.Mode = SPI_MODE_MASTER;
+	hspi1.Init.Direction = SPI_DIRECTION_2LINES;
+	hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
+	hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
+	hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
+	hspi1.Init.NSS = SPI_NSS_SOFT;
+	hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
+	hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
+	hspi1.Init.TIMode = SPI_TIMODE_DISABLED;
+	hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
+	hspi1.Init.NSSPMode = SPI_NSS_PULSE_ENABLED;
+	HAL_SPI_Init(&hspi1);
 
 
-  /* Generate a reset */
-  SSD_Reset_Low();
-  uint32_t i;
-  for(i=5000; i>1; i--)
-  SSD_Reset_High();
+	// Generate a reset
+	SSD_Reset_Low();
+	uint32_t i;
+	for(i=5000; i>1; i--)
+	SSD_Reset_High();
 
-  WriteCommand(0xAE);
-  WriteCommand(0xD5);
-  WriteCommand(0x80);
-  WriteCommand(0xA8);
-  WriteCommand(0x1F);
-  WriteCommand(0xD3);
-  WriteCommand(0x00);
-  WriteCommand(0x40 | 0x00); // line #0
-  WriteCommand(0x8D);
-  WriteCommand(0x14); //10 or 14 if not externalvcc
-  WriteCommand(0x20);
-  WriteCommand(0x00);
-//  WriteCommand(0xA0 | 0x1); // segremap (normal)
-  WriteCommand(0xA0); // segremap (flip)
-//  WriteCommand(0xC8); // comscandec (normal)
-  WriteCommand(0xC0); // comscandec (flip)
-  WriteCommand(0xDA); // setcompins
-  WriteCommand(0x02);
-  WriteCommand(0x81); // contrast
-  WriteCommand(0x0F); // contrast value. 8f is a good one.
-  WriteCommand(0xD9);
-  WriteCommand(0xF1); //22 or F1 if not externalvcc
-  WriteCommand(0xDB);
-  WriteCommand(0x40);
-  WriteCommand(0xA4); // dispalyallon_resume
-  WriteCommand(0xA6); // normaldisplay
+	WriteCommand(0xAE);
+	WriteCommand(0xD5);
+	WriteCommand(0x80);
+	WriteCommand(0xA8);
+	WriteCommand(0x1F);
+	WriteCommand(0xD3);
+	WriteCommand(0x00);
+	WriteCommand(0x40 | 0x00); // line #0
+	WriteCommand(0x8D);
+	WriteCommand(0x14); //10 or 14 if not externalvcc
+	WriteCommand(0x20);
+	WriteCommand(0x00);
+	//  WriteCommand(0xA0 | 0x1); // segremap (normal)
+	WriteCommand(0xA0); // segremap (flip)
+	//  WriteCommand(0xC8); // comscandec (normal)
+	WriteCommand(0xC0); // comscandec (flip)
+	WriteCommand(0xDA); // setcompins
+	WriteCommand(0x02);
+	WriteCommand(0x81); // contrast
+	WriteCommand(0x0F); // contrast value. 8f is a good one.
+	WriteCommand(0xD9);
+	WriteCommand(0xF1); //22 or F1 if not externalvcc
+	WriteCommand(0xDB);
+	WriteCommand(0x40);
+	WriteCommand(0xA4); // dispalyallon_resume
+	WriteCommand(0xA6); // normaldisplay
 
 
-  WriteCommand(0xAF); // display on
+	WriteCommand(0xAF); // display on
 }
 
 
@@ -209,31 +208,39 @@ static const char fontData[][5] =
     {0x00,0x00,0x7F,0x00,0x00},         //   ( 92)  | - 0x007C Vertical Line
     {0x00,0x41,0x36,0x08,0x00},         //   ( 93)  } - 0x007D Right Curly Bracket
     {0x02,0x01,0x02,0x04,0x02},         //   ( 94)  ~ - 0x007E Tilde
-    {0x08,0x14,0x2A,0x14,0x22},         //   ( 95) << - 0x00AB Left-Pointing Double Angle Quotation Mark
+	{0x08,0x14,0x2A,0x14,0x22},         //   ( 95) << - 0x00AB Left-Pointing Double Angle Quotation Mark
     {0x00,0x02,0x05,0x02,0x00},         //   ( 96)    - 0x00B0 Degree Sign
-//    {0x44,0x44,0x5F,0x44,0x44},         //   ( 97) +- - 0x00B1 Plus-Minus Sign
-//    {0x7E,0x20,0x20,0x10,0x3E},         //   ( 98)  u - 0x00B5 Micro Sign
-//    {0x22,0x14,0x2A,0x14,0x08},         //   ( 99) >> - 0x00BB Right-Pointing Double Angle Quotation Mark
-//    {0x30,0x48,0x45,0x40,0x20},         //   (100)  ? - 0x00BF Inverted Question Mark
-//    {0x22,0x14,0x08,0x14,0x22},         //   (101)  x - 0x00D7 Multiplcation Sign
-//    {0x08,0x08,0x2A,0x08,0x08},         //   (102)  + - 0x00F7 Division Sign
-//    {0x18,0x14,0x08,0x14,0x0C},         //   (103)    - 0x221E Infinity
-//    {0x44,0x4A,0x4A,0x51,0x51},         //   (104)  < - 0x2264 Less-Than or Equal to
-//    {0x51,0x51,0x4A,0x4A,0x44},         //   (105)  > - 0x2265 Greater-Than or Equal to
-//    {0x54,0x14,0x64,0x08,0x70},         //   (106)  .: - RF Symbol
-//    {0x70,0x7C,0x72,0x7C,0x70},         //   (107)  ^ - Lock symbol
-//    {0x70,0x5C,0x52,0x54,0x70},         //   (108)  / - Unlock symbol
-//    {0x0C,0x1E,0x3C,0x1E,0x0C},         //   (109)  <3 - Heart Symbol
-//    {0x18,0x22,0xFF,0x12,0x0C},         //   (110)  U - USB Symbol
+    {0x44,0x44,0x5F,0x44,0x44},         //   ( 97) +- - 0x00B1 Plus-Minus Sign
+    {0x7E,0x20,0x20,0x10,0x3E},         //   ( 98)  u - 0x00B5 Micro Sign
+    {0x22,0x14,0x2A,0x14,0x08},         //   ( 99) >> - 0x00BB Right-Pointing Double Angle Quotation Mark
+    {0x30,0x48,0x45,0x40,0x20},         //   (100)  ? - 0x00BF Inverted Question Mark
+    {0x22,0x14,0x08,0x14,0x22},         //   (101)  x - 0x00D7 Multiplication Sign
+    {0x08,0x08,0x2A,0x08,0x08},         //   (102)  + - 0x00F7 Division Sign
+    {0x18,0x14,0x08,0x14,0x0C},         //   (103)    - 0x221E Infinity
+    {0x44,0x4A,0x4A,0x51,0x51},         //   (104)  < - 0x2264 Less-Than or Equal to
+    {0x51,0x51,0x4A,0x4A,0x44},         //   (105)  > - 0x2265 Greater-Than or Equal to
+    {0x54,0x14,0x64,0x08,0x70},         //   (106)  .: - RF Symbol
+    {0x70,0x7C,0x72,0x7C,0x70},         //   (107)  ^ - Lock symbol
+    {0x70,0x5C,0x52,0x54,0x70},         //   (108)  / - Unlock symbol
+    {0x0C,0x1E,0x3C,0x1E,0x0C},         //   (109)  <3 - Heart Symbol
+    {0x18,0x22,0xFF,0x12,0x0C},         //   (110)  U - USB Symbol
+	{0x22,0x5d,0x22,0x00,0x00},			//   (111) ez updown
+	{0x14,0x3e,0x14,0x00,0x00},         //   (112) ez updown short
 };
+/*
 
 
+*/
+
+// Set start page
 static void setStartPage(unsigned char d)
 {
     WriteCommand(0xB0|d);       // Set Page Start Address for Page Addressing Mode
                                 // Default => 0xB0 (0x00)
 }
-/* Below are functions used to configure the OLED */
+
+
+// Set start column
 static void setStartColumn(unsigned char d)
 {
     WriteCommand(0x00+d%16);    // Set Lower Column Start Address for Page Addressing Mode
@@ -242,6 +249,7 @@ static void setStartColumn(unsigned char
 }
 
 
+// Therm logo
 const uint8_t row[4][32] = {
 
 	{0x00,0x00,0x01,0x03,0x07,0x0F,0x1E,0x3C,0x3C,0x7C,0x7C,0x7C,0xFC,0xFF,0xFF,0xFC,0xFC,0xFC,0xFC,0xFF,0x7F,0x7F,0x7F,0x3C,0x3C,0x1C,0x0C,0x06,0x03,0x01,0x00,0x00},
@@ -254,6 +262,8 @@ const uint8_t row[4][32] = {
 
 };
 
+
+// Clear the screen
 void ssd1306_clearscreen()
 {
     uint8_t i = 0;
@@ -270,6 +280,8 @@ void ssd1306_clearscreen()
     WriteData(0x00);
 }
 
+
+// Draw the therm logo on the left side of the OLED
 void ssd1306_drawlogo()
 {
     uint8_t i = 0;
@@ -307,10 +319,11 @@ void ssd1306_drawlogo()
     WriteData(0x00);
 }
 
-/* Print a single character from font.cpp */
-void ssd1306_drawchar(char ascii, unsigned char row, unsigned char xPos)
+
+// Print a single character
+void ssd1306_drawchar(unsigned char ascii, unsigned char row, unsigned char xPos)
 {
-    const char *srcPointer = (char*)-1;
+    const unsigned char *srcPointer = (char*)-1;
 
     srcPointer = &fontData[(ascii-32)][0];
 
@@ -325,7 +338,9 @@ void ssd1306_drawchar(char ascii, unsign
     WriteData(0x00);
 }
 
-void ssd1306_drawcharbig(char ascii, unsigned char row, unsigned char xPos)
+
+// Print a single large character
+void ssd1306_drawcharbig(unsigned char ascii, unsigned char row, unsigned char xPos)
 {
     const char *srcPointer = (char*)-1;
 
@@ -387,11 +402,13 @@ void ssd1306_drawcharbig(char ascii, uns
 
 }
 
-void ssd1306_drawstring(const char *dataPtr, unsigned char row, unsigned char xPos)
+
+// Print a string to the display
+void ssd1306_drawstring(const unsigned char *dataPtr, unsigned char row, unsigned char xPos)
 {
-    char *srcPointer;
+    unsigned char *srcPointer;
 
-    srcPointer = (char*)dataPtr;
+    srcPointer = (unsigned char*)dataPtr;
     ssd1306_drawchar(' ',row,xPos); // NBSP must be written first before the string start
 
     while(1)
@@ -404,7 +421,8 @@ void ssd1306_drawstring(const char *data
 }
 
 
-void ssd1306_drawstringbig(const char *dataPtr, unsigned char row, unsigned char xPos)
+// Print a string to the display, big font
+void ssd1306_drawstringbig(const unsigned char *dataPtr, unsigned char row, unsigned char xPos)
 {
     char *srcPointer;
 
diff --git a/inc/ssd1306.h b/lib/ssd1306/ssd1306.h
rename from inc/ssd1306.h
rename to lib/ssd1306/ssd1306.h
--- a/inc/ssd1306.h
+++ b/lib/ssd1306/ssd1306.h
@@ -31,11 +31,13 @@
 //#define   SPI_SendByte(data)  SPI_I2S_SendData(SPI1,data)
 //#define   SPI_Wait()       while(!(SPI1->SR&SPI_FLAG_TXE));while(SPI1->SR&SPI_FLAG_BSY);
 
+SPI_HandleTypeDef* spi_get();
+
 void ssd1306_init(void);
-void ssd1306_drawchar(char ascii, unsigned char row, unsigned char xPos);
-void ssd1306_drawcharbig(char ascii, unsigned char row, unsigned char xPos);
-void ssd1306_drawstring(const char *dataPtr, unsigned char row, unsigned char xPos);
-void ssd1306_drawstringbig(const char *dataPtr, unsigned char row, unsigned char xPos);
+void ssd1306_drawchar(unsigned char ascii, unsigned char row, unsigned char xPos);
+void ssd1306_drawcharbig(unsigned char ascii, unsigned char row, unsigned char xPos);
+void ssd1306_drawstring(const unsigned char *dataPtr, unsigned char row, unsigned char xPos);
+void ssd1306_drawstringbig(const unsigned char *dataPtr, unsigned char row, unsigned char xPos);
 void ssd1306_drawlogo();
 void ssd1306_clearscreen();
 
diff --git a/src/buttons.c b/src/buttons.c
deleted file mode 100644
--- a/src/buttons.c
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * buttons.c
- *
- *  Created on: Oct 22, 2016
- *      Author: Nicholas Orlando
- */
-
-#include "stm32f3xx_hal.h"
-#include "stm32f3xx.h"
-#include "gpio.h"
-#include "ssd1306.h"
-#include "buttons.h"
-#include <math.h>
-
-
-uint32_t last_button_debounce_time = 0;		// the system time at which the button values were last checked
-uint32_t debounce_resolution = 5;	// period in which to check pin state in milliseconds
-float filter_fraction = 0.2;	// a number between 0 and 1, which adjusts the heaviness of the filter.
-								// 0 = never changes and 1 = no filter at all.
-uint8_t l_thresh = 20; //lower threshold for when then button is considered pressed vs not pressed.
-uint8_t u_thresh = 80; //upper threshold for when then button is considered pressed vs not pressed.
-
-uint8_t temp_counter = 0; // temporary counter for testing purposes
-
-float sw_btn_avg = 0; // some sort of running average
-uint8_t sw_btn_state = NOT_PRESSED; // the state of the pin after filtering
-uint8_t sw_btn_old_state = NOT_PRESSED; // the state of the pin after filtering
-
-void freaking_debounce(void)
-{
-	if(HAL_GetTick() - last_button_debounce_time > debounce_resolution)
-	{
-		char buffer[256]; // needed for writing stuff to screen
-
-		//averaging to filter button presses
-		if(HAL_GPIO_ReadPin(SW_BTN) == GPIO_PIN_RESET)
-		{
-			sw_btn_avg = sw_btn_avg + filter_fraction * (100 - sw_btn_avg);
-		}
-		else
-		{
-			sw_btn_avg = sw_btn_avg + filter_fraction * (0 - sw_btn_avg);
-		}
-
-		// check to see if the btn average value has crossed a threshold
-		if(sw_btn_avg < l_thresh)
-		{
-			sw_btn_state = NOT_PRESSED;
-//			snprintf(buffer, 256, "NOT_PRESSED");
-//			ssd1306_drawstring(buffer, 1, 0);
-		}
-		if(sw_btn_avg > u_thresh)
-		{
-			sw_btn_state = PRESSED;
-//			snprintf(buffer, 256, "PRESSED    ");
-//			ssd1306_drawstring(buffer, 1, 0);
-		}
-
-		// do something when state has changed
-		if((sw_btn_state == PRESSED) && (sw_btn_old_state == NOT_PRESSED))
-		{
-//			temp_counter++;
-//			snprintf(buffer, 256, "%i", temp_counter);
-//			ssd1306_drawstring(buffer, 1, 0);
-		}
-
-
-//		snprintf(buffer, 256, "sw-btn-avg: %.1f", sw_btn_avg);
-//		ssd1306_drawstring(buffer, 2, 0);
-
-		// save previous button states
-		sw_btn_old_state = sw_btn_state;
-
-		last_button_debounce_time = HAL_GetTick();
-	}
-}
diff --git a/src/display.c b/src/display.c
new file mode 100644
--- /dev/null
+++ b/src/display.c
@@ -0,0 +1,748 @@
+//
+// Display: render menus on OLED display
+//
+
+#include "display.h"
+#include "gpio.h"
+#include "ssd1306/ssd1306.h"
+#include "system/stringhelpers.h"
+#include "flash.h"
+
+// Private function prototypes
+static void draw_setpoint(therm_status_t* status);
+
+
+// Button transition variables
+static uint8_t sw_btn_last = 0;
+static uint8_t sw_up_last = 0;
+static uint8_t sw_down_last = 0;
+static uint8_t sw_left_last = 0;
+static uint8_t sw_right_last = 0;
+
+
+// Buttonpress macros
+#define SW_BTN_PRESSED (sw_btn_last == 0 && sw_btn == 1) // rising edge on buttonpress
+#define SW_UP_PRESSED (sw_up_last == 0 && sw_up == 1)
+#define SW_DOWN_PRESSED (sw_down_last == 0 && sw_down == 1)
+#define SW_LEFT_PRESSED (sw_left_last == 0 && sw_left == 1)
+#define SW_RIGHT_PRESSED (sw_right_last == 0 && sw_right == 1)
+
+
+// States
+static uint8_t trigger_drawsetpoint = 1;
+static int16_t last_temp = 21245;
+static int16_t last_temp_frac = 21245;
+static int16_t last_state = 123;
+static uint8_t goto_mode = MODE_HEAT;
+static uint8_t reset_mode = RESET_REBOOT;
+
+static char* sensor_lookup[] = {"NTC", "K  ", "E  ", "N  ", "R  ", "S  ", "T  "};
+
+static uint8_t toggle = 0;
+
+void display_1hz(void)
+{
+	toggle = !toggle;
+}
+
+static char updown(void)
+{
+	if(toggle)
+		return '\x8f';
+	else
+		return '\x90';
+}
+
+// Display state machine
+void display_process(void)
+{
+	therm_status_t* status = runtime_status();
+	therm_settings_t* set = flash_getsettings();
+
+
+    uint8_t state_changed = status->state != last_state;
+    last_state = status->state;
+    
+    uint8_t temp_changed = status->temp != last_temp;
+    last_temp = status->temp;
+
+    uint8_t sw_btn = !HAL_GPIO_ReadPin(SW_BTN);
+    uint8_t sw_up = !HAL_GPIO_ReadPin(SW_UP);
+    uint8_t sw_down = !HAL_GPIO_ReadPin(SW_DOWN);
+    uint8_t sw_left = !HAL_GPIO_ReadPin(SW_LEFT);
+    uint8_t sw_right = !HAL_GPIO_ReadPin(SW_RIGHT);
+
+    switch(status->state)
+    {
+        // Idle state
+        case STATE_IDLE:
+        {
+            // Write text to OLED
+            // [ therm :: idle ]
+            ssd1306_drawstring("therm \x87 idle ", 0, 40);
+            status->pid_enabled = 0;
+
+            if(temp_changed || state_changed) {
+                char tempstr[16];
+                snprintf(tempstr, 16, "Temp: %g", status->temp);
+                ssd1306_drawstring("             ", 3, 40);
+
+                ssd1306_drawstring(tempstr, 3, 40);
+            }
+
+            if (state_changed) {
+            	ssd1306_drawlogo();
+            }
+
+            switch(goto_mode) {
+
+                case MODE_HEAT:
+                {
+					if(set->val.plant_type == PLANT_HEATER)
+						ssd1306_drawstring("\x83 heat      ", 1, 40);
+					else
+						ssd1306_drawstring("\x83 cool      ", 1, 40);
+
+                } break;
+
+                case MODE_SETUP:
+                {
+                    ssd1306_drawstring("\x83 setup    ", 1, 40);
+                } break;
+
+                case MODE_RESET:
+                {
+                    ssd1306_drawstring("\x83 reset    ", 1, 40);
+                } break;
+
+				#ifdef BOOTLOADER_SHORTCUT
+                case MODE_BOOTLOADER:
+                {
+                    ssd1306_drawstring("\x83 dfu      ", 1, 40);
+                }
+				#endif
+            }
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                switch(goto_mode) {
+                    case MODE_HEAT:
+                        status->state = STATE_PREHEAT;
+                        break;
+                    case MODE_SETUP:
+                        status->state = STATE_SETSENSORTYPE;
+                        break;
+                    case MODE_RESET:
+                        status->state = STATE_RESET;
+                        reset_mode = RESET_REBOOT;
+                        break;
+					#ifdef BOOTLOADER_SHORTCUT
+                    case MODE_BOOTLOADER:
+                        ssd1306_clearscreen();
+                        ssd1306_drawstring("Bootloader Entered", 0, 0);
+                        ssd1306_drawstring("Device won't boot", 2, 0);
+                        ssd1306_drawstring("until reflashed!", 3, 0);
+//                        bootloader_enter(); // Resets into bootloader
+                        status->state = STATE_RESET; // Just in case
+                        break;
+					#endif
+                    default:
+                        status->state = STATE_PREHEAT;
+                }
+            }
+            else if(SW_DOWN_PRESSED && goto_mode < (MODE_SIZE - 1)) {
+                goto_mode++;
+            }
+            else if(SW_UP_PRESSED && goto_mode > 0) {
+                goto_mode--;
+            }
+
+
+            // Event Handler
+            // N/A
+
+        } break;
+
+
+
+        case STATE_SETSENSORTYPE:
+        {
+            // Write text to OLED
+            // [ therm :: set mode ]
+            // [ m =          ]
+            ssd1306_drawstring("Sensor Type", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+			ssd1306_drawstring(sensor_lookup[set->val.sensor_type], 1, 60);
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETMODE;
+            }
+            else
+            {
+            	user_input((uint16_t*)&set->val.sensor_type);
+                if(set->val.sensor_type > 6)
+                	set->val.sensor_type = 6;
+
+            }
+            // Event Handler
+            // N/A
+
+        } break;
+
+        case STATE_SETMODE:
+        {
+            // Write text to OLED
+            // [ therm :: set mode ]
+            // [ m =          ]
+            ssd1306_drawstring("Control Mode", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+
+            if(set->val.control_mode == MODE_PID)
+                ssd1306_drawstring("PID       ", 1, 60);
+            else
+                ssd1306_drawstring("Thermostat", 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+            
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETPLANTTYPE;
+            }
+            else if (!HAL_GPIO_ReadPin(SW_UP)) {
+                set->val.control_mode = MODE_PID;
+            }
+            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {
+                set->val.control_mode = MODE_THERMOSTAT;
+            }
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+
+        case STATE_SETPLANTTYPE:
+        {
+            // Write text to OLED
+            // [ therm :: set mode ]
+            // [ m =          ]
+            ssd1306_drawstring("Plant Type", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52	);
+
+            if(set->val.plant_type == PLANT_HEATER)
+                ssd1306_drawstring("Heater", 1, 60);
+            else
+                ssd1306_drawstring("Cooler", 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+            
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                if(set->val.control_mode == MODE_PID)
+                    status->state = STATE_SETP;
+                else
+                    status->state = STATE_SETHYSTERESIS;
+            }
+            else if (!HAL_GPIO_ReadPin(SW_UP)) {
+                set->val.plant_type = PLANT_COOLER;
+            }
+            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {
+                set->val.plant_type = PLANT_HEATER;
+            }
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+
+
+
+        case STATE_SETP:
+        {
+            // Write text to OLED
+            // [ therm :: set p ]
+            // [ p = 12         ]
+            ssd1306_drawstring("Proportional", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            char tempstr[12];
+            snprintf(tempstr, 12, "P=%d", set->val.k_p);
+            ssd1306_drawstring(tempstr, 1, 60);
+            ssd1306_drawstring("Press to accept", 3, 40);
+            
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETI;
+            }
+            else {
+                user_input((uint16_t*)&set->val.k_p);
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+        case STATE_SETI:
+        {
+            // Write text to OLED
+            // [ therm :: set i ]
+            // [ i = 12         ]
+            ssd1306_drawstring("Integral", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            char tempstr[12];
+            snprintf(tempstr, 12, "I=%d", set->val.k_i);
+            ssd1306_drawstring(tempstr, 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+            
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETD;
+            }
+            else {
+                user_input((uint16_t*)&set->val.k_i);
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+        case STATE_SETD:
+        {
+            // Write text to OLED
+            // [ therm :: set d ]
+            // [ d = 12         ]
+            ssd1306_drawstring("Derivative", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            char tempstr[12];
+            snprintf(tempstr, 12, "D=%d", set->val.k_d);
+            ssd1306_drawstring(tempstr, 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETWINDUP;
+            }
+            else {
+                user_input((uint16_t*)&set->val.k_d);
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+
+
+        case STATE_SETHYSTERESIS:
+        {
+            // Write text to OLED
+            ssd1306_drawstring("Hysteresis", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            char tempstr[12];
+            snprintf(tempstr, 12, "H=%d", set->val.hysteresis);
+            ssd1306_drawstring(tempstr, 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETBOOTTOBREW;
+            }
+            else {
+                user_input((uint16_t*)&set->val.hysteresis);
+            }
+
+            // Event Handler
+            // N/A
+
+        } break;
+
+
+        case STATE_SETWINDUP:
+        {
+            // Write text to OLED
+            // [ therm :: set windup ]
+            // [ g = 12         ]
+            ssd1306_drawstring("Windup Guard", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            char tempstr[12];
+            snprintf(tempstr, 12, "G=%d", set->val.windup_guard);
+            ssd1306_drawstring(tempstr, 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETBOOTTOBREW;
+            }
+            else {
+                user_input((uint16_t*)&set->val.windup_guard);
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+        case STATE_SETBOOTTOBREW:
+        {
+            // Write text to OLED
+            // [ therm :: set windup ]
+            // [ g = 12         ]
+            ssd1306_drawstring("Start on Boot", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawstring("sob=", 1, 50);
+            
+            ssd1306_drawchar(updown(), 1, 43);
+
+            if(set->val.boottobrew)
+                ssd1306_drawstring("Enable ", 1, 75);
+            else
+                ssd1306_drawstring("Disable", 1, 75);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETUNITS;
+            }
+            else if(!HAL_GPIO_ReadPin(SW_UP)) {
+                set->val.boottobrew = 1;
+            }
+            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {
+                set->val.boottobrew = 0;
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+        case STATE_SETUNITS:
+        {
+            // Write text to OLED
+            // [ therm :: set windup ]
+            // [ g = 12         ]
+            ssd1306_drawstring("Units: ", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            if(set->val.temp_units == TEMP_UNITS_FAHRENHEIT)
+                ssd1306_drawstring("Fahrenheit", 1, 60);
+            else
+                ssd1306_drawstring("Celsius   ", 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_SETTEMPOFFSET;
+            }
+            else if(!HAL_GPIO_ReadPin(SW_UP)) {
+                set->val.temp_units = TEMP_UNITS_FAHRENHEIT;
+            }
+            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {
+                set->val.temp_units = TEMP_UNITS_CELSIUS;
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+
+        case STATE_SETTEMPOFFSET:
+        {
+            // Write text to OLED
+            // [ therm :: set temp offset ]
+            // [ g = 12         ]
+            ssd1306_drawstring("Temp Cal Offset", 0, 40);
+            ssd1306_drawlogo();
+
+            ssd1306_drawchar(updown(), 1, 52);
+
+            char tempstr[12];
+            snprintf(tempstr, 12, "O=%d", set->val.temp_offset);
+            ssd1306_drawstring(tempstr, 1, 60);
+
+            ssd1306_drawstring("Press to accept", 3, 40);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+//                flash_save(set);
+                status->state = STATE_IDLE;
+            }
+            else {
+                user_input_signed(&set->val.temp_offset);
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+
+        case STATE_PREHEAT:
+        {
+            // Write text to OLED
+            // [ therm : preheating brew ]
+            // [ 30 => 120 C             ]
+            if(set->val.plant_type == PLANT_HEATER)
+                ssd1306_drawstring("Preheating...", 0, 0);
+            else
+                ssd1306_drawstring("Precooling...", 0, 0);
+
+            //ssd1306_drawlogo();
+            draw_setpoint(status);
+
+            status->pid_enabled = 1;
+            status->setpoint = set->val.setpoint_brew;
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_IDLE;
+            }
+            else {
+                user_input_signed(&set->val.setpoint_brew);
+            }
+
+            // Event Handler
+            if(status->temp >= status->setpoint) {
+                status->state = STATE_MAINTAIN;
+            }
+ 
+        } break;
+
+        case STATE_MAINTAIN:
+        {
+            // Write text to OLED
+            // [ therm : ready to brew ]
+            // [ 30 => 120 C           ]
+
+            if(set->val.plant_type == PLANT_HEATER)
+                ssd1306_drawstring("Preheated!", 0, 0);
+            else
+                ssd1306_drawstring("Precooled!", 0, 0);
+
+            draw_setpoint(status);
+            status->pid_enabled = 1;
+            status->setpoint = set->val.setpoint_brew;
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_IDLE;
+            }
+            else {
+                user_input_signed(&set->val.setpoint_brew);
+            }
+
+            // Event Handler
+            // N/A
+ 
+        } break;
+
+        // Thermocouple error
+        case STATE_TC_ERROR:
+        {
+            // Write text to OLED
+            // [ therm : ready to steam ]
+            // [ 30 => 120 C            ]
+            ssd1306_drawstring("Error:              ", 0, 0);
+
+            char tempstr[6];
+            itoa(status->error_code, tempstr, 10);
+            ssd1306_drawstring(tempstr, 0, 57);
+
+            //TODO: add RTD error codes
+
+            if(status->error_code == 1)
+                ssd1306_drawstring("    TC Open Circuit", 1, 0);
+            else if(status->error_code == 4)
+                ssd1306_drawstring("    TC Short to GND", 1, 0);
+            else if(status->error_code == 8)
+                ssd1306_drawstring("    TC Short to VCC", 1, 0);
+            else
+                ssd1306_drawstring("#?, Unknown Error", 1, 0);
+            ssd1306_drawstring("                    ", 2, 0);
+
+            ssd1306_drawstring("-> to ignore all or", 2, 0);
+            ssd1306_drawstring("press to continue", 3, 0);
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                status->state = STATE_IDLE;
+				#ifdef MAX31865_RTD_SENSOR
+				max31865_clear_errors(spi_get());
+				#endif
+            }
+            else if(SW_RIGHT_PRESSED) {
+                set->val.ignore_error = 1;
+                status->state = STATE_IDLE;
+            }
+            // Event Handler
+            // Maybe handle if TC is plugged in
+            // N/A
+ 
+        } break;
+
+
+        // Reset state
+        case STATE_RESET:
+        {
+            // Write text to OLED
+            // [ therm :: reset ]
+            ssd1306_drawstring("therm :: reset ", 0, 40);
+            status->pid_enabled = 0;
+
+            ssd1306_drawlogo();
+
+            switch(reset_mode) {
+				case RESET_DEFAULTS:
+				{
+					ssd1306_drawstring("-> defaults   ", 1, 40);
+				} break;
+                case RESET_BOOTLOADER:
+                {
+                    ssd1306_drawstring("-> bootloader ", 1, 40);
+                } break;
+                case RESET_REBOOT:
+                {
+                    ssd1306_drawstring("-> reboot     ", 1, 40);
+                } break;
+                case RESET_EXIT:
+                {
+                    ssd1306_drawstring("-> exit       ", 1, 40);
+                } break;
+            }
+
+            // Button handler
+            if(SW_BTN_PRESSED) {
+                switch(reset_mode) {
+                    case RESET_BOOTLOADER:
+                    {
+                        ssd1306_clearscreen();
+                        ssd1306_drawstring("Bootloader Entered", 0, 0);
+                        ssd1306_drawstring("Device won't boot", 2, 0);
+                        ssd1306_drawstring("until reflashed!", 3, 0);
+                        HAL_Delay(1000);
+//                        bootloader_enter(); // Resets into bootloader
+                        status->state = STATE_RESET; // Just in case
+                    } break;
+                    case RESET_DEFAULTS:
+                    {
+                        status->state = STATE_RESET;
+//                        flash_load_defaults(set);
+//                        flash_save(set);
+                        NVIC_SystemReset();
+                    } break;
+                    case RESET_REBOOT:
+                    {
+                        status->state = STATE_RESET;
+                        NVIC_SystemReset();
+                    } break;
+                    case RESET_EXIT:
+                    {
+                        status->state = STATE_IDLE;
+                    } break;
+                }
+            }
+            else if(SW_DOWN_PRESSED && reset_mode < (RESET_SIZE-1)) {
+                reset_mode++;
+            }
+            else if(SW_UP_PRESSED && reset_mode > 0) {
+                reset_mode--;
+            }
+
+
+            // Event Handler
+            // N/A
+
+        } break;
+
+        // Something is terribly wrong
+        default:
+        {
+            status->state = STATE_IDLE;
+            status->pid_enabled = 0;
+
+        } break;
+            
+    }
+
+    if(last_state != status->state) {
+        // Clear screen on state change
+        goto_mode = MODE_HEAT;
+        trigger_drawsetpoint = 1;
+        ssd1306_clearscreen();
+    }
+
+    // Last buttonpress
+    sw_btn_last = sw_btn;
+    sw_up_last = sw_up;
+    sw_down_last = sw_down;
+    sw_left_last = sw_left;
+    sw_right_last = sw_right;
+}
+
+
+static float temp_last = 43002.0;
+static float setpoint_last = 10023.0;
+
+// Draw current setpoint on display
+static void draw_setpoint(therm_status_t* status) {
+    // FIXME: need to do this when switching modes too
+    if(status->temp != temp_last || trigger_drawsetpoint) {
+        char tempstr[8];
+        snprintf(tempstr, 8, "%g     ", status->temp);
+        ssd1306_drawstringbig(tempstr, 3, 0);
+    }
+
+    if(trigger_drawsetpoint) 
+        ssd1306_drawstringbig(">", 3, 74);
+
+    if(status->setpoint != setpoint_last || trigger_drawsetpoint) {
+        char tempstr[4];
+        snprintf(tempstr, 4, "%g     ", status->setpoint);
+        ssd1306_drawstringbig(tempstr, 3, 90);
+    }
+
+    trigger_drawsetpoint = 0;
+    setpoint_last = status->setpoint;
+    temp_last = status->temp;
+}
+
+void display_startup_screen() {
+    ssd1306_clearscreen();
+    ssd1306_drawstring("therm v0.4", 1, 40);
+    ssd1306_drawstring("protofusion.org/therm", 3, 0);
+}
+
+// vim:softtabstop=4 shiftwidth=4 expandtab 
diff --git a/src/dma.c b/src/dma.c
deleted file mode 100644
--- a/src/dma.c
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * dma.c
- *
- *  Created on: Aug 9, 2016
- *      Author: Nicholas Orlando
- */
-
-/* Includes ------------------------------------------------------------------*/
-#include "dma.h"
-
-/* USER CODE BEGIN 0 */
-
-/* USER CODE END 0 */
-
-/*----------------------------------------------------------------------------*/
-/* Configure DMA                                                              */
-/*----------------------------------------------------------------------------*/
-
-/* USER CODE BEGIN 1 */
-
-/* USER CODE END 1 */
-
-/**
-  * Enable DMA controller clock
-  */
-void MX_DMA_Init(void)
-{
-
-}
-
-/* USER CODE BEGIN 2 */
-
-/* USER CODE END 2 */
diff --git a/src/main.c b/src/main.c
--- a/src/main.c
+++ b/src/main.c
@@ -6,61 +6,106 @@
 
 #include "stm32f3xx_hal.h"
 #include "config.h"
-
 #include "watchdog.h"
 #include "system.h"
+#include "display.h"
+#include "thermostat.h"
 #include "gpio.h"
+#include "tempsense.h"
+#include "pid.h"
 #include "error.h"
 #include "flash.h"
-#include "ssd1306.h"
-#include "dma.h"
-#include "interrupts.h"
-#include "buttons.h"
+#include "ssd1306/ssd1306.h"
+#include "pwmout.h"
+
 
 int main(void)
 {
 	sysclock_init();
 	hal_init();
 	gpio_init();
+
 	ssd1306_init();
-	ssd1306_clearscreen();
+
+	// Startup screen
+    display_startup_screen();
+    HAL_Delay(2000);
+    ssd1306_clearscreen();
 	ssd1306_drawlogo();
-	HAL_Delay(2000);
-	flash_init();
-	watchdog_init();
 
-	//just some example code for getting flash values
-//	flash_getsettings()->values.can_id = 67;
-//
-//	if(flash_getsettings()->values.can_id == 12);
+    // Default status
+	runtime_status()->temp = 0.0;
+	runtime_status()->state_resume = 0;
+	runtime_status()->state = STATE_IDLE;
+	runtime_status()->setpoint = 70;
+	runtime_status()->pid_enabled = 0;
 
-//	ssd1306_drawstring(const char *dataPtr, unsigned char row, unsigned char xPos)
-//	ssd1306_drawstring("[ ProtoFuse ]", 0, 0);
+    pid_init();
+    pwmout_init();
+    flash_init();
+	watchdog_init();
+	tempsense_init();
 
-	float temp_counter = 0;
-
-	// Software timers
-	uint32_t last_screen_update_time = HAL_GetTick();
-
+	// Soft timers
+    uint32_t last_pid = 0;
+    uint32_t last_thermostat = 0;
+    uint32_t last_1hz = 0;
+    uint32_t last_5hz = 0;
 
 	while (1)
 	{
-		// function that checks all the buttons
-		freaking_debounce();
+		float duty = 0.0;
 
-		// Grab and transmit data
-		if(HAL_GetTick() - last_screen_update_time > 100)
+		if(HAL_GetTick() - last_1hz > 750)
 		{
-			HAL_GPIO_TogglePin(LED_GPIO_Port, LED_PIN);
-			char buffer[256];
-			snprintf(buffer, 256, "Counter: %.1f", temp_counter);
-			ssd1306_drawstring(buffer, 3, 45);
-			ssd1306_drawstring("Therm 0.4", 0, 40);
-			temp_counter = temp_counter + 0.1;
-			last_screen_update_time = HAL_GetTick();
+			display_1hz();
+			last_1hz = HAL_GetTick();
+		}
+
+		if(HAL_GetTick() - last_5hz > 200)
+		{
+			tempsense_readtemp();
+//			runtime_status = tempsense_gettemp();
+			last_5hz = HAL_GetTick();
 		}
 
-		watchdog_feed();
+        if(flash_getsettings()->val.control_mode == MODE_PID && (HAL_GetTick() - last_pid > PID_PERIOD))
+        {
+//        	runtime_status()->temp = tempsense_readtemp();
+        	duty = pid_process();
+            last_pid = HAL_GetTick();
+        }
+
+        // Thermostatic control
+        if(flash_getsettings()->val.control_mode == MODE_THERMOSTAT && HAL_GetTick() - last_thermostat > SSR_PERIOD)
+        {
+//        	runtime_status()->temp = tempsense_readtemp();
+        	duty = thermostat_process();
+            last_thermostat = HAL_GetTick();
+        }
+
+        pwmout_process(duty);
+        display_process();
+        watchdog_feed();
+
+
+//        // Transmit temperature over USB-CDC on a regular basis
+//        if(HAL_GetTick() - last_vcp_tx > VCP_TX_FREQ)
+//        {
+//            // Print temp to cdc
+//            char tempstr[16];
+//            itoa_fp(status.temp, status.temp_frac, tempstr);
+//            uint8_t numlen = strlen(tempstr);
+//            tempstr[numlen] = '\r';
+//            tempstr[numlen+1] = '\n';
+//
+//    //        if(set.val.usb_plugged)
+//    //            CDC_Transmit_FS(tempstr, numlen+2);
+//           // while(CDC_Transmit_FS("\r\n", 2) == USBD_BUSY);
+//
+//            last_vcp_tx = HAL_GetTick();
+//        }
+
 	}
 }
 
diff --git a/src/pid.c b/src/pid.c
new file mode 100644
--- /dev/null
+++ b/src/pid.c
@@ -0,0 +1,109 @@
+//
+// PID: proportional/integral/derivative controller
+//
+
+#include "pid.h"
+#include "flash.h"
+
+// PID implementation
+
+static pid_state_t state;
+
+
+void pid_init()
+{
+	state.i_state = 0;
+	state.last_pid_temp = 0;
+	state.last_pid_temp_frac = 0;
+}
+
+float pid_process(void)
+{
+
+
+//            #ifdef MAX31865_RTD_SENSOR
+//            max31865_readtemp(spi_get(), &set, &status);
+//			#else
+//			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
+//			#endif
+
+	float ssr_output = 0;
+
+
+	if(runtime_status()->pid_enabled)
+	{
+		// Get ssr output for next time
+		int16_t power_percent = pid_update();
+
+		if(flash_getsettings()->val.plant_type == PLANT_HEATER)
+			power_percent *= -1;
+
+		//power-percent is 0-1000?
+		ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;
+
+		// put ssr output on display
+		ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
+		char tempstr[6];
+		snprintf(tempstr, 6, "%g", ssr_output);
+		ssd1306_drawstring(tempstr, 0, 90);
+	}
+	else
+	{
+		ssr_output = 0.0;
+	}
+
+	return ssr_output;
+}
+
+int16_t pid_update(void)
+{
+	therm_status_t* status = runtime_status();
+	therm_settings_t* set = flash_getsettings();
+
+	// Convert temperature to fixed point number with 1/10th resolution
+	float temp = status->temp;
+
+	// Calculate instantaneous error
+	int16_t error = status->setpoint * 10 - temp; // TODO: Use fixed point fraction
+
+	// Proportional component
+	int32_t p_term = set->val.k_p * error;
+
+	// Error accumulator (integrator)
+	state.i_state += error;
+
+	// to prevent the iTerm getting huge from lots of
+	//  error, we use a "windup guard"
+	// (this happens when the machine is first turned on and
+	// it cant help be cold despite its best efforts)
+	// not necessary, but this makes windup guard values
+	// relative to the current iGain
+	int32_t windup_guard_res = (set->val.windup_guard * 10) / set->val.k_i;
+
+	// Calculate integral term with windup guard
+	if (state.i_state > windup_guard_res)
+	  state.i_state = windup_guard_res;
+	else if (state.i_state < -windup_guard_res)
+	  state.i_state = -windup_guard_res;
+
+	int32_t i_term = set->val.k_i * state.i_state;
+
+	// Calculate differential term (slope since last iteration)
+	int32_t d_term = (set->val.k_d * (temp - state.last_pid_temp));
+
+	// Save temperature for next iteration
+	state.last_pid_temp = temp;
+
+	int16_t result = (p_term + i_term - d_term) / 10;
+
+	// Put out tenths of percent, 0-1000.
+	if(result > 1000)
+	result = 1000;
+	else if(result < -1000)
+	result = -1000;
+
+	// Return feedback
+	return result;
+}
+
+
diff --git a/src/pwmout.c b/src/pwmout.c
new file mode 100644
--- /dev/null
+++ b/src/pwmout.c
@@ -0,0 +1,63 @@
+//
+// PWM Out: generate PWM waveform to control factory
+//
+
+#include "pwmout.h"
+#include "gpio.h"
+#include "flash.h"
+
+
+static uint32_t last_ssr_on = 0;
+
+
+void pwmout_init(void)
+{
+	GPIO_InitTypeDef GPIO_InitStruct;
+
+	// Configure LED GPIO pins
+	GPIO_InitStruct.Pin = SSR_PIN;
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+	HAL_GPIO_Init(SSR_GPIO_Port, &GPIO_InitStruct);
+
+}
+
+
+// freaking integral term isn't compatible with both heating and cooling... due to the discard
+// functionality. this is a problem.
+
+// also duty cycling isn't working correctly...
+void pwmout_process(float duty)
+{
+
+	uint32_t ssr_output = duty; // meh
+
+	// Kill SSR once the desired on-time has elapsed
+	if(flash_getsettings()->val.control_mode == MODE_PID && ((HAL_GetTick() - last_ssr_on > ssr_output) || ssr_output <= 0))
+	{
+		HAL_GPIO_WritePin(SSR, 0);
+		HAL_GPIO_WritePin(LED, 0);
+	}
+
+
+	// Every 200ms, set the SSR on unless output is 0
+	if(flash_getsettings()->val.control_mode == MODE_PID && HAL_GetTick() - last_ssr_on > SSR_PERIOD)
+	{
+		// Heat or cool, if we need to
+		if(ssr_output > 0)
+		{
+			HAL_GPIO_WritePin(SSR, 1);
+			HAL_GPIO_WritePin(LED, 1);
+			last_ssr_on = HAL_GetTick();
+		}
+		else {
+			// Make sure everything is off
+			HAL_GPIO_WritePin(SSR, 0);
+			HAL_GPIO_WritePin(LED, 0);
+		}
+
+	}
+}
+
+
diff --git a/src/error.c b/src/system/error.c
rename from src/error.c
rename to src/system/error.c
diff --git a/src/flash.c b/src/system/flash.c
rename from src/flash.c
rename to src/system/flash.c
--- a/src/flash.c
+++ b/src/system/flash.c
@@ -8,8 +8,9 @@
 
 // Takes up 1 page (1k size)
 static __attribute__((__section__(".eeprom"))) uint16_t eeprom_emulation[512];
-settings_t settings;
 
+therm_settings_t settings;
+therm_status_t status;
 
 // Initialize flash and restore settings
 void flash_init(void)
@@ -19,7 +20,7 @@ void flash_init(void)
 
 
 // Save settings to flash memory
-void flash_savesettings(void)
+void flash_savesettings()
 {
 	// Unlock flash memory
 	HAL_FLASH_Unlock();
@@ -68,13 +69,23 @@ void flash_restoresettings(void)
 	// No data in flash! Set defaults here
 	else
 	{
-		settings.values.can_id = 22;
+		settings.val.k_p = 10;
+		settings.val.k_i = 0;
+		settings.val.k_d = 0;
+		settings.val.windup_guard = 300;
+		//torestore.values.can_id = 22;
 	}
 }
 
 
 // Accessor to retrieve settings structure
-inline settings_t* flash_getsettings(void)
+inline therm_settings_t* flash_getsettings(void)
 {
 	return &settings;
 }
+
+inline therm_status_t* runtime_status(void)
+{
+	return &status;
+}
+
diff --git a/src/gpio.c b/src/system/gpio.c
rename from src/gpio.c
rename to src/system/gpio.c
--- a/src/gpio.c
+++ b/src/system/gpio.c
@@ -6,6 +6,48 @@
 
 #include "gpio.h"
 
+#define CHANGE_PERIOD_MS 100
+#define CHANGE_ELAPSED (HAL_GetTick() - change_time_reset) > CHANGE_PERIOD_MS
+#define CHANGE_RESET change_time_reset = HAL_GetTick()
+
+// Increase on each press, and increase at a fast rate after duration elapsed of continuously holding down... somehow...
+static uint32_t change_time_reset = 0;
+
+
+// Increment/decrement unsigned variable with up/down buttons
+void user_input(uint16_t* to_modify)
+{
+    if(CHANGE_ELAPSED) {
+        if(!HAL_GPIO_ReadPin(SW_UP) ) {
+            CHANGE_RESET;
+            (*to_modify)++;
+        }
+        else if(!HAL_GPIO_ReadPin(SW_DOWN) && (*to_modify) > 0) {
+            CHANGE_RESET;
+            (*to_modify)--;
+        }
+    }
+}
+
+
+// Increment/decrement signed variable with up/down buttons
+void user_input_signed(int32_t* to_modify)
+{
+	//fixme: need to cast to 16/32 bits correctly
+    if(CHANGE_ELAPSED) {
+        if(!HAL_GPIO_ReadPin(SW_UP) ) {
+            CHANGE_RESET;
+            if (*to_modify < 32768)
+            	(*to_modify)++;
+        }
+        else if(!HAL_GPIO_ReadPin(SW_DOWN)) {
+            CHANGE_RESET;
+            if (*to_modify >= -32768)
+            	(*to_modify)--;
+        }
+    }
+}
+
 
 // Initialize GPIOs
 void gpio_init(void)
diff --git a/src/interrupts.c b/src/system/interrupts.c
rename from src/interrupts.c
rename to src/system/interrupts.c
diff --git a/src/sbrk.c b/src/system/sbrk.c
rename from src/sbrk.c
rename to src/system/sbrk.c
diff --git a/src/system/stringhelpers.c b/src/system/stringhelpers.c
new file mode 100644
--- /dev/null
+++ b/src/system/stringhelpers.c
@@ -0,0 +1,72 @@
+#include <inttypes.h>
+
+char const digit[] = "0123456789";
+
+char* zitoa(int16_t i, char b[]){
+
+    char* p = b;
+    if(i<0){
+        *p++ = '-';
+        i *= -1;
+    }
+    uint16_t shifter = i;
+    do{ //Move to where representation ends
+        ++p;
+        shifter = shifter/10;
+    }while(shifter);
+    *p = '\0';
+    do{ //Move back, inserting digits as you go
+        *--p = digit[i%10];
+        i = i/10;
+    }while(i);
+    return b;
+}
+
+char* itoa_fp(int16_t i, uint8_t frac, char b[]){
+
+    // set p to beginning of char array
+    char* p = b;
+
+    // If negative, set current char to '-' and inc, unnegate number
+    if(i<0){
+        *p++ = '-';
+        i *= -1;
+    }
+
+    // Init shifter to numeric value
+    uint16_t shifter = i;
+    uint16_t frac_shifter = frac;
+
+    // Iterate through 10s places, incrementing text pointer as we go
+    do{ 
+        ++p;
+        shifter = shifter/10;
+    }while(shifter);
+    
+    ++p; // increment for decimal point
+
+    do{
+        ++p;
+        frac_shifter = frac_shifter/10;
+    }while(frac_shifter);
+        
+
+    // Null-terminate the string
+    *p = '\0';
+
+    // Go backwards and write out fractional digits 
+    do{ 
+        *--p = digit[frac%10];
+        frac = frac/10;
+    }while(frac);
+
+    *--p = '.'; // insert decimal point
+
+    // Go backwards and write out remaining digits 
+    do{ 
+        *--p = digit[i%10];
+        i = i/10;
+    }while(i);
+    return b;
+}
+
diff --git a/src/system.c b/src/system/system.c
rename from src/system.c
rename to src/system/system.c
diff --git a/src/watchdog.c b/src/system/watchdog.c
rename from src/watchdog.c
rename to src/system/watchdog.c
diff --git a/src/tempsense.c b/src/tempsense.c
new file mode 100644
--- /dev/null
+++ b/src/tempsense.c
@@ -0,0 +1,53 @@
+//
+// TempSense: read temperature from TC, RTD, or NTC
+//
+
+#include "tempsense.h"
+#include "flash.h"
+#include "ssd1306/ssd1306.h"
+#include "max31856/max31856.h"
+
+
+void tempsense_init(void)
+{
+	// TODO: Rework SPI stuff... init the port in a sharedlib then pass ref to display and tempsense
+
+	max31856_init(spi_get(), TEMPSENSE_MAX_CS_PORT, TEMPSENSE_MAX_CS_PIN, 0);
+
+	// Maybe don't perform temp sensor setup in here, but in readtemp?
+	// what happens if the user changes the tempsense type while running?
+	// we need to re-init...
+
+
+}
+
+// Returns the latest reading from the configured temperature sensor
+float tempsense_readtemp(void)
+{
+
+	switch(flash_getsettings()->val.sensor_type)
+	{
+		case SENSOR_TC_K:
+		case SENSOR_TC_E:
+		case SENSOR_TC_N:
+		case SENSOR_TC_R:
+		case SENSOR_TC_S:
+		case SENSOR_TC_T:
+		{
+			// Read MAX31856
+		} break;
+
+		case SENSOR_NTC:
+		{
+			// Read analog value from internal ADC, linearize the reading, etc
+		} break;
+
+	}
+
+	// either return latest reading from DMA loop (NTC, etc)
+	// or initiate a blocking read and return it.
+	// Need to gracefully handle the timeout...
+}
+
+
+
diff --git a/src/thermostat.c b/src/thermostat.c
new file mode 100644
--- /dev/null
+++ b/src/thermostat.c
@@ -0,0 +1,62 @@
+//
+// Thermostat: Thermostatic controller
+//
+
+#include "thermostat.h"
+#include "flash.h"
+#include "gpio.h"
+
+// PID implementation
+
+float thermostat_process(void)
+{
+
+//            #ifdef MAX31865_RTD_SENSOR
+//            max31865_readtemp(spi_get(), &set, &status);
+//			#else
+//			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
+//			#endif
+
+			float ssr_output = 0.0;
+			uint8_t thermostat_plant_on = 0;
+
+			// TODO: Migrate this FxP conversion to the readtemp code or similar
+			float temp = runtime_status()->temp;
+
+
+			// EMZ FIXME: This could be way simpler
+			if(flash_getsettings()->val.plant_type == PLANT_HEATER && runtime_status()->setpoint * 10 < temp - flash_getsettings()->val.hysteresis * 10)
+				thermostat_plant_on = 1;
+			else if(flash_getsettings()->val.plant_type == PLANT_HEATER && runtime_status()->setpoint * 10 > temp + flash_getsettings()->val.hysteresis * 10)
+				thermostat_plant_on = 0;
+
+			if(flash_getsettings()->val.plant_type == PLANT_COOLER && runtime_status()->setpoint * 10 > temp + flash_getsettings()->val.hysteresis * 10)
+				thermostat_plant_on = 1;
+			else if(flash_getsettings()->val.plant_type == PLANT_COOLER && runtime_status()->setpoint * 10 < temp - flash_getsettings()->val.hysteresis * 10)
+				thermostat_plant_on = 0;
+
+			// EMZ: TODO: Refactor to output_enabled or something
+			if(runtime_status()->pid_enabled && thermostat_plant_on)
+			{
+				// EMZ TODO: functionalize this
+				// put ssr output on display
+				ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
+				char tempstr[6];
+				itoa(ssr_output, tempstr, 10);
+				ssd1306_drawstring(tempstr, 0, 90);
+
+
+
+//                HAL_GPIO_WritePin(SSR_PIN, 1);
+				HAL_GPIO_WritePin(LED, 1);
+			}
+			else
+			{
+//                HAL_GPIO_WritePin(SSR_PIN, 0);
+				HAL_GPIO_WritePin(LED, 0);
+			}
+
+			return ssr_output;
+}
+
+