Coded inputs and udp sending them

program/CMakeLists.txt

@@ -17,6 +17,7 @@ pico_sdk_init()
 add_executable(controller
   src/main.c
   src/controller.c
+  src/inputs.c
   src/wifi/wifi_operator.c
   src/wifi/udp_server.c
   src/wifi/dhcp_server.c
@@ -29,9 +30,7 @@ target_include_directories(controller PRIVATE
 
 target_link_libraries(controller
   pico_stdlib
-  hardware_i2c
+  hardware_adc
-  hardware_pwm
-  hardware_uart
   pico_cyw43_arch_lwip_poll
 )
 

program/src/controller.c

@@ -18,6 +18,8 @@ void controller_init(void)
 
     cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
 
+    inputs_init();
+
     if(wifi_operator_init())
         controller.is_running = false;
 
@@ -61,9 +63,11 @@ static inline void update_time(void)
 
 void controller_handle_inputs_outputs(void)
 {
+    update_time();
+
     cyw43_arch_poll();
 
-    update_time();
+    inputs_update();
 
     udp_server_send();
 

program/src/headers/controller.h

@@ -2,10 +2,12 @@
 #define ROBOT_H
 
 #include <stdbool.h>
+#include "headers/inputs.h"
 #include "wifi/headers/dhcp_server.h"
 #include "wifi/headers/udp_server.h"
 
 typedef struct controller_t {
+    inputs_t inputs;
     dhcp_server_t dhcp_server;
     udp_server_t udp_server;
 

program/src/headers/inputs.h

@@ -0,0 +1,41 @@
+#ifndef INPUTS_H
+#define INPUTS_H
+
+#include <pico/types.h>
+
+typedef enum inputs_pin_t {
+    JOYSTICK_X_AXIS_PIN = 26,
+    JOYSTICK_Y_AXIS_PIN = 27,
+
+    BUTTON_BLACK_PIN = 1,
+    BUTTON_BLUE_PIN = 5,
+    BUTTON_WHITE_PIN = 6,
+    BUTTON_GREEN_PIN = 2,
+
+    BUTTON_L_PIN = 9,
+    BUTTON_R_PIN = 13,
+}  inputs_pin_t;
+
+#define JOYSTICK_X_AXIS_ADC_INPUT 0
+#define JOYSTICK_Y_AXIS_ADC_INPUT 1
+
+typedef struct inputs_t {
+    int8_t joystick_x;
+    int8_t joystick_y;
+
+    struct {
+        bool button_black : 1;
+        bool button_blue  : 1;
+        bool button_white : 1;
+        bool button_green : 1;
+        bool button_l     : 1;
+        bool button_r     : 1;
+    } buttons;
+} __attribute__((__packed__)) inputs_t;
+
+// Init all inputs
+void inputs_init(void);
+// Update inputs
+void inputs_update(void);
+
+#endif // INPUTS_H

program/src/inputs.c

@@ -0,0 +1,51 @@
+#include "headers/inputs.h"
+
+#include <pico/stdlib.h>
+#include <hardware/gpio.h>
+#include <hardware/adc.h>
+#include "headers/controller.h"
+
+#define JOYSTICK_DEAD_ZONE 3
+
+void inputs_init(void)
+{
+    adc_init();
+    adc_gpio_init(JOYSTICK_X_AXIS_PIN);
+    adc_gpio_init(JOYSTICK_Y_AXIS_PIN);
+
+    const uint BUTTONS_PIN[] = {BUTTON_BLACK_PIN, BUTTON_BLUE_PIN, BUTTON_WHITE_PIN, BUTTON_GREEN_PIN, BUTTON_L_PIN, BUTTON_R_PIN};
+
+    for(size_t i = 0; i < sizeof(BUTTONS_PIN) / sizeof(uint); i++)
+    {
+        gpio_init(BUTTONS_PIN[i]);
+        gpio_set_dir(BUTTONS_PIN[i], GPIO_IN);
+        gpio_pull_up(BUTTONS_PIN[i]);
+    }
+}
+
+void inputs_update(void)
+{
+    // Update joystick x and y position
+    adc_select_input(JOYSTICK_X_AXIS_ADC_INPUT);
+    uint16_t joystick_raw = adc_read();
+    controller.inputs.joystick_x = (joystick_raw >> 4) - 128;
+
+    adc_select_input(JOYSTICK_Y_AXIS_ADC_INPUT);
+    joystick_raw = adc_read();
+    controller.inputs.joystick_y = (joystick_raw >> 4) - 128;
+
+    // Dead zone
+    if(abs(controller.inputs.joystick_x) < JOYSTICK_DEAD_ZONE)
+        controller.inputs.joystick_x = 0;
+
+    if(abs(controller.inputs.joystick_y) < JOYSTICK_DEAD_ZONE)
+        controller.inputs.joystick_y = 0;
+
+    // Buttons
+    controller.inputs.buttons.button_black = !gpio_get(BUTTON_BLACK_PIN);
+    controller.inputs.buttons.button_blue  = !gpio_get(BUTTON_BLUE_PIN);
+    controller.inputs.buttons.button_white = !gpio_get(BUTTON_WHITE_PIN);
+    controller.inputs.buttons.button_green = !gpio_get(BUTTON_GREEN_PIN);
+    controller.inputs.buttons.button_l     = !gpio_get(BUTTON_L_PIN);
+    controller.inputs.buttons.button_r     = !gpio_get(BUTTON_R_PIN);
+}

program/src/wifi/udp_server.c

@@ -1,9 +1,11 @@
 #include "headers/udp_server.h"
 
 #include <stdio.h>
+#include <string.h>
 #include "headers/controller.h"
 
-#define MSG_LEN 128
+#define MSG_LEN (sizeof(uint16_t) + sizeof(inputs_t)) / sizeof(uint8_t)
+#define MSG_DELAY_MS 50.0f
 
 int udp_server_init(void)
 {
@@ -16,34 +18,48 @@ int udp_server_init(void)
 
     if(udp_bind(controller.udp_server.pcb, IP_ADDR_ANY, UDP_SERVER_PORT))
     {
-        printf("Error bind UDP server");
+        puts("Error binding UDP server");
         return -1;
     }
 
-    puts("UDP server started");
-
     return 0;
 }
 
 void udp_server_send(void)
 {
-    static float elapsed_time = 0.0f;
+    static float elapsed_time_ms = 0.0f;
-    elapsed_time += controller.delta_time_ms;
+    elapsed_time_ms += controller.delta_time_ms;
 
     static uint8_t counter = 0;
 
-    if(elapsed_time >= 1000.0f)
+    if(elapsed_time_ms >= MSG_DELAY_MS)
     {
-        elapsed_time = 0.0f;
+        static union data_t {
+            struct {
+                uint16_t packet_number;
+                inputs_t inputs;
+            } hard;
+
+            uint8_t raw[MSG_LEN];
+        } data = {0};
+
+        data.hard.packet_number++;
+        data.hard.inputs = controller.inputs;
 
         struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, MSG_LEN, PBUF_RAM);
-        uint8_t *payload = (uint8_t *)p->payload;
+        memcpy((uint8_t *)p->payload, data.raw, MSG_LEN);
-        payload[0] = counter;
+        //printf("%d:%d   %d\n", data.raw[2], data.raw[3], data.raw[4]);
 
-        udp_sendto(controller.udp_server.pcb, p, IP_ADDR_ANY, UDP_CLIENT_PORT);
+        err_t error_code = udp_sendto(controller.udp_server.pcb, p, IP_ADDR_ANY, UDP_CLIENT_PORT);
 
         pbuf_free(p);
 
+        if(error_code)
+        {
+            printf("UDP send error : %d\n", error_code);
+        }
+
+        elapsed_time_ms = 0.0f;
         counter++;
     }
 }