diff --git a/firmware/topfloor/CMakeLists.txt b/firmware/topfloor/CMakeLists.txt
new file mode 100644
index 0000000..5d4badc
--- /dev/null
+++ b/firmware/topfloor/CMakeLists.txt
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.12)
+
+set(PICO_BOARD pico CACHE STRING "Board type")
+include(pico_sdk_import.cmake)    
+
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+project(i2c C CXX ASM)
+
+pico_sdk_init()
+
+
+add_executable(topfloor
+    src/main.c 
+    src/actions/user.c 
+    src/servo/servo.c
+    src/stepper/stepper.c
+    )
+
+pico_set_program_name(topfloor "i2ctest")
+pico_set_program_version(topfloor "0.1")
+
+pico_enable_stdio_uart(topfloor 0)
+pico_enable_stdio_usb(topfloor 1)
+
+target_link_libraries(topfloor
+    pico_stdlib
+    pico_stdio
+    pico_time
+    pico_i2c_slave
+    pico_multicore
+    hardware_i2c
+    hardware_pwm
+)
+
+target_include_directories(topfloor PRIVATE
+  ${CMAKE_CURRENT_LIST_DIR}
+  src/
+)
+
+pico_add_extra_outputs(topfloor)
\ No newline at end of file
diff --git a/firmware/topfloor/pico_sdk_import.cmake b/firmware/topfloor/pico_sdk_import.cmake
new file mode 100644
index 0000000..d493cc2
--- /dev/null
+++ b/firmware/topfloor/pico_sdk_import.cmake
@@ -0,0 +1,121 @@
+# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+# Copyright 2020 (c) 2020 Raspberry Pi (Trading) Ltd.
+#
+# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
+# following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
+# disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products
+# derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
+    set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
+    message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
+endif ()
+
+if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
+  set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
+  message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
+endif()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
+
+if (NOT PICO_SDK_PATH)
+    if (PICO_SDK_FETCH_FROM_GIT)
+        include(FetchContent)
+        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+        if (PICO_SDK_FETCH_FROM_GIT_PATH)
+            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+        endif ()
+        FetchContent_Declare(
+                pico_sdk
+                GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+        )
+
+        if (NOT pico_sdk)
+            message("Downloading Raspberry Pi Pico SDK")
+            # GIT_SUBMODULES_RECURSE was added in 3.17
+            if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
+                FetchContent_Populate(
+                        pico_sdk
+                        QUIET
+                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+                        GIT_SUBMODULES_RECURSE FALSE
+
+                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
+                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
+                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
+                )
+            else ()
+                FetchContent_Populate(
+                        pico_sdk
+                        QUIET
+                        GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                        GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+
+                        SOURCE_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-src
+                        BINARY_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-build
+                        SUBBUILD_DIR ${FETCHCONTENT_BASE_DIR}/pico_sdk-subbuild
+                )
+            endif ()
+
+            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+        endif ()
+        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+    else ()
+        message(FATAL_ERROR
+                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+                )
+    endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
diff --git a/firmware/topfloor/src/actions/user.c b/firmware/topfloor/src/actions/user.c
new file mode 100644
index 0000000..6b477de
--- /dev/null
+++ b/firmware/topfloor/src/actions/user.c
@@ -0,0 +1,62 @@
+#include "user.h"
+#include "../servo/servo.h"
+#include "../stepper/stepper.h"
+
+#include "stdio.h"
+
+volatile uint8_t state_complete = 0;
+
+static uint8_t current_state = 0;
+
+#define START_PIN 22
+
+void setup() {
+    gpio_init(START_PIN);
+    gpio_pull_up(START_PIN);
+    gpio_set_dir(START_PIN, GPIO_IN);
+
+    gpio_init(25);
+    gpio_set_dir(25, GPIO_OUT);
+}
+
+void loop(uint8_t requested_state) {
+    // printf("Requested state is %d\n", requested_state);
+    if(current_state == 0) {
+        current_state = requested_state;
+    }
+    switch (current_state) {
+        case 0: 
+            break;
+        case 1: 
+            startup_mode();
+            break;
+    }
+    return;
+}
+
+void startup_mode() {
+    printf("Waiting for pin\n");
+    int debounce = 0;
+    while(debounce < 10) {
+        if(gpio_get(START_PIN)) {
+            debounce++;
+        } else {
+            debounce = 0;
+        }
+        sleep_ms(10);
+    }
+    gpio_put(25,1);
+    printf("Waiting for pin to get yanked\n");
+    debounce = 0;
+    while(debounce < 10) {
+        if(!gpio_get(START_PIN)) {
+            debounce++;
+        } else {
+            debounce = 0;
+        }
+        sleep_ms(10);
+    }
+    current_state = 0;
+    state_complete = 1;
+    gpio_put(25,0);
+}
\ No newline at end of file
diff --git a/firmware/topfloor/src/actions/user.h b/firmware/topfloor/src/actions/user.h
new file mode 100644
index 0000000..70dde9a
--- /dev/null
+++ b/firmware/topfloor/src/actions/user.h
@@ -0,0 +1,21 @@
+#pragma once
+
+// Ne brisatic vec implementirati
+#include "stdint.h"
+
+void setup();
+void loop(uint8_t requested_state);
+
+void startup_mode();
+
+#define NOOP 0x0
+
+/* Actions to build tower */
+#define PIKCUP 0x1
+#define DROP_CAN 0x2
+
+#define RAISE_LEVEL_1 0x3
+#define RAISE_LEVEL_2 0x3
+#define RAISE_LEVEL_3 0x3
+
+extern volatile uint8_t state_complete;
\ No newline at end of file
diff --git a/firmware/topfloor/src/main.c b/firmware/topfloor/src/main.c
new file mode 100644
index 0000000..c159611
--- /dev/null
+++ b/firmware/topfloor/src/main.c
@@ -0,0 +1,85 @@
+#include "hardware/i2c.h"
+#include "pico/i2c_slave.h"
+#include "pico/stdlib.h"
+#include "pico/multicore.h"
+#include "pico/util/queue.h"
+#include "stdio.h"
+
+#include "actions/user.h"
+
+#define I2C_PORT i2c1       // Use I2C0 on GPIO 8 (SDA) and GPIO 9 (SCL)
+#define I2C_SLAVE_ADDR 0x41 // Must match Raspberry Pi 4
+
+#define GPIO_SDA 18
+#define GPIO_SCL 19
+
+volatile uint8_t requested_state = 0;
+
+queue_t queue;
+
+
+bool Start = true;
+
+// Callback function for I2C slave write operation
+void i2c_slave_callback(i2c_inst_t *i2c, i2c_slave_event_t event) {
+  if (event == I2C_SLAVE_RECEIVE) {
+    // Data is received
+    char byte = 0;
+    state_complete = 0;
+    byte = i2c_read_byte_raw(i2c); // Read data from master // Null-terminate string
+    printf("Received: %d\n", byte);
+    queue_add_blocking(&queue,&byte);
+  } else if (event == I2C_SLAVE_REQUEST) {
+    if (Start) {
+      if(state_complete > 0) {
+        printf("Sent: %d\n", state_complete);
+        i2c_write_byte_raw(i2c, state_complete);
+        state_complete = 0;
+      } else {
+        i2c_write_byte_raw(i2c, '\x00');
+      }
+      Start = false;
+    } else {
+      i2c_write_byte_raw(i2c, '\x00');
+    }
+  } else if (event == I2C_SLAVE_FINISH) {
+    Start = true;
+  }
+}
+
+void core2() {
+  setup();
+  while(true) {
+    uint8_t req = 0;
+    if(!queue_is_empty(&queue)) {
+      queue_remove_blocking(&queue, &req); 
+      printf("Recieved req %d", req);
+    }
+    loop(req);
+  }
+}
+
+int main() {
+  stdio_init_all();
+
+  // Initialize I2C as a slave
+  i2c_init(I2C_PORT, 100000);          // 100kHz is a safe speed
+  gpio_set_function(GPIO_SDA, GPIO_FUNC_I2C); // SDA pin (GPIO 8)
+  gpio_set_function(GPIO_SCL, GPIO_FUNC_I2C); // SCL pin (GPIO 9)
+  gpio_pull_up(GPIO_SDA);
+  gpio_pull_up(GPIO_SCL);
+
+  queue_init(&queue, 1, 2);
+
+
+  // Enable the I2C slave events
+  i2c_slave_init(I2C_PORT, I2C_SLAVE_ADDR, &i2c_slave_callback);
+
+  multicore_launch_core1(&core2);
+
+  printf("Pico I2C Slave Ready...\n");
+
+  while (1) {
+    tight_loop_contents(); // Keeps the CPU in a tight loop waiting for events
+  }
+}
\ No newline at end of file
diff --git a/firmware/topfloor/src/servo/servo.c b/firmware/topfloor/src/servo/servo.c
new file mode 100644
index 0000000..0d52c31
--- /dev/null
+++ b/firmware/topfloor/src/servo/servo.c
@@ -0,0 +1,35 @@
+#include <hardware/structs/clocks.h>
+#include <stdint.h>
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "hardware/clocks.h"
+
+#include "servo.h"
+
+#define ROTATE_180 2500
+#define ROTATE_0 500
+
+
+void setup_servo(uint8_t pin) {
+    // Initialize the GPIO pin for PWM output
+    gpio_set_function(pin, GPIO_FUNC_PWM);
+    
+    // Get the PWM slice for the given pin
+    uint slice_num = pwm_gpio_to_slice_num(pin);
+
+    // Set the frequency and wrap (period)
+    uint32_t clk  = clock_get_hz(clk_sys);
+    uint32_t div  = clk / 20000/ 50;
+    pwm_set_clkdiv_int_frac4(slice_num,div,1);  // Set the clock divider
+    pwm_set_wrap(slice_num, 20000);
+
+    // Enable the PWM
+    pwm_set_enabled(slice_num, true);
+}
+
+void set_degree(uint8_t pin, float deg) {
+
+    int duty = (((float)(ROTATE_180 - ROTATE_0) / 180.0) * deg) + ROTATE_0;
+    pwm_set_gpio_level(pin, duty);
+}
\ No newline at end of file
diff --git a/firmware/topfloor/src/servo/servo.h b/firmware/topfloor/src/servo/servo.h
new file mode 100644
index 0000000..b15b4ba
--- /dev/null
+++ b/firmware/topfloor/src/servo/servo.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "pico/stdlib.h"
+
+void setup_servo(uint8_t pin);
+
+void set_degree(uint8_t pin, float angle);
\ No newline at end of file
diff --git a/firmware/topfloor/src/stepper/indentity.h b/firmware/topfloor/src/stepper/indentity.h
new file mode 100644
index 0000000..8ee8058
--- /dev/null
+++ b/firmware/topfloor/src/stepper/indentity.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#define INVALID 0
+#define STEPPER_DRIVER 1
+#define SERVO_DRIVER 2
\ No newline at end of file
diff --git a/firmware/topfloor/src/stepper/stepper.c b/firmware/topfloor/src/stepper/stepper.c
new file mode 100644
index 0000000..34f5776
--- /dev/null
+++ b/firmware/topfloor/src/stepper/stepper.c
@@ -0,0 +1,24 @@
+#include "stepper.h"
+
+
+
+
+stepper setup_stepper(uint8_t dir, uint8_t pulse) {
+    gpio_init(dir);
+    gpio_init(pulse);
+    gpio_set_dir(dir, GPIO_OUT);
+    gpio_set_dir(pulse, GPIO_OUT);
+    stepper s ={.dir = dir,.pulse = pulse}; 
+    return s;
+}
+
+void stepper_move_block(const stepper *stepper, int steps, uint32_t wait_ms) {
+    gpio_put(stepper->dir, steps > 0);
+    steps = (steps < 0)? -steps : steps;
+    for(int i = 0; i < steps; i++) {
+        gpio_put(stepper->pulse, 1);
+        sleep_us(wait_ms);
+        gpio_put(stepper->pulse, 0);
+        sleep_us(wait_ms);
+    }
+}
\ No newline at end of file
diff --git a/firmware/topfloor/src/stepper/stepper.h b/firmware/topfloor/src/stepper/stepper.h
new file mode 100644
index 0000000..6d10c54
--- /dev/null
+++ b/firmware/topfloor/src/stepper/stepper.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "pico/stdlib.h"
+
+typedef struct servo {
+  uint8_t dir;
+  uint8_t pulse;
+} stepper;
+
+stepper setup_stepper(uint8_t dir, uint8_t pulse);
+
+void stepper_move_block(const stepper *stepper, int steps, uint32_t speed);
\ No newline at end of file