diff --git a/Core/Inc/ts_state_machine.h b/Core/Inc/ts_state_machine.h
index 01eefc6..8ac4e57 100644
--- a/Core/Inc/ts_state_machine.h
+++ b/Core/Inc/ts_state_machine.h
@@ -9,6 +9,8 @@
 #define MIN_VEHICLE_SIDE_VOLTAGE 150000 // mV
 // Time to wait after reaching 95% of battery voltage before exiting precharge
 #define PRECHARGE_95_DURATION 500 // ms
+// Time to wait for discharge
+#define DISCHARGE_DURATION 5000 // ms
 // Time to wait after there is no more error condition before exiting TS_ERROR
 #define NO_ERROR_TIME 1000 // ms
 // Time to wait for charger voltage before going to TS_ERROR
diff --git a/Core/Src/ts_state_machine.c b/Core/Src/ts_state_machine.c
index 8c8d429..c3fc778 100644
--- a/Core/Src/ts_state_machine.c
+++ b/Core/Src/ts_state_machine.c
@@ -10,6 +10,7 @@ TSStateHandle ts_state;
 
 static uint32_t precharge_95_reached_timestamp = 0;
 static uint32_t charging_check_timestamp = 0;
+static uint32_t discharge_begin_timestamp = 0;
 
 void ts_sm_init() {
   ts_state.current_state = TS_INACTIVE;
@@ -63,6 +64,7 @@ TSState ts_sm_update_inactive() {
 
 TSState ts_sm_update_active() {
   if (ts_state.target_state == TS_INACTIVE) {
+    discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
 
@@ -71,6 +73,7 @@ TSState ts_sm_update_active() {
 
 TSState ts_sm_update_precharge() {
   if (ts_state.target_state == TS_INACTIVE) {
+    discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
   if (shunt_data.voltage2 > MIN_VEHICLE_SIDE_VOLTAGE &&
@@ -88,8 +91,11 @@ TSState ts_sm_update_precharge() {
 }
 
 TSState ts_sm_update_discharge() {
-  // TODO: Actually wait for discharge
-  return TS_INACTIVE;
+  if (HAL_GetTick() - discharge_begin_timestamp >= DISCHARGE_DURATION) {
+    return TS_INACTIVE;
+  } else {
+    return TS_DISCHARGE;
+  }
 }
 
 TSState ts_sm_update_error() {
@@ -109,6 +115,7 @@ TSState ts_sm_update_error() {
 
 TSState ts_sm_update_charging_check() {
   if (ts_state.target_state == TS_INACTIVE) {
+    discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
 
@@ -124,6 +131,7 @@ TSState ts_sm_update_charging_check() {
 
 TSState ts_sm_update_charging() {
   if (ts_state.target_state == TS_INACTIVE) {
+    discharge_begin_timestamp = HAL_GetTick();
     return TS_DISCHARGE;
   }
   if (shunt_data.current < 0) {
@@ -136,6 +144,7 @@ TSState ts_sm_update_charging() {
 void ts_sm_set_relay_positions(TSState state) {
   switch (state) {
   case TS_INACTIVE:
+  case TS_DISCHARGE:
   case TS_ERROR:
     ts_sm_set_relay_position(RELAY_NEG, 0);
     ts_sm_set_relay_position(RELAY_POS, 0);
@@ -143,7 +152,6 @@ void ts_sm_set_relay_positions(TSState state) {
     break;
   case TS_ACTIVE:
   case TS_CHARGING:
-  case TS_DISCHARGE:
     ts_sm_set_relay_position(RELAY_NEG, 1);
     ts_sm_set_relay_position(RELAY_POS, 1);
     ts_sm_set_relay_position(RELAY_PRECHARGE, 1);