ulysse
/
controller_2025-2026
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: ulysse:d256b99cc0e57b74e3091d6bc6c9eb042c3d4803
Branches Tags
ulysse:main
..
compare: ulysse:5e7dd2af1c3be444e425033a60e2f69636c76537
Branches Tags
ulysse:main

No commits in common. "d256b99cc0e57b74e3091d6bc6c9eb042c3d4803" and "5e7dd2af1c3be444e425033a60e2f69636c76537" have entirely different histories.
d256b99cc0 ... 5e7dd2af1c

8 changed files with 56 additions and 166 deletions
Show Stats Expand all files Collapse all files

5
program/CMakeLists.txt
View File

@ -17,7 +17,6 @@ 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
@ -30,7 +29,9 @@ target_include_directories(controller PRIVATE
target_link_libraries(controller
pico_stdlib
hardware_adc
hardware_i2c
hardware_pwm
hardware_uart
pico_cyw43_arch_lwip_poll
)

27
program/src/controller.c
View File

@ -18,8 +18,6 @@ void controller_init(void)
cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
inputs_init();
if(wifi_operator_init())
controller.is_running = false;
@ -42,31 +40,30 @@ void controller_init(void)
static inline void update_time(void)
{
static float last_time_ms = 0.0;
float start_time_ms = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
controller.delta_time_ms = start_time_ms - last_time_ms;
last_time_ms = start_time_ms;
static float last_time = 0.0;
float start_time = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
controller.delta_time_ms = start_time - last_time;
last_time = start_time;
static float elapsed_time_ms = 0.0f;
elapsed_time_ms += controller.delta_time_ms;
static float elapsed_time = 0.0f;
elapsed_time += controller.delta_time_ms;
static bool led_state = false;
if(elapsed_time_ms >= 1000.0f)
if(elapsed_time >= 1000.0f)
{
elapsed_time = 0.0f;
cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
elapsed_time_ms = 0.0f;
led_state = !led_state;
}
}
void controller_handle_inputs_outputs(void)
{
update_time();
cyw43_arch_poll();
inputs_update();
update_time();
udp_server_send();
@ -76,8 +73,4 @@ void controller_handle_inputs_outputs(void)
void controller_deinit(void)
{
udp_server_deinit();
dhcp_server_deinit();
cyw43_arch_deinit();
}

2
program/src/headers/controller.h
View File

@ -2,12 +2,10 @@
#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;

41
program/src/headers/inputs.h
View File

@ -1,41 +0,0 @@
#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

51
program/src/inputs.c
View File

@ -1,51 +0,0 @@
#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);
}

55
program/src/wifi/dhcp_server.c
View File

@ -60,7 +60,7 @@ typedef struct {
uint8_t options[312]; // optional parameters, variable, starts with magic
} dhcp_msg_t;
static void dhcp_socket_sendto(struct netif *nif, const void *buf, size_t len, uint32_t ip, uint16_t port)
static int dhcp_socket_sendto(struct udp_pcb **pcb, struct netif *nif, const void *buf, size_t len, uint32_t ip, uint16_t port)
{
if(len > 0xffff)
{
@ -70,8 +70,7 @@ static void dhcp_socket_sendto(struct netif *nif, const void *buf, size_t len, u
struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
if(p == NULL)
{
puts("Pbuf allocation error");
return;
return -ENOMEM;
}
memcpy(p->payload, buf, len);
@ -83,19 +82,21 @@ static void dhcp_socket_sendto(struct netif *nif, const void *buf, size_t len, u
if(nif != NULL)
{
err = udp_sendto_if(controller.dhcp_server.pcb, p, &dest, port, nif);
err = udp_sendto_if(*pcb, p, &dest, port, nif);
}
else
{
err = udp_sendto(controller.dhcp_server.pcb, p, &dest, port);
err = udp_sendto(*pcb, p, &dest, port);
}
pbuf_free(p);
if(err != ERR_OK)
{
printf("Error sending dhcp packets : %d\n", err);
return err;
}
return len;
}
static uint8_t *opt_find(uint8_t *opt, uint8_t cmd)
@ -143,8 +144,10 @@ static void opt_write_u32(uint8_t **opt, uint8_t cmd, uint32_t val)
*opt = o;
}
static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unused, struct pbuf *p, const ip_addr_t *src_addr __unused, u16_t src_port __unused)
static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct pbuf *p, const ip_addr_t *src_addr __unused, u16_t src_port __unused)
{
dhcp_server_t *d = arg;
// This is around 548 bytes
dhcp_msg_t dhcp_msg;
@ -159,7 +162,7 @@ static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unuse
}
dhcp_msg.op = DHCPOFFER;
memcpy(&dhcp_msg.yiaddr, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip)), 4);
memcpy(&dhcp_msg.yiaddr, &ip4_addr_get_u32(ip_2_ip4(&d->ip)), 4);
uint8_t *opt = (uint8_t *)&dhcp_msg.options;
opt += 4; // assume magic cookie: 99, 130, 83, 99
@ -174,21 +177,21 @@ static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unuse
case DHCPDISCOVER: {
int yi = DHCPS_MAX_IP;
for (int i = 0; i < DHCPS_MAX_IP; ++i) {
if (memcmp(controller.dhcp_server.lease[i].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
if (memcmp(d->lease[i].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
// MAC match, use this IP address
yi = i;
break;
}
if (yi == DHCPS_MAX_IP) {
// Look for a free IP address
if (memcmp(controller.dhcp_server.lease[i].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
if (memcmp(d->lease[i].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
// IP available
yi = i;
}
uint32_t expiry = controller.dhcp_server.lease[i].expiry << 16 | 0xffff;
uint32_t expiry = d->lease[i].expiry << 16 | 0xffff;
if ((int32_t)(expiry - cyw43_hal_ticks_ms()) < 0) {
// IP expired, reuse it
memset(controller.dhcp_server.lease[i].mac, 0, MAC_LEN);
memset(d->lease[i].mac, 0, MAC_LEN);
yi = i;
}
}
@ -208,7 +211,7 @@ static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unuse
// Should be NACK
goto ignore_request;
}
if (memcmp(o + 2, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip)), 3) != 0) {
if (memcmp(o + 2, &ip4_addr_get_u32(ip_2_ip4(&d->ip)), 3) != 0) {
// Should be NACK
goto ignore_request;
}
@ -217,17 +220,17 @@ static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unuse
// Should be NACK
goto ignore_request;
}
if (memcmp(controller.dhcp_server.lease[yi].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
if (memcmp(d->lease[yi].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
// MAC match, ok to use this IP address
} else if (memcmp(controller.dhcp_server.lease[yi].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
} else if (memcmp(d->lease[yi].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
// IP unused, ok to use this IP address
memcpy(controller.dhcp_server.lease[yi].mac, dhcp_msg.chaddr, MAC_LEN);
memcpy(d->lease[yi].mac, dhcp_msg.chaddr, MAC_LEN);
} else {
// IP already in use
// Should be NACK
goto ignore_request;
}
controller.dhcp_server.lease[yi].expiry = (cyw43_hal_ticks_ms() + DEFAULT_LEASE_TIME_S * 1000) >> 16;
d->lease[yi].expiry = (cyw43_hal_ticks_ms() + DEFAULT_LEASE_TIME_S * 1000) >> 16;
dhcp_msg.yiaddr[3] = DHCPS_BASE_IP + yi;
opt_write_u8(&opt, DHCP_OPT_MSG_TYPE, DHCPACK);
printf("DHCPS: client connected: MAC=%02x:%02x:%02x:%02x:%02x:%02x IP=%u.%u.%u.%u\n",
@ -240,14 +243,14 @@ static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unuse
goto ignore_request;
}
opt_write_n(&opt, DHCP_OPT_SERVER_ID, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip)));
opt_write_n(&opt, DHCP_OPT_SUBNET_MASK, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.nm)));
opt_write_n(&opt, DHCP_OPT_ROUTER, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip))); // aka gateway; can have multiple addresses
opt_write_n(&opt, DHCP_OPT_DNS, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip))); // this server is the dns
opt_write_n(&opt, DHCP_OPT_SERVER_ID, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip)));
opt_write_n(&opt, DHCP_OPT_SUBNET_MASK, 4, &ip4_addr_get_u32(ip_2_ip4(&d->nm)));
opt_write_n(&opt, DHCP_OPT_ROUTER, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); // aka gateway; can have multiple addresses
opt_write_n(&opt, DHCP_OPT_DNS, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); // this server is the dns
opt_write_u32(&opt, DHCP_OPT_IP_LEASE_TIME, DEFAULT_LEASE_TIME_S);
*opt++ = DHCP_OPT_END;
struct netif *nif = ip_current_input_netif();
dhcp_socket_sendto(nif, &dhcp_msg, opt - (uint8_t *)&dhcp_msg, 0xffffffff, PORT_DHCP_CLIENT);
dhcp_socket_sendto(&d->pcb, nif, &dhcp_msg, opt - (uint8_t *)&dhcp_msg, 0xffffffff, PORT_DHCP_CLIENT);
ignore_request:
pbuf_free(p);
@ -261,19 +264,19 @@ int dhcp_server_init(void)
controller.dhcp_server.pcb = udp_new();
if (controller.dhcp_server.pcb == NULL) {
puts("Error creating DHCP server");
return -1;
return -ENOMEM;
}
// Register callback
udp_recv(controller.dhcp_server.pcb, dhcp_server_process, NULL);
udp_recv(controller.dhcp_server.pcb, dhcp_server_process, (void *)&controller.dhcp_server);
if(udp_bind(controller.dhcp_server.pcb, IP_ANY_TYPE, PORT_DHCP_SERVER))
{
puts("Error binding DHCP server");
return -1;
}
puts("DHCP server started");
return 0;
}

38
program/src/wifi/udp_server.c
View File

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

3
program/src/wifi/wifi_operator.c
View File

@ -14,7 +14,10 @@ int wifi_operator_init(void)
}
cyw43_arch_enable_ap_mode(WIFI_OPERATOR_SSID, WIFI_OPERATOR_PASSWORD, CYW43_AUTH_WPA2_AES_PSK);
cyw43_wifi_ap_set_channel(&cyw43_state, WIFI_OPERATOR_CHANNEL);
puts("AP mode enabled");
return 0;
}