diff --git a/motor-control/firmware/TODOs.md b/motor-control/firmware/TODOs.md
new file mode 100644
index 0000000..e69de29
diff --git a/motor-control/firmware/cmake/tmc2209.cmake b/motor-control/firmware/cmake/tmc2209.cmake
index ea22f65..f15d80e 100644
--- a/motor-control/firmware/cmake/tmc2209.cmake
+++ b/motor-control/firmware/cmake/tmc2209.cmake
@@ -1,5 +1,6 @@
 add_library(tmc2209_driver STATIC
     src/tmc2209/uart_interface.cpp
+    src/tmc2209/uart_registers.cpp
 )
 
 target_include_directories(tmc2209_driver PRIVATE
diff --git a/motor-control/firmware/include/tmc2209/registers_map.hpp b/motor-control/firmware/include/tmc2209/registers_map.hpp
index 2c9b8b6..5a25194 100644
--- a/motor-control/firmware/include/tmc2209/registers_map.hpp
+++ b/motor-control/firmware/include/tmc2209/registers_map.hpp
@@ -382,10 +382,93 @@ struct REG_MSCURACT {
 
 struct REG_CHOPCONF {
     /**
-     * (Reset default: OTP)
-     * Chopper control register
+     * @brief off time and driver enable
+     * Off time setting controls duration of slow decay phase 
+     * NCLK= 24 + 32*TOFF 
+     * %0000: Driver disable, all bridges off 
+     * %0001: 1 – use only with TBL ≥ 2 
+     * %0010 ... %1111: 2 ... 15 
+     * (Default: OTP, resp. 3 in StealthChop mode)
      */
-    unsigned int chopconf: 32;
+    unsigned int toff: 4;
+
+    /**
+     * @brief hysteresis start value added to HEND
+     * %000 ... %111:
+     * Add 1, 2, ..., 8 to hysteresis low value HEND
+     * (1/512 of this setting adds to current setting)
+     * Attention: Effective HEND+HSTRT ≤ 16.
+     * Hint: Hysteresis decrement is done each 16 clocks
+     */
+    unsigned int hstrt: 3;
+
+    /**
+     * @brief HEND hysteresis low value OFFSET sine wave offset
+     * %0000 ... %1111:
+     * Hysteresis is -3, -2, -1, 0, 1, ..., 12
+     * (1/512 of this setting adds to current setting)
+     * This is the hysteresis value which becomes used for the hysteresis chopper.
+     * (Default: OTP, resp. 0 in StealthChop mode)
+     */
+    unsigned int hend: 4;
+
+    unsigned int :1;
+
+    /**
+     * @brief TBL blank time select
+     * %00 ... %11:
+     * Set comparator blank time to 16, 24, 32 or 40 clocks
+     * Hint: %00 or %01 is recommended for most applications
+     * (Default: OTP)
+     */
+    unsigned int tbl: 2;
+
+    /**
+     * @brief sense resistor voltage based current scaling
+     * 0: Low sensitivity, high sense resistor voltage
+     * 1: High sensitivity, low sense resistor voltage
+     */
+    unsigned int vsense: 1;
+
+    unsigned int :6;
+
+    /**
+     * @brief micro step resolution
+     * %0000: Native 256 microstep setting.
+     * %0001 ... %1000:
+     * 128, 64, 32, 16, 8, 4, 2, FULLSTEP
+     * Reduced microstep resolution.
+     * The resolution gives the number of microstep entries per sine quarter wave.
+     * When choosing a lower microstep resolution, the driver automatically uses microstep positions which result in a symmetrical wave.
+     * Number of microsteps per step pulse = 2^MRES (Selection by pins unless disabled by GCONF. mstep_reg_select)
+     */
+    unsigned int mres: 4;
+
+    /**
+     * @brief interpolation to 256 microsteps
+     * 1: The actual microstep resolution (MRES) becomes extrapolated to 256 microsteps for smoothest motor operation. (Default: 1)
+     */
+    unsigned int intpol: 1;
+
+    /**
+     * @brief enable double edge step pulses
+     * 1: Enable step impulse at each step edge to reduce step frequency requirement. This mode is not compatible with the step filtering function (multistep_filt)
+     */
+    unsigned int dedge: 1;
+
+    /**
+     * @brief short to GND protection disable
+     * 0: Short to GND protection is on 
+     * 1: Short to GND protection is disabled
+     */
+    unsigned int diss2g: 1;
+
+    /**
+     * @brief Low side short protection disable
+     * 0: Short protection low side is on 
+     * 1: Short protection low side is disabled
+     */
+    unsigned int diss2vs: 1;
 };
 
 struct DRV_STATUS {
diff --git a/motor-control/firmware/include/tmc2209/tmc2209.hpp b/motor-control/firmware/include/tmc2209/tmc2209.hpp
index 221b66c..341ca4f 100644
--- a/motor-control/firmware/include/tmc2209/tmc2209.hpp
+++ b/motor-control/firmware/include/tmc2209/tmc2209.hpp
@@ -73,14 +73,53 @@ public:
     bool crc_error_flag = false;
 };
 
+/**
+ * @brief Higher level interface to TMC2209 driver
+ * This class simplifies dealing with the registers of the TMC2209 driver
+ * so you can just set the values
+ */
 class TMC2209_Registers {
 
 private:
     TMC2209_UART uart_driver;
+
+    REG_CHOPCONF chopconfBuffer;
+
 public:
     TMC2209_Registers(TMC2209_UART uart_driver);
 
-    void read_all_state();
+    void set_reg_gconf(
+        bool i_scale_analog,
+        bool internal_rsense,
+        bool en_SpreadCycle,
+        bool shaft,
+        bool index_otpw,
+        bool index_step,
+        bool pdn_disable,
+        bool mstep_reg_select,
+        bool multistep_filt
+    );
 
+    void set_holdCurrent(
+        uint8_t currentHold,
+        uint8_t currentRun,
+        uint8_t holdDelay
+    );
+
+    void set_thresholdPowerdown(uint8_t threshold);
+    void set_thresholdStealthChop(uint32_t threshold);
+    void set_thresholdCoolStep(uint32_t threshold);
+    void set_thresholdStall(uint8_t threshold);
+
+    void set_chopConfig(
+        bool lowsideShortProtection,
+        bool GNDShortProtection,
+        bool doubleEdge,
+        bool interpolation,
+        uint8_t microstepResolution,
+        bool voltageSensatifity,
+        uint8_t hysteresisOffset,
+        uint8_t hysteresisStart
+    );
 
 };
diff --git a/motor-control/firmware/src/tmc2209/uart_registers.cpp b/motor-control/firmware/src/tmc2209/uart_registers.cpp
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/motor-control/firmware/src/tmc2209/uart_registers.cpp
@@ -0,0 +1 @@
+