diff --git a/libraries/master/src/master_task.c b/libraries/master/src/master_task.c
index cc3caf1bf..51bf563f9 100644
--- a/libraries/master/src/master_task.c
+++ b/libraries/master/src/master_task.c
@@ -47,9 +47,9 @@ void check_late_cycle(BaseType_t delay) {
 }
 
 TASK(master_task, TASK_STACK_512) {
-  TickType_t xLastWakeTime = xTaskGetTickCount();
   uint32_t counter = 1;
   pre_loop_init();
+  TickType_t xLastWakeTime = xTaskGetTickCount();
   while (true) {
     // LOG_DEBUG("counter: %u\n", counter);
 
diff --git a/libraries/ms-common/inc/soft_timer.h b/libraries/ms-common/inc/soft_timer.h
index ab10af7f7..14fdbb3fd 100644
--- a/libraries/ms-common/inc/soft_timer.h
+++ b/libraries/ms-common/inc/soft_timer.h
@@ -25,7 +25,15 @@ typedef void (*SoftTimerCallback)(SoftTimerId id);
 // Adds a software timer. The provided duration is the number of
 // miliseconds before running and the callback is the process to run once
 // the time has expired.
-StatusCode soft_timer_start(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer);
+StatusCode soft_timer_init_and_start(uint32_t duration_ms, SoftTimerCallback callback,
+                                     SoftTimer *timer);
+
+// Creates a new software timer with given params (same as soft_timer_init_and_start) but doesn't
+// start the timer
+StatusCode soft_timer_init(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer);
+
+// Starts the software timer. The timer must already be initialized
+StatusCode soft_timer_start(SoftTimer *timer);
 
 // Cancels the soft timer specified by name. Returns true if successful.
 // the timer is not cancelled immediately,
diff --git a/libraries/ms-common/inc/watchdog.h b/libraries/ms-common/inc/watchdog.h
index d0d311b20..bf8432486 100644
--- a/libraries/ms-common/inc/watchdog.h
+++ b/libraries/ms-common/inc/watchdog.h
@@ -7,7 +7,7 @@ typedef SoftTimer WatchDog;
 
 // start watchdog
 #define watchdog_start(watchdog, timeout_ms, callback) \
-  soft_timer_start(timeout_ms, callback, watchdog)
+  soft_timer_init_and_start(timeout_ms, callback, watchdog)
 
 // :)
 #define watchdog_kick(watchdog) soft_timer_reset(watchdog)
diff --git a/libraries/ms-common/src/persist.c b/libraries/ms-common/src/persist.c
index 0fa0fb558..6cc8bd733 100644
--- a/libraries/ms-common/src/persist.c
+++ b/libraries/ms-common/src/persist.c
@@ -46,7 +46,7 @@ static void prv_periodic_commit(SoftTimerId timer_id) {
   }
 
   s_context = persist;
-  soft_timer_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
+  soft_timer_init_and_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
 }
 
 StatusCode persist_init(PersistStorage *persist, FlashPage page, void *blob, size_t blob_size,
@@ -120,14 +120,15 @@ StatusCode persist_init(PersistStorage *persist, FlashPage page, void *blob, siz
     persist->flash_addr += sizeof(header) + header.size_bytes;
   }
   s_context = persist;
-  return soft_timer_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
+  return soft_timer_init_and_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
 }
 
 StatusCode persist_ctrl_periodic(PersistStorage *persist, bool enabled) {
   if (id == soft_timer_invalid_timer && enabled) {
     // Enable periodic commit - previously disabled
     s_context = persist;
-    return soft_timer_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
+    return soft_timer_init_and_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit,
+                                     &persist->timer);
   } else if (id != soft_timer_invalid_timer && !enabled) {
     // Disable periodic commit - previously enabled
     soft_timer_cancel(&persist->timer);
diff --git a/libraries/ms-common/src/soft_timer.c b/libraries/ms-common/src/soft_timer.c
index d899aec24..3f8549b2b 100644
--- a/libraries/ms-common/src/soft_timer.c
+++ b/libraries/ms-common/src/soft_timer.c
@@ -1,12 +1,25 @@
 #include "soft_timer.h"
 
-StatusCode soft_timer_start(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer) {
+StatusCode soft_timer_init_and_start(uint32_t duration_ms, SoftTimerCallback callback,
+                                     SoftTimer *timer) {
+  status_ok_or_return(soft_timer_init(duration_ms, callback, timer));
+  return soft_timer_start(timer);
+}
+
+StatusCode soft_timer_init(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer) {
   if (timer->id != NULL) {
     // timer already exist/inuse, delete the old timer
     xTimerDelete(timer->id, 0);
   }
   timer->id = xTimerCreateStatic(NULL, pdMS_TO_TICKS(duration_ms), pdFALSE,  //
                                  NULL, callback, &timer->buffer);
+  return STATUS_CODE_OK;
+}
+
+StatusCode soft_timer_start(SoftTimer *timer) {
+  if (timer->id == NULL) {
+    return STATUS_CODE_UNINITIALIZED;
+  }
   if (xTimerStart(timer->id, 0) != pdPASS) {
     return status_msg(STATUS_CODE_INTERNAL_ERROR, "timer command queue is full");
   }
diff --git a/libraries/ms-common/test/test_soft_timer.c b/libraries/ms-common/test/test_soft_timer.c
index a5306c889..4a9881d00 100644
--- a/libraries/ms-common/test/test_soft_timer.c
+++ b/libraries/ms-common/test/test_soft_timer.c
@@ -31,7 +31,7 @@ void test_soft_timer() {
   for (int i = 0; i < 3; ++i) {
     triggered = false;
     // test soft timer start and cancel
-    soft_timer_start(50, prv_set, &s_timer);
+    soft_timer_init_and_start(50, prv_set, &s_timer);
 
     xTaskDelayUntil(&last_wake, 49);
 
@@ -47,7 +47,7 @@ void test_soft_timer() {
     TEST_ASSERT_FALSE(soft_timer_inuse(&s_timer));
 
     // test start to finish
-    soft_timer_start(200, prv_set, &s_timer);
+    soft_timer_init_and_start(200, prv_set, &s_timer);
 
     xTaskDelayUntil(&last_wake, 99);
 
@@ -70,8 +70,8 @@ void test_multiple_timer() {
   for (int i = 0; i < 3; ++i) {
     triggered = false;
     // test soft timer start and cancel
-    soft_timer_start(100, prv_set, &s_timer);
-    soft_timer_start(50, prv_set, &s_timer_2);
+    soft_timer_init_and_start(100, prv_set, &s_timer);
+    soft_timer_init_and_start(50, prv_set, &s_timer_2);
 
     delay_ms(51);
 
diff --git a/libraries/ms-drivers/inc/pca9555_gpio_expander.h b/libraries/ms-drivers/inc/pca9555_gpio_expander.h
index f6b2c2ec4..2fd3d0190 100644
--- a/libraries/ms-drivers/inc/pca9555_gpio_expander.h
+++ b/libraries/ms-drivers/inc/pca9555_gpio_expander.h
@@ -54,7 +54,7 @@ typedef struct {
 } Pca9555GpioSettings;
 
 // Initialize PCA9555 GPIO at this I2C port and address.
-StatusCode pca9555_gpio_init(const I2CPort i2c_port, const I2CAddress i2c_address);
+StatusCode pca9555_gpio_init(const I2CPort i2c_port);
 
 // Initialize an PCA9555 GPIO pin by address.
 StatusCode pca9555_gpio_init_pin(const Pca9555GpioAddress *address,
diff --git a/libraries/ms-drivers/src/ads1015.c b/libraries/ms-drivers/src/ads1015.c
index 85a5a7836..769afdd57 100644
--- a/libraries/ms-drivers/src/ads1015.c
+++ b/libraries/ms-drivers/src/ads1015.c
@@ -66,7 +66,7 @@ static void prv_timer_callback(SoftTimerId id) {
   current_channel = (Ads1015Channel)(__builtin_ffs(storage->pending_channel_bitset) - 1);
   // Update so that the ADS1015 reads from the next channel.
   prv_set_channel(storage, current_channel);
-  soft_timer_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
+  soft_timer_init_and_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
 }
 
 // Inits the storage for ADS1015 and starts the soft timer.
@@ -80,7 +80,7 @@ StatusCode ads1015_init(Ads1015Storage *storage, I2CPort i2c_port, Ads1015Addres
     storage->channel_readings[channel] = ADS1015_DISABLED_CHANNEL_READING;
   }
   s_context = storage;
-  return soft_timer_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
+  return soft_timer_init_and_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
 }
 
 // Enable/disables a channel, and sets a callback for the channel.
diff --git a/libraries/ms-drivers/src/pca9555_gpio_expander.c b/libraries/ms-drivers/src/pca9555_gpio_expander.c
index c63f41446..e72a07941 100644
--- a/libraries/ms-drivers/src/pca9555_gpio_expander.c
+++ b/libraries/ms-drivers/src/pca9555_gpio_expander.c
@@ -20,7 +20,7 @@ static uint8_t prv_pin_bit(const Pca9555PinAddress pin) {
   return prv_is_port_0(pin) ? pin : pin - PCA9555_PIN_IO1_0;
 }
 
-StatusCode pca9555_gpio_init(const I2CPort i2c_port, const I2CAddress i2c_address) {
+StatusCode pca9555_gpio_init(const I2CPort i2c_port) {
   if (s_i2c_port == NUM_I2C_PORTS) {
     s_i2c_port = i2c_port;
   }
diff --git a/libraries/ms-drivers/test/test_pca9555_gpio_expander.c b/libraries/ms-drivers/test/test_pca9555_gpio_expander.c
index 500c2b807..f2cafc53b 100644
--- a/libraries/ms-drivers/test/test_pca9555_gpio_expander.c
+++ b/libraries/ms-drivers/test/test_pca9555_gpio_expander.c
@@ -30,7 +30,7 @@ void setup_test(void) {
   gpio_it_init();
 
   i2c_init(i2c_port, &i2c_settings);
-  pca9555_gpio_init(i2c_port, MOCK_I2C_ADDRESS);
+  pca9555_gpio_init(i2c_port);
 }
 
 void teardown_test(void) {}
diff --git a/projects/bms_carrier/src/current_sense.c b/projects/bms_carrier/src/current_sense.c
index bb6c84a43..d031fb063 100644
--- a/projects/bms_carrier/src/current_sense.c
+++ b/projects/bms_carrier/src/current_sense.c
@@ -45,7 +45,7 @@ static void prv_periodic_ads_read(SoftTimerId id) {
   s_current_storage->ring_idx = (s_current_storage->ring_idx + 1) % NUM_STORED_CURRENT_READINGS;
   ads1259_get_conversion_data(&s_ads1259_storage);
   // Kick new soft timer
-  soft_timer_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
+  soft_timer_init_and_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
 
   // update average
   int32_t sum = 0;
@@ -92,6 +92,6 @@ StatusCode current_sense_init(CurrentStorage *storage, SpiSettings *settings,
   s_current_storage->conv_period_ms = conv_period_ms;
   status_ok_or_return(ads1259_init(&s_ads1259_storage, &ads_settings));
   ads1259_get_conversion_data(&s_ads1259_storage);
-  soft_timer_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
+  soft_timer_init_and_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
   return STATUS_CODE_OK;
 }
diff --git a/projects/centre_console/src/drive_fsm.c b/projects/centre_console/src/drive_fsm.c
index 04c5c0b3d..74007ef6b 100644
--- a/projects/centre_console/src/drive_fsm.c
+++ b/projects/centre_console/src/drive_fsm.c
@@ -236,7 +236,7 @@ StatusCode init_drive_fsm(void) {
   // Add gpio register interrupts
   Pca9555GpioSettings pca_settings = { .direction = PCA9555_GPIO_DIR_OUT,
                                        .state = PCA9555_GPIO_STATE_LOW };
-  pca9555_gpio_init(I2C_PORT_1, 0x20);
+  pca9555_gpio_init(I2C_PORT_1);
   for (int i = 0; i < NUM_DRIVE_FSM_BUTTONS; i++) {
     status_ok_or_return(pca9555_gpio_init_pin(&s_drive_btn_leds[i], &pca_settings));
   }
diff --git a/projects/power_distribution/inc/lights_fsm.h b/projects/power_distribution/inc/lights_fsm.h
index e228da9b3..aa59080a3 100644
--- a/projects/power_distribution/inc/lights_fsm.h
+++ b/projects/power_distribution/inc/lights_fsm.h
@@ -10,7 +10,6 @@
 #include "task.h"
 
 #define NUM_LIGHTS_STATES 4
-#define NUM_LIGHTS_TRANSITIONS 10
 #define SIGNAL_BLINK_PERIOD_MS 600  // Signal blink frequency of 1.66Hz
 DECLARE_FSM(lights);
 
@@ -33,7 +32,7 @@ typedef enum EELightType {
 } EELightType;
 
 typedef enum HazardStatus {
-  HAZARD_OFF = NUM_EE_LIGHT_TYPES + 1,
+  HAZARD_OFF = 0,
   HAZARD_ON,
 } HazardStatus;
 
diff --git a/projects/power_distribution/src/lights_fsm.c b/projects/power_distribution/src/lights_fsm.c
index f0d42cdfb..103efc677 100644
--- a/projects/power_distribution/src/lights_fsm.c
+++ b/projects/power_distribution/src/lights_fsm.c
@@ -1,44 +1,63 @@
 #include "lights_fsm.h"
 
+#include "log.h"
+#include "outputs.h"
 #include "power_distribution_getters.h"
 
 // Placeholder GPIO Address, will be updated
-GpioAddress RIGHT_LIGHT_ADDR = { .port = GPIO_PORT_A, .pin = 11 };
-GpioAddress LEFT_LIGHT_ADDR = { .port = GPIO_PORT_A, .pin = 12 };
+GpioAddress RIGHT_LIGHT_ADDR = { .port = GPIO_PORT_B, .pin = 5 };
+GpioAddress LEFT_LIGHT_ADDR = { .port = GPIO_PORT_A, .pin = 15 };
 
 // Softtimer module setup for light blinkers
 static SoftTimer s_timer_single;
 static EELightType light_id_callback;
 
 FSM(lights, NUM_LIGHTS_STATES);
+static OutputState left_signal_state = OUTPUT_STATE_OFF;
+static OutputState right_signal_state = OUTPUT_STATE_OFF;
+static LightsStateId fsm_prev_state = INIT_STATE;
 
 static void prv_lights_signal_blinker(SoftTimerId id) {
-  if (light_id_callback == EE_LIGHT_TYPE_SIGNAL_LEFT) {
-    gpio_toggle_state(&LEFT_LIGHT_ADDR);
-    soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
-
-  } else if (light_id_callback == EE_LIGHT_TYPE_SIGNAL_RIGHT) {
-    gpio_toggle_state(&RIGHT_LIGHT_ADDR);
-    soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
-
-  } else if (light_id_callback == EE_LIGHT_TYPE_SIGNAL_HAZARD) {
-    GpioState sync_state = gpio_get_state(&LEFT_LIGHT_ADDR, &sync_state);
-    gpio_set_state(&RIGHT_LIGHT_ADDR, sync_state);
-    gpio_toggle_state(&LEFT_LIGHT_ADDR);
-    gpio_toggle_state(&RIGHT_LIGHT_ADDR);
-    soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
-  } else {
-    // in the initial state light_id_callback == NUM_EE_LIGHT_TYPES
-    gpio_set_state(&RIGHT_LIGHT_ADDR, GPIO_STATE_LOW);
-    gpio_set_state(&LEFT_LIGHT_ADDR, GPIO_STATE_LOW);
-    return;
+  switch (light_id_callback) {
+    case EE_LIGHT_TYPE_SIGNAL_LEFT:
+      left_signal_state ^= 1;
+      pd_set_output_group(OUTPUT_GROUP_LEFT_TURN, left_signal_state);
+      if (right_signal_state == OUTPUT_STATE_ON) {
+        right_signal_state = OUTPUT_STATE_OFF;
+        pd_set_output_group(OUTPUT_GROUP_RIGHT_TURN, right_signal_state);
+      }
+      soft_timer_start(&s_timer_single);
+      break;
+    case EE_LIGHT_TYPE_SIGNAL_RIGHT:
+      right_signal_state ^= 1;
+      pd_set_output_group(OUTPUT_GROUP_RIGHT_TURN, right_signal_state);
+      if (left_signal_state == OUTPUT_STATE_ON) {
+        left_signal_state = OUTPUT_STATE_OFF;
+        pd_set_output_group(OUTPUT_GROUP_LEFT_TURN, left_signal_state);
+      }
+      soft_timer_start(&s_timer_single);
+      break;
+    case EE_LIGHT_TYPE_SIGNAL_HAZARD:
+      if (left_signal_state != right_signal_state) {
+        left_signal_state = OUTPUT_STATE_OFF;
+        right_signal_state = OUTPUT_STATE_OFF;
+      }
+      left_signal_state ^= 1;
+      right_signal_state ^= 1;
+      pd_set_output_group(OUTPUT_GROUP_HAZARD, left_signal_state);
+      soft_timer_start(&s_timer_single);
+      break;
+    default:
+      left_signal_state = OUTPUT_STATE_OFF;
+      right_signal_state = OUTPUT_STATE_OFF;
+      pd_set_output_group(OUTPUT_GROUP_HAZARD, left_signal_state);
   }
 }
 
 static void prv_init_state_input(Fsm *fsm, void *context) {
   // can transition to LEFT, RIGHT, HAZARD
-  EELightType light_event = get_steering_info_input_lights();
-  HazardStatus hazard_status = get_power_info_hazard_state();
+  EELightType light_event = get_steering_info_input_lights();  // can msg id = 682 = 0x2AA
+  HazardStatus hazard_status = get_power_info_hazard_state();  // can msg id = 1026 = 0x5E2
 
   if (hazard_status == HAZARD_ON) {
     fsm_transition(fsm, HAZARD);
@@ -52,6 +71,7 @@ static void prv_init_state_input(Fsm *fsm, void *context) {
 static void prv_init_state_output(void *context) {
   LOG_DEBUG("Transitioned to INIT_STATE\n");
   light_id_callback = NUM_EE_LIGHT_TYPES;
+  fsm_prev_state = INIT_STATE;
 }
 
 static void prv_left_signal_input(Fsm *fsm, void *context) {
@@ -72,7 +92,10 @@ static void prv_left_signal_output(void *context) {
   LOG_DEBUG("Transitioned to LEFT_SIGNAL\n");
   // Toggle Left Signal blinkers at 100 BPM -> 0.6s
   light_id_callback = EE_LIGHT_TYPE_SIGNAL_LEFT;
-  soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
+  if (fsm_prev_state == INIT_STATE) {
+    soft_timer_start(&s_timer_single);
+  }
+  fsm_prev_state = LEFT_SIGNAL;
 }
 
 static void prv_right_signal_input(Fsm *fsm, void *context) {
@@ -93,14 +116,25 @@ static void prv_right_signal_output(void *context) {
   LOG_DEBUG("Transitioned to RIGHT_SIGNAL\n");
   // Toggle Right Signal blinkers at 100 BPM -> 0.6 s
   light_id_callback = EE_LIGHT_TYPE_SIGNAL_RIGHT;
-  soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
+  if (fsm_prev_state == INIT_STATE) {
+    soft_timer_start(&s_timer_single);
+  }
+  fsm_prev_state = RIGHT_SIGNAL;
 }
 
 static void prv_hazard_input(Fsm *fsm, void *context) {
   // can transition to INIT, BPS_FAULT
   EELightType light_event = get_steering_info_input_lights();
+  HazardStatus hazard_status = get_power_info_hazard_state();
 
-  if (light_event == EE_LIGHT_TYPE_OFF) {
+  if (hazard_status == HAZARD_ON) {
+    return;
+  }
+  if (light_event == EE_LIGHT_TYPE_SIGNAL_LEFT) {
+    fsm_transition(fsm, LEFT_SIGNAL);
+  } else if (light_event == EE_LIGHT_TYPE_SIGNAL_RIGHT) {
+    fsm_transition(fsm, RIGHT_SIGNAL);
+  } else {
     fsm_transition(fsm, INIT_STATE);
   }
 }
@@ -109,7 +143,10 @@ static void prv_hazard_output(void *context) {
   LOG_DEBUG("Transitioned to HAZARD\n");
   // Toggle Left and Right Signal blinkers at 100 BPM -> 0.6s
   light_id_callback = EE_LIGHT_TYPE_SIGNAL_HAZARD;
-  soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
+  if (fsm_prev_state == INIT_STATE) {
+    soft_timer_start(&s_timer_single);
+  }
+  fsm_prev_state = HAZARD;
 }
 
 // Lights FSM declaration for states and transitions
@@ -125,13 +162,12 @@ static bool s_PD_transition_list[NUM_LIGHTS_STATES][NUM_LIGHTS_STATES] = {
   TRANSITION(INIT_STATE, HAZARD),        TRANSITION(LEFT_SIGNAL, INIT_STATE),
   TRANSITION(LEFT_SIGNAL, HAZARD),       TRANSITION(LEFT_SIGNAL, RIGHT_SIGNAL),
   TRANSITION(RIGHT_SIGNAL, INIT_STATE),  TRANSITION(RIGHT_SIGNAL, HAZARD),
-  TRANSITION(RIGHT_SIGNAL, LEFT_SIGNAL), TRANSITION(HAZARD, INIT_STATE)
+  TRANSITION(RIGHT_SIGNAL, LEFT_SIGNAL), TRANSITION(HAZARD, INIT_STATE),
+  TRANSITION(HAZARD, LEFT_SIGNAL),       TRANSITION(HAZARD, RIGHT_SIGNAL)
 };
 
 StatusCode init_lights(void) {
-  gpio_init_pin(&LEFT_LIGHT_ADDR, GPIO_OUTPUT_OPEN_DRAIN, GPIO_STATE_LOW);
-  gpio_init_pin(&RIGHT_LIGHT_ADDR, GPIO_OUTPUT_OPEN_DRAIN, GPIO_STATE_LOW);
-
+  soft_timer_init(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
   fsm_init(lights, s_PD_lights_list, s_PD_transition_list, INIT_STATE, NULL);
   return STATUS_CODE_OK;
 }
diff --git a/projects/power_distribution/src/main.c b/projects/power_distribution/src/main.c
index 238c85f0d..32fed0657 100644
--- a/projects/power_distribution/src/main.c
+++ b/projects/power_distribution/src/main.c
@@ -1,43 +1,68 @@
 #include <stdio.h>
 
+#include "bts_load_switch.h"
 #include "can.h"
 #include "gpio.h"
+#include "i2c.h"
+#include "interrupt.h"
 #include "lights_fsm.h"
 #include "log.h"
 #include "master_task.h"
+#include "output_current_sense.h"
+#include "pin_defs.h"
 #include "power_seq_fsm.h"
 #include "tasks.h"
 
-#define DEVICE_ID 0x04
-
-#define DEVICE_ID 0x04
+#define DEVICE_ID 0x06
 
 static CanStorage s_can_storage = { 0 };
 const CanSettings can_settings = {
   .device_id = DEVICE_ID,
-  .bitrate = CAN_HW_BITRATE_125KBPS,
+  .bitrate = CAN_HW_BITRATE_500KBPS,
   .tx = { GPIO_PORT_A, 12 },
   .rx = { GPIO_PORT_A, 11 },
-  .loopback = true,
+  .loopback = false,
+};
+
+I2CSettings i2c_settings = {
+  .speed = I2C_SPEED_STANDARD,
+  .sda = PD_I2C_SDA,
+  .scl = PD_I2C_SCL,
 };
 
-void pre_loop_init() {}
+void pre_loop_init() {
+  pd_output_init();
+  pd_sense_init();
+  adc_init();
+}
 
 void run_fast_cycle() {}
+void run_medium_cycle() {
+  run_can_rx_cycle();
+  wait_tasks(1);
 
-void run_medium_cycle() {}
+  fsm_run_cycle(lights);
+  wait_tasks(1);
+
+  run_can_tx_cycle();
+  wait_tasks(1);
+}
 
 void run_slow_cycle() {}
 
 int main() {
+  LOG_DEBUG("starting here...\n");
   tasks_init();
   log_init();
   gpio_init();
-  LOG_DEBUG("Welcome to TEST!");
-
+  interrupt_init();
+  i2c_init(I2C_PORT_1, &i2c_settings);
+  pca9555_gpio_init(I2C_PORT_1);
   can_init(&s_can_storage, &can_settings);
-  init_power_seq();
+  // init_power_seq();
   init_lights();
+
+  LOG_DEBUG("Welcome to PD!\n");
   init_master_task();
 
   tasks_start();
diff --git a/projects/power_distribution/src/output_config.c b/projects/power_distribution/src/output_config.c
index c73af3443..cd0361881 100644
--- a/projects/power_distribution/src/output_config.c
+++ b/projects/power_distribution/src/output_config.c
@@ -206,17 +206,17 @@ BtsLoadSwitchOutput g_output_config[NUM_OUTPUTS] = {
 // Output Group Definitions
 static OutputGroupDef s_output_group_left_signal = {
   .num_outputs = 1,
-  .outputs = { FRONT_OUTPUT_LEFT_FRONT_TURN_LIGHT },
+  .outputs = { FRONT_OUTPUT_PEDAL },
 };
 
 static OutputGroupDef s_output_group_right_signal = {
   .num_outputs = 1,
-  .outputs = { FRONT_OUTPUT_RIGHT_FRONT_TURN_LIGHT },
+  .outputs = { FRONT_OUTPUT_STEERING },
 };
 
 static OutputGroupDef s_output_group_hazards = {
   .num_outputs = 2,
-  .outputs = { FRONT_OUTPUT_LEFT_FRONT_TURN_LIGHT, FRONT_OUTPUT_RIGHT_FRONT_TURN_LIGHT },
+  .outputs = { FRONT_OUTPUT_STEERING, FRONT_OUTPUT_PEDAL },
 };
 
 static OutputGroupDef s_output_group_test = {
diff --git a/smoke/pd_load_switch_validate/src/main.c b/smoke/pd_load_switch_validate/src/main.c
index e507672e0..8bdb2ee48 100644
--- a/smoke/pd_load_switch_validate/src/main.c
+++ b/smoke/pd_load_switch_validate/src/main.c
@@ -5,6 +5,7 @@
 #include "master_task.h"
 #include "pca_config.h"
 #include "tasks.h"
+#include "i2c.h"
 
 Pca9555GpioAddress addy[] = {
   { .i2c_address = I2C_ADDRESS_1, .pin = PCA9555_PIN_IO1_0 },
@@ -47,7 +48,7 @@ int main() {
   gpio_it_init();
 
   i2c_init(I2C_PORT_1, &settings);
-  pca9555_gpio_init(0, I2C_ADDRESS_1);
+  pca9555_gpio_init(I2C_PORT_1);
 
   LOG_DEBUG("Welcome to TEST!");
 
diff --git a/smoke/power_distribution/src/main.c b/smoke/power_distribution/src/main.c
index 6161eeedb..907819076 100644
--- a/smoke/power_distribution/src/main.c
+++ b/smoke/power_distribution/src/main.c
@@ -63,7 +63,7 @@ int main() {
   interrupt_init();
   gpio_init();
   i2c_init(0, &i2c_settings);
-  pca9555_gpio_init(I2C_PORT_1, 0); // second param (i2c_address) isn't needed/used by pca init
+  pca9555_gpio_init(I2C_PORT_1);
 
   tasks_init_task(smoke_pd, TASK_PRIORITY(2), NULL);
   LOG_DEBUG("PD Smoke!\n");