Added i2c action for starter coil

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);
+}

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;

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
+  }
+}

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);
+}

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);

firmware/topfloor/src/stepper/indentity.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#define INVALID 0
+#define STEPPER_DRIVER 1
+#define SERVO_DRIVER 2

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);
+    }
+}

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);