diff --git a/program/CMakeLists.txt b/program/CMakeLists.txt index 8b50421..dfd2034 100644 --- a/program/CMakeLists.txt +++ b/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_pwm - hardware_uart + hardware_adc pico_cyw43_arch_lwip_poll ) diff --git a/program/src/controller.c b/program/src/controller.c index a829c2f..8d05cbf 100644 --- a/program/src/controller.c +++ b/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(); diff --git a/program/src/headers/controller.h b/program/src/headers/controller.h index b2d4d4c..610830f 100644 --- a/program/src/headers/controller.h +++ b/program/src/headers/controller.h @@ -2,10 +2,12 @@ #define ROBOT_H #include +#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; diff --git a/program/src/headers/inputs.h b/program/src/headers/inputs.h new file mode 100644 index 0000000..20b4e48 --- /dev/null +++ b/program/src/headers/inputs.h @@ -0,0 +1,41 @@ +#ifndef INPUTS_H +#define INPUTS_H + +#include + +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 \ No newline at end of file diff --git a/program/src/inputs.c b/program/src/inputs.c new file mode 100644 index 0000000..943a10f --- /dev/null +++ b/program/src/inputs.c @@ -0,0 +1,51 @@ +#include "headers/inputs.h" + +#include +#include +#include +#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); +} diff --git a/program/src/wifi/udp_server.c b/program/src/wifi/udp_server.c index 0631456..49c6571 100644 --- a/program/src/wifi/udp_server.c +++ b/program/src/wifi/udp_server.c @@ -1,9 +1,11 @@ #include "headers/udp_server.h" #include +#include #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; - elapsed_time += controller.delta_time_ms; + static float elapsed_time_ms = 0.0f; + 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; - payload[0] = counter; + memcpy((uint8_t *)p->payload, data.raw, MSG_LEN); + //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++; } }