From 2ad7d2c898a8d0030d1d820b30ac754ba03a30c7 Mon Sep 17 00:00:00 2001 From: Ryan Yocum Date: Tue, 17 Mar 2026 16:20:38 -0400 Subject: [PATCH 01/16] Start robot control --- components/encoder/CMakeLists.txt | 2 +- components/motorhat/include/motorhat.h | 2 +- components/motorhat/motorhat.c | 20 ++++++++++---------- main/Kconfig.projbuild | 6 ++++++ main/esp_driver.c | 26 ++++++-------------------- 5 files changed, 24 insertions(+), 32 deletions(-) diff --git a/components/encoder/CMakeLists.txt b/components/encoder/CMakeLists.txt index 208239d..a7f6abc 100644 --- a/components/encoder/CMakeLists.txt +++ b/components/encoder/CMakeLists.txt @@ -1,3 +1,3 @@ idf_component_register(SRCS "encoder.c" - PRIV_REQUIRES esp_driver_pcnt esp_driver_gpio + REQUIRES esp_driver_pcnt esp_driver_gpio INCLUDE_DIRS "include") diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index eee26d0..01de8f4 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -48,7 +48,7 @@ typedef struct { * @brief Motor HAT device handle */ typedef struct { - pca9685_handle_t *pca9685; + pca9685_handle_t pca9685; } motorhat_handle_t; /** diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index ff68113..d66a612 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -7,7 +7,7 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, return ESP_ERR_INVALID_ARG; } - return pca9685_init(handle->pca9685, &config->pca9685_config); + return pca9685_init(&handle->pca9685, &config->pca9685_config); } esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, @@ -23,7 +23,7 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, const motorhat_motor_channels_t *channels = &motor_channels[motor]; - return pca9685_set_duty_cycle(handle->pca9685, channels->pwm_channel, speed); + return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); } esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, @@ -40,28 +40,28 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, switch (direction) { case MOTORHAT_DIRECTION_FORWARD: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, true); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); if (err != ESP_OK) return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, false); + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); break; case MOTORHAT_DIRECTION_BACKWARD: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, false); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); if (err != ESP_OK) return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, true); + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); break; case MOTORHAT_DIRECTION_BRAKE: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, true); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); if (err != ESP_OK) return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, true); + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); break; case MOTORHAT_DIRECTION_RELEASE: - err = pca9685_digital_write(handle->pca9685, channels->in1_channel, false); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); if (err != ESP_OK) return err; - err = pca9685_digital_write(handle->pca9685, channels->in2_channel, false); + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); break; default: return ESP_ERR_INVALID_ARG; diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index 1a4487e..f4eb925 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -62,6 +62,12 @@ menu "Talos ESP Driver Configuration" 0=128SPS, 1=250SPS, 2=490SPS, 3=920SPS, 4=1600SPS, 5=2400SPS, 6=3300SPS + config DRIVER_LIMIT_SWITCH_PIN + int "Limit Switch Pin" + default 22 + help + GPIO pin for limit switch alerts. + config DRIVER_WIFI_SSID string "WiFi SSID" default "YourSSID" diff --git a/main/esp_driver.c b/main/esp_driver.c index cbef674..ad50ecf 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -6,9 +6,9 @@ #include "i2c_bus.h" #include "ads1015.h" +#include "limit_switch.h" #include "motorhat.h" #include "nvs_flash.h" -#include "limit_switch.h" #define TAG "MAIN" @@ -28,7 +28,7 @@ void app_main(void) { ESP_ERROR_CHECK(ret); limit_switch_config_t limit_switch_config = { - .limit_gpio = CONFIG_LIMIT_SWITCH_PIN, + .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, }; ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config)); @@ -40,20 +40,12 @@ void app_main(void) { }; ESP_ERROR_CHECK(i2c_bus_init(&bus, &bus_config)); - i2c_bus_t adc_bus; - i2c_bus_config_t adc_bus_config = { - .port = I2C_NUM_1, - .sda_io_num = CONFIG_DRIVER_ADS1015_SDA_PIN, - .scl_io_num = CONFIG_DRIVER_ADS1015_SCL_PIN, - }; - ESP_ERROR_CHECK(i2c_bus_init(&adc_bus, &adc_bus_config)); - ads1015_handle_t ads; ads1015_config_t ads_config = { .i2c_addr = CONFIG_DRIVER_ADS1015_ADDRESS, .i2c_speed_hz = 400000, .rdy_gpio = CONFIG_DRIVER_ADS1015_RDY_PIN, - .bus_handle = adc_bus.handle, + .bus_handle = bus.handle, .adc_data_rate = CONFIG_DRIVER_ADS1015_DATA_RATE, }; ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); @@ -88,15 +80,9 @@ void app_main(void) { ESP_ERROR_CHECK(driver_socket_init(&socket_handle, &socket_config)); - motorhat_set_motor_direction(&motorhat, MOTORHAT_MOTOR1, - MOTORHAT_DIRECTION_FORWARD); - motorhat_set_motor_speed(&motorhat, MOTORHAT_MOTOR1, 200); - - vTaskDelay(1000 / portTICK_PERIOD_MS); - motorhat_set_motor_speed(&motorhat, MOTORHAT_MOTOR1, - MOTORHAT_DIRECTION_RELEASE); + int pulse_count; while (1) { - vTaskDelay(pdMS_TO_TICKS(10000)); - } + esp_err_t err = encoder_get_raw_count( + } } From 83c5861222f78ec0ee209143c259af01c488d383 Mon Sep 17 00:00:00 2001 From: frey808 Date: Fri, 20 Mar 2026 18:59:18 -0400 Subject: [PATCH 02/16] set up event group --- components/driver_socket/driver_socket_api.c | 4 ++ components/limit_switch/CMakeLists.txt | 2 +- .../limit_switch/include/limit_switch.h | 2 +- components/limit_switch/limit_switch.c | 34 ++--------- components/signal_bus/CMakeLists.txt | 4 ++ components/signal_bus/include/signal_bus.h | 36 ++++++++++++ components/signal_bus/signal_bus.c | 18 ++++++ main/CMakeLists.txt | 2 +- main/esp_driver.c | 58 ++++++++++++++++--- 9 files changed, 118 insertions(+), 42 deletions(-) create mode 100644 components/signal_bus/CMakeLists.txt create mode 100644 components/signal_bus/include/signal_bus.h create mode 100644 components/signal_bus/signal_bus.c diff --git a/components/driver_socket/driver_socket_api.c b/components/driver_socket/driver_socket_api.c index f635687..eaefc3f 100644 --- a/components/driver_socket/driver_socket_api.c +++ b/components/driver_socket/driver_socket_api.c @@ -60,21 +60,25 @@ esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN: driver_socket_api_polar_pan_payload_t *polar_pan_payload = (driver_socket_api_polar_pan_payload_t *)&wrapper->payload_head; driver_socket_api_polar_pan_payload_swap_endianess(polar_pan_payload); + // TODO add actual motor control ESP_LOGI(TAG, "Processing POLAR_PAN command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, polar_pan_payload->delay_ms, polar_pan_payload->time_ms); break; case DRIVER_SOCKET_API_CMD_ID_HOME: driver_socket_api_home_payload_t *home_payload = (driver_socket_api_home_payload_t *)&wrapper->payload_head; driver_socket_api_home_payload_swap_endianess(home_payload); + // TODO add actual motor control ESP_LOGI(TAG, "Processing HOME command: delay_ms=%d", home_payload->delay_ms); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_START: driver_socket_api_polar_pan_start_payload_t *polar_pan_start_payload = (driver_socket_api_polar_pan_start_payload_t *)&wrapper->payload_head; // No endianess swap needed for int8_t fields + // TODO add actual motor control ESP_LOGI(TAG, "Processing POLAR_PAN_START command: delta_azimuth=%d, delta_altitude=%d", polar_pan_start_payload->delta_azimuth, polar_pan_start_payload->delta_altitude); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_STOP: + // TODO add actual motor control ESP_LOGI(TAG, "Processing POLAR_PAN_STOP command"); break; default: diff --git a/components/limit_switch/CMakeLists.txt b/components/limit_switch/CMakeLists.txt index 357cf8f..ec3086a 100644 --- a/components/limit_switch/CMakeLists.txt +++ b/components/limit_switch/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS "limit_switch.c" INCLUDE_DIRS "include" - REQUIRES esp_driver_gpio + REQUIRES esp_driver_gpio signal_bus ) diff --git a/components/limit_switch/include/limit_switch.h b/components/limit_switch/include/limit_switch.h index 9cac676..68bf19c 100644 --- a/components/limit_switch/include/limit_switch.h +++ b/components/limit_switch/include/limit_switch.h @@ -25,6 +25,6 @@ typedef struct { * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument */ -esp_err_t limit_switch_init(const limit_switch_config_t *config); +esp_err_t limit_switch_init(const limit_switch_config_t *config, EventGroupHandle_t events); #endif // _LIMIT_SWITCH_H \ No newline at end of file diff --git a/components/limit_switch/limit_switch.c b/components/limit_switch/limit_switch.c index fe0a462..8d2a8bf 100644 --- a/components/limit_switch/limit_switch.c +++ b/components/limit_switch/limit_switch.c @@ -1,6 +1,7 @@ #include "limit_switch.h" #include "esp_log.h" #include "freertos/FreeRTOS.h" +#include "signal_bus.h" #define TAG "limit_switch" @@ -9,33 +10,11 @@ static TaskHandle_t limit_switch_task_handle = NULL; static void IRAM_ATTR limit_switch_isr(void *arg){ BaseType_t higher_priority_task_woken = pdFALSE; - if (limit_switch_task_handle){ - xTaskNotifyFromISR( - limit_switch_task_handle, - 0, - eNoAction, - &higher_priority_task_woken - ); - } - - if (higher_priority_task_woken) { - portYIELD_FROM_ISR(); - } -} - -void limit_switch_task(void *arg){ - while (1) { - // Wait until GPIO interrupt fires - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - - ESP_LOGW(TAG,"Limit switch hit!"); - // TODO: React to limit switch - } + xEventGroupSetBitsFromISR(g_motor_events, LIMIT_MOTOR_0, &higher_priority_task_woken); + portYIELD_FROM_ISR(higher_priority_task_woken); } -esp_err_t limit_switch_init(const limit_switch_config_t *config) { - ESP_LOGI(TAG, "Initializing limit switches..."); - +esp_err_t limit_switch_init(const limit_switch_config_t *config, EventGroupHandle_t events){ if (!config) { return ESP_ERR_INVALID_ARG; } @@ -51,11 +30,6 @@ esp_err_t limit_switch_init(const limit_switch_config_t *config) { ESP_ERROR_CHECK(gpio_config(&io_conf)); ESP_ERROR_CHECK(gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, NULL)); - - // Start waiting task - xTaskCreate(limit_switch_task, "limit_switch_task", 4096, NULL, 7, &limit_switch_task_handle); - - ESP_LOGI(TAG, "limit_switch initialized"); return ESP_OK; } \ No newline at end of file diff --git a/components/signal_bus/CMakeLists.txt b/components/signal_bus/CMakeLists.txt new file mode 100644 index 0000000..49e88e9 --- /dev/null +++ b/components/signal_bus/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRCS "signal_bus.c" + INCLUDE_DIRS "include" +) \ No newline at end of file diff --git a/components/signal_bus/include/signal_bus.h b/components/signal_bus/include/signal_bus.h new file mode 100644 index 0000000..27bb26f --- /dev/null +++ b/components/signal_bus/include/signal_bus.h @@ -0,0 +1,36 @@ +#ifndef _SIGNAL_BUS_H +#define _SIGNAL_BUS_H + +#include "freertos/FreeRTOS.h" +#include "freertos/event_groups.h" +#include "esp_err.h" + +// One bit per motor limit switch +#define LIMIT_MOTOR_0 (1 << 0) +#define LIMIT_MOTOR_1 (1 << 1) +#define LIMIT_MOTOR_2 (1 << 2) +#define LIMIT_MOTOR_3 (1 << 3) +#define LIMIT_MOTOR_4 (1 << 4) +#define LIMIT_MOTOR_5 (1 << 5) +#define LIMIT_MOTOR_6 (1 << 6) +#define ESTOP_OVERCURRENT (1 << 7) + +#define ESTOP_ANY (ESTOP_OVERCURRENT) +#define LIMIT_ANY (0x7F) // bits 0-6 +#define FAULT_ANY (LIMIT_ANY | ESTOP_ANY) + +extern EventGroupHandle_t g_motor_events; + +/** + * @brief Initialize the signal bus + * + * This function initializes the signal bus and sets up the necessary resources. + * + * @return + * - ESP_OK: Success + * - ESP_ERR_INVALID_ARG: Invalid argument + * - ESP_FAIL: Failed to initialize signal bus + */ +esp_err_t signal_bus_init(); + +#endif // _SIGNAL_BUS_H \ No newline at end of file diff --git a/components/signal_bus/signal_bus.c b/components/signal_bus/signal_bus.c new file mode 100644 index 0000000..5a06f7b --- /dev/null +++ b/components/signal_bus/signal_bus.c @@ -0,0 +1,18 @@ +#include "signal_bus.h" +#include "esp_log.h" + +#define TAG "signal_bus" + +EventGroupHandle_t g_motor_events = NULL; + +esp_err_t signal_bus_init(void) { + ESP_LOGI(TAG, "Initializing signal bus..."); + + g_motor_events = xEventGroupCreate(); + + if (g_motor_events == NULL) { + return ESP_FAIL; + } else{ + return ESP_OK; + } +} diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index b3d7aba..ae02928 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,2 +1,2 @@ idf_component_register(SRCS "esp_driver.c" - INCLUDE_DIRS "." PRIV_REQUIRES i2c_bus motorhat driver_socket driver_wifi ads1015 encoder limit_switch) + INCLUDE_DIRS "." PRIV_REQUIRES i2c_bus motorhat driver_socket driver_wifi ads1015 encoder limit_switch signal_bus) diff --git a/main/esp_driver.c b/main/esp_driver.c index ad50ecf..936c3f4 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -7,9 +7,13 @@ #include "ads1015.h" #include "limit_switch.h" +#include "encoder.h" #include "motorhat.h" +#include "signal_bus.h" #include "nvs_flash.h" +#include "driver/gpio.h" + #define TAG "MAIN" void app_main(void) { @@ -17,6 +21,9 @@ void app_main(void) { // Install GPIO interrupt service ESP_ERROR_CHECK(gpio_install_isr_service(0)); + // Initialize signal bus + signal_bus_init(); + // Initialize NVS esp_err_t ret = nvs_flash_init(); if (ret == ESP_ERR_NVS_NO_FREE_PAGES || @@ -24,14 +31,10 @@ void app_main(void) { ESP_ERROR_CHECK(nvs_flash_erase()); ret = nvs_flash_init(); } - ESP_ERROR_CHECK(ret); - limit_switch_config_t limit_switch_config = { - .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, - }; - ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config)); + // Initialize I2C bus i2c_bus_t bus; i2c_bus_config_t bus_config = { .port = I2C_NUM_0, @@ -40,6 +43,15 @@ void app_main(void) { }; ESP_ERROR_CHECK(i2c_bus_init(&bus, &bus_config)); + + // Initialize limit switches + limit_switch_config_t limit_switch_config = { + .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, + }; + ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config, g_motor_events)); + + + // Initialize current sensing ads1015_handle_t ads; ads1015_config_t ads_config = { .i2c_addr = CONFIG_DRIVER_ADS1015_ADDRESS, @@ -48,10 +60,23 @@ void app_main(void) { .bus_handle = bus.handle, .adc_data_rate = CONFIG_DRIVER_ADS1015_DATA_RATE, }; - ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); + // ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); - motorhat_handle_t motorhat; + // Initialize encoders + encoder_handle_t encoder; + encoder_config_t encoder_config = { + .P0_pin = CONFIG_ENCODER_0_P0_PIN, + .P1_pin = CONFIG_ENCODER_0_P1_PIN, + .resolution = CONFIG_ENCODER_0_RESOLUTION, + .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, + .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, + }; + ESP_ERROR_CHECK(encoder_init(&encoder, &encoder_config)); + + + // Initialize motor controller + motorhat_handle_t motorhat; motorhat_config_t motorhat_config = { .pca9685_config = { @@ -61,9 +86,10 @@ void app_main(void) { .bus_handle = bus.handle, }, }; + // ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config)); - ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config)); + // Initialize Wi-Fi and socket connection driver_wifi_config_t wifi_config = { .ssid = CONFIG_DRIVER_WIFI_SSID, .password = CONFIG_DRIVER_WIFI_PASSWORD, @@ -80,9 +106,23 @@ void app_main(void) { ESP_ERROR_CHECK(driver_socket_init(&socket_handle, &socket_config)); + int pulse_count; while (1) { - esp_err_t err = encoder_get_raw_count( + esp_err_t err = encoder_get_raw_count(&encoder, &pulse_count); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); + } + + // connect limit switches to stop motor in motorhat control + + // overcurrent estop + + // connect socket commands to motorhat control + + vTaskDelay(pdMS_TO_TICKS(1000)); + + ESP_LOGI(TAG, "Event group: %d", xEventGroupGetBits(g_motor_events)); } } From 3515ec7a313abc7206636aa858ce8c1fbcfce830 Mon Sep 17 00:00:00 2001 From: frey808 Date: Fri, 20 Mar 2026 21:18:26 -0400 Subject: [PATCH 03/16] connected ads to signal bus --- components/ads1015/CMakeLists.txt | 2 +- components/ads1015/ads1015.c | 20 +++++++------------- components/ads1015/include/ads1015.h | 4 +++- main/esp_driver.c | 3 ++- 4 files changed, 13 insertions(+), 16 deletions(-) diff --git a/components/ads1015/CMakeLists.txt b/components/ads1015/CMakeLists.txt index 0d8c30a..802ad45 100644 --- a/components/ads1015/CMakeLists.txt +++ b/components/ads1015/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS "ads1015.c" INCLUDE_DIRS "include" - REQUIRES esp_driver_i2c i2c_bus esp_driver_gpio + REQUIRES esp_driver_i2c i2c_bus esp_driver_gpio signal_bus ) \ No newline at end of file diff --git a/components/ads1015/ads1015.c b/components/ads1015/ads1015.c index 0dad4d0..5b0d48e 100644 --- a/components/ads1015/ads1015.c +++ b/components/ads1015/ads1015.c @@ -3,6 +3,7 @@ #include "esp_err.h" #include "esp_log.h" #include "freertos/FreeRTOS.h" +#include "signal_bus.h" #define TAG "ADS1015" @@ -28,6 +29,7 @@ static void IRAM_ATTR ads1015_isr(void *arg){ void adc_task(void *arg){ ads1015_handle_t *handle = (ads1015_handle_t *)arg; uint16_t config_reg = handle->config_reg | (1 << ADS1015_OS_BIT); // Modify so that writing will start conversions + EventGroupHandle_t events = handle->events; bool mux_state = true; // Differential input to read (true = A2-A3, false = A0-A1) ads1015_write_register(handle, ADS1015_CONFIG, config_reg); // Start conversions @@ -39,7 +41,9 @@ void adc_task(void *arg){ uint16_t raw; if (ads1015_read_register(handle, ADS1015_CONVERSION, &raw) == ESP_OK){ int16_t value = ((int16_t)raw) >> 4; - ads1015_check_current(value, mux_state); + if (value >= CONFIG_ADS1015_HIGH_THRESH || value <= CONFIG_ADS1015_LOW_THRESH){ + xEventGroupSetBits(events, ESTOP_OVERCURRENT); + } } // Switch MUX inputs and start next conversion @@ -52,9 +56,7 @@ void adc_task(void *arg){ } } -esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config) { - ESP_LOGI(TAG, "Initializing ADS1015..."); - +esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config, EventGroupHandle_t events) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } @@ -99,6 +101,7 @@ esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config) } handle->config_reg = config_reg; + handle->events = events; // Configure alert GPIO and interrupt service gpio_config_t io_conf = { @@ -126,15 +129,6 @@ esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config) return ESP_OK; } -esp_err_t ads1015_check_current(int16_t value, bool mux_state){ - if (value >= CONFIG_ADS1015_HIGH_THRESH || value <= CONFIG_ADS1015_LOW_THRESH){ - ESP_LOGW(TAG, "Threshhold exceeded: %i (mux: %s)", value, (mux_state ? ("A2-A3") - : ("A0-A1"))); - // Trigger E-stop - } - return ESP_OK; -} - esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t *data) { if (handle == NULL || data == NULL) { return ESP_ERR_INVALID_ARG; diff --git a/components/ads1015/include/ads1015.h b/components/ads1015/include/ads1015.h index 0f9d696..51eb270 100644 --- a/components/ads1015/include/ads1015.h +++ b/components/ads1015/include/ads1015.h @@ -142,6 +142,7 @@ typedef struct { typedef struct { i2c_master_dev_handle_t dev_handle; /**< I2C device handle */ uint16_t config_reg; /**< ADS1015 config register values */ + EventGroupHandle_t events; /**< Event group handle for signaling overcurrent events */ } ads1015_handle_t; /** @@ -152,6 +153,7 @@ typedef struct { * * @param[out] handle Pointer to ADS1015 handle structure * @param[in] config Pointer to configuration structure + * @param[in] events Event group handle for signaling overcurrent events * * @return * - ESP_OK: Success @@ -159,7 +161,7 @@ typedef struct { * frequency) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config); +esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config, EventGroupHandle_t events); /** * @brief Check the ADC value against the configured threshholds and trigger E-stop if it exceeds them diff --git a/main/esp_driver.c b/main/esp_driver.c index 936c3f4..a8ba4c0 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -60,7 +60,7 @@ void app_main(void) { .bus_handle = bus.handle, .adc_data_rate = CONFIG_DRIVER_ADS1015_DATA_RATE, }; - // ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); + ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config, g_motor_events)); // Initialize encoders @@ -124,5 +124,6 @@ void app_main(void) { vTaskDelay(pdMS_TO_TICKS(1000)); ESP_LOGI(TAG, "Event group: %d", xEventGroupGetBits(g_motor_events)); + xEventGroupClearBits(g_motor_events, FAULT_ANY); } } From 38e6c86a3337758a6efd4182603b4e0b3714efb6 Mon Sep 17 00:00:00 2001 From: frey808 Date: Fri, 20 Mar 2026 22:18:33 -0400 Subject: [PATCH 04/16] faults stop motors --- components/limit_switch/limit_switch.c | 2 - components/motorhat/CMakeLists.txt | 2 +- components/motorhat/include/motorhat.h | 26 ++++- components/motorhat/motorhat.c | 113 +++++++++++++++++---- components/signal_bus/include/signal_bus.h | 16 +-- main/esp_driver.c | 13 +-- 6 files changed, 127 insertions(+), 45 deletions(-) diff --git a/components/limit_switch/limit_switch.c b/components/limit_switch/limit_switch.c index 8d2a8bf..c116ccf 100644 --- a/components/limit_switch/limit_switch.c +++ b/components/limit_switch/limit_switch.c @@ -5,8 +5,6 @@ #define TAG "limit_switch" -static TaskHandle_t limit_switch_task_handle = NULL; - static void IRAM_ATTR limit_switch_isr(void *arg){ BaseType_t higher_priority_task_woken = pdFALSE; diff --git a/components/motorhat/CMakeLists.txt b/components/motorhat/CMakeLists.txt index 5475637..149f993 100644 --- a/components/motorhat/CMakeLists.txt +++ b/components/motorhat/CMakeLists.txt @@ -1,2 +1,2 @@ idf_component_register(SRCS "motorhat.c" "pca9685.c" - INCLUDE_DIRS "include" REQUIRES esp_driver_i2c i2c_bus) + INCLUDE_DIRS "include" REQUIRES esp_driver_i2c i2c_bus signal_bus) diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index 01de8f4..9ca21dc 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -3,6 +3,8 @@ #include "pca9685.h" #include +#include "freertos/FreeRTOS.h" +#include "freertos/event_groups.h" #define DEFAULT_FREQUENCY_HZ 1526.0f @@ -48,7 +50,9 @@ typedef struct { * @brief Motor HAT device handle */ typedef struct { - pca9685_handle_t pca9685; + pca9685_handle_t pca9685; /**< Handle for the underlying PCA9685 controller */ + EventGroupHandle_t events; /**< Event group handle for motor fault signaling */ + EventBits_t stop_bits; /**< Which bits trigger motor stop */ } motorhat_handle_t; /** @@ -76,6 +80,7 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { * * @param[out] handle Pointer to Motor HAT handle structure * @param[in] config Pointer to configuration structure containing PCA9685 settings + * @param[in] events Event group handle for signaling motor faults * * @return * - ESP_OK: Success @@ -85,8 +90,7 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { * @note The handle->pca9685 pointer must be allocated and point to a valid * pca9685_handle_t structure before calling this function */ -esp_err_t motorhat_init(motorhat_handle_t *handle, - const motorhat_config_t *config); +esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *config, EventGroupHandle_t events); /** * @brief Set motor speed @@ -101,6 +105,7 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, * @return * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or speed > 4096) + * - ESP_ERR_INVALID_STATE: Motor is in fault state (stop bits set) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations * * @note For typical usage with 0-255 speed range, scale your value: @@ -126,6 +131,7 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, * @return * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or invalid direction) + * - ESP_ERR_INVALID_STATE: Motor is in fault state (stop bits set) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations * * @note Setting direction does not affect speed. Set speed separately using @@ -135,4 +141,18 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, motorhat_motor_t motor, motorhat_direction_t direction); +/** + * @brief Emergency stop all motors + * + * Immediately stops all motors and releases control signals. + * + * @param[in] handle Pointer to Motor HAT handle + * + * @return + * - ESP_OK: Success + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) + */ + +esp_err_t motorhat_emergency_stop(motorhat_handle_t *handle); + #endif // _MOTORHAT_H_ diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index d66a612..c698e76 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -1,13 +1,44 @@ #include "motorhat.h" #include "esp_err.h" +#include "esp_log.h" +#include "signal_bus.h" -esp_err_t motorhat_init(motorhat_handle_t *handle, - const motorhat_config_t *config) { +#define TAG "motorhat" + +void motor_stop_task(void *args) { + motorhat_handle_t *handle = (motorhat_handle_t *)args; + + while (1) { + // Block indefinitely until any stop bit is set + xEventGroupWaitBits(handle->events, + handle->stop_bits, + pdFALSE, // don't clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); + + motorhat_emergency_stop(handle); + + // Wait until all stop bits are cleared before watching again + xEventGroupWaitBits(handle->events, + handle->stop_bits, + pdFALSE, + pdTRUE, // all bits (AND) + portMAX_DELAY); + } +} + +esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *config, EventGroupHandle_t events) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } - return pca9685_init(&handle->pca9685, &config->pca9685_config); + handle->events = events; + handle->stop_bits = FAULT_ANY; + + xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); + + // return pca9685_init(&handle->pca9685, &config->pca9685_config); + return ESP_OK; } esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, @@ -17,13 +48,19 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, return ESP_ERR_INVALID_ARG; } + if (xEventGroupGetBits(handle->events) & handle->stop_bits) { + ESP_LOGW(TAG, "Motor %d in fault state, cannot set speed", motor); + return ESP_ERR_INVALID_STATE; + } + if (speed > PCA9685_PWM_MAX) { return ESP_ERR_INVALID_ARG; } const motorhat_motor_channels_t *channels = &motor_channels[motor]; - return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); + // return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); + return ESP_OK; } esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, @@ -34,34 +71,47 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, return ESP_ERR_INVALID_ARG; } + if (xEventGroupGetBits(handle->events) & handle->stop_bits) { + ESP_LOGW(TAG, "Motor %d in fault state, cannot set direction", motor); + return ESP_ERR_INVALID_STATE; + } + const motorhat_motor_channels_t *channels = &motor_channels[motor]; esp_err_t err; switch (direction) { case MOTORHAT_DIRECTION_FORWARD: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + // if (err != ESP_OK) + // return err; + // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + ESP_LOGI(TAG, "Set motor %d direction to FORWARD", motor); + err = ESP_OK; break; case MOTORHAT_DIRECTION_BACKWARD: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + // if (err != ESP_OK) + // return err; + // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + ESP_LOGI(TAG, "Set motor %d direction to BACKWARD", motor); + err = ESP_OK; break; case MOTORHAT_DIRECTION_BRAKE: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + // if (err != ESP_OK) + // return err; + // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + ESP_LOGI(TAG, "Set motor %d direction to BRAKE", motor); + err = ESP_OK; break; case MOTORHAT_DIRECTION_RELEASE: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + // if (err != ESP_OK) + // return err; + // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + ESP_LOGI(TAG, "Set motor %d direction to RELEASE", motor); + err = ESP_OK; break; default: return ESP_ERR_INVALID_ARG; @@ -69,3 +119,26 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, return err; } + +esp_err_t motorhat_emergency_stop(motorhat_handle_t *handle) { + if (handle == NULL) { + return ESP_ERR_INVALID_ARG; + } + + esp_err_t first_err = ESP_OK; + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + const motorhat_motor_channels_t *ch = &motor_channels[m]; + + // Stop PWM cycle and release both control lines + ESP_LOGI(TAG, "Emergency stopping motor %d", m); + // esp_err_t err = pca9685_set_duty_cycle(&handle->pca9685, ch->pwm_channel, 0); + // if (err != ESP_OK && first_err == ESP_OK) first_err = err; + + // err = pca9685_digital_write(&handle->pca9685, ch->in1_channel, false); + // if (err != ESP_OK && first_err == ESP_OK) first_err = err; + + // err = pca9685_digital_write(&handle->pca9685, ch->in2_channel, false); + // if (err != ESP_OK && first_err == ESP_OK) first_err = err; + } + return first_err; +} diff --git a/components/signal_bus/include/signal_bus.h b/components/signal_bus/include/signal_bus.h index 27bb26f..e9167c0 100644 --- a/components/signal_bus/include/signal_bus.h +++ b/components/signal_bus/include/signal_bus.h @@ -5,14 +5,14 @@ #include "freertos/event_groups.h" #include "esp_err.h" -// One bit per motor limit switch -#define LIMIT_MOTOR_0 (1 << 0) -#define LIMIT_MOTOR_1 (1 << 1) -#define LIMIT_MOTOR_2 (1 << 2) -#define LIMIT_MOTOR_3 (1 << 3) -#define LIMIT_MOTOR_4 (1 << 4) -#define LIMIT_MOTOR_5 (1 << 5) -#define LIMIT_MOTOR_6 (1 << 6) +// Motor limit switch and fault event bits +#define LIMIT_MOTOR_0 (1 << 0) +#define LIMIT_MOTOR_1 (1 << 1) +#define LIMIT_MOTOR_2 (1 << 2) +#define LIMIT_MOTOR_3 (1 << 3) +#define LIMIT_MOTOR_4 (1 << 4) +#define LIMIT_MOTOR_5 (1 << 5) +#define LIMIT_MOTOR_6 (1 << 6) #define ESTOP_OVERCURRENT (1 << 7) #define ESTOP_ANY (ESTOP_OVERCURRENT) diff --git a/main/esp_driver.c b/main/esp_driver.c index a8ba4c0..b9441c7 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -86,7 +86,7 @@ void app_main(void) { .bus_handle = bus.handle, }, }; - // ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config)); + ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config, g_motor_events)); // Initialize Wi-Fi and socket connection @@ -114,16 +114,7 @@ void app_main(void) { if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); } - - // connect limit switches to stop motor in motorhat control - - // overcurrent estop - - // connect socket commands to motorhat control - + vTaskDelay(pdMS_TO_TICKS(1000)); - - ESP_LOGI(TAG, "Event group: %d", xEventGroupGetBits(g_motor_events)); - xEventGroupClearBits(g_motor_events, FAULT_ANY); } } From 3f469dfee6879776b666209ea7348b85bfb94fc7 Mon Sep 17 00:00:00 2001 From: frey808 Date: Sun, 22 Mar 2026 12:55:32 -0400 Subject: [PATCH 05/16] connected socket commands to motorhat functions --- components/driver_socket/driver_socket.c | 11 +++- components/driver_socket/driver_socket_api.c | 28 +++++---- .../driver_socket/include/driver_socket.h | 5 +- .../driver_socket/include/driver_socket_api.h | 24 ++++++++ components/motorhat/include/motorhat.h | 57 +++++++++++++++++++ components/motorhat/motorhat.c | 27 +++++++++ main/esp_driver.c | 17 ++++-- 7 files changed, 151 insertions(+), 18 deletions(-) diff --git a/components/driver_socket/driver_socket.c b/components/driver_socket/driver_socket.c index 616738b..e0b8365 100644 --- a/components/driver_socket/driver_socket.c +++ b/components/driver_socket/driver_socket.c @@ -191,7 +191,16 @@ void driver_socket_task(void *arg) { } esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, - const driver_socket_config_t *config) { + const driver_socket_config_t *config, const driver_socket_api_motor_interface_t *motor_interface) { + + if (socket_handle == NULL || config == NULL || motor_interface == NULL) { + return ESP_ERR_INVALID_ARG; + } + + esp_err_t ret = driver_socket_api_init(motor_interface); + if (ret != ESP_OK) { + return ret; + } task_args_t *task_args = malloc(sizeof(task_args_t)); if (task_args == NULL) { diff --git a/components/driver_socket/driver_socket_api.c b/components/driver_socket/driver_socket_api.c index eaefc3f..746b41a 100644 --- a/components/driver_socket/driver_socket_api.c +++ b/components/driver_socket/driver_socket_api.c @@ -5,8 +5,9 @@ #define TAG "driver_socket_api" -static void -driver_socket_api_header_swap_endianess(driver_socket_api_header_t *header) { +static driver_socket_api_motor_interface_t s_motor_interface; + +static void driver_socket_api_header_swap_endianess(driver_socket_api_header_t *header) { header->msg_id = be32toh(header->msg_id); header->cmd_id = be16toh(header->cmd_id); header->len = be16toh(header->len); @@ -36,6 +37,14 @@ static esp_err_t validate_checksum(uint8_t *data, size_t len, uint8_t checksum) return (calculated_checksum == checksum) ? ESP_OK : ESP_ERR_INVALID_CRC; } +esp_err_t driver_socket_api_init(const driver_socket_api_motor_interface_t *motor_interface) { + if (motor_interface == NULL) { + return ESP_ERR_INVALID_ARG; + } + s_motor_interface = *motor_interface; + return ESP_OK; +} + esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { ESP_LOGI(TAG, "Received data size: %d", buffer_size); ESP_LOG_BUFFER_HEX_LEVEL(TAG, buffer, buffer_size, ESP_LOG_INFO); @@ -60,26 +69,21 @@ esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN: driver_socket_api_polar_pan_payload_t *polar_pan_payload = (driver_socket_api_polar_pan_payload_t *)&wrapper->payload_head; driver_socket_api_polar_pan_payload_swap_endianess(polar_pan_payload); - // TODO add actual motor control - ESP_LOGI(TAG, "Processing POLAR_PAN command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", - polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, polar_pan_payload->delay_ms, polar_pan_payload->time_ms); + s_motor_interface.polar_pan(polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, + polar_pan_payload->delay_ms, polar_pan_payload->time_ms); break; case DRIVER_SOCKET_API_CMD_ID_HOME: driver_socket_api_home_payload_t *home_payload = (driver_socket_api_home_payload_t *)&wrapper->payload_head; driver_socket_api_home_payload_swap_endianess(home_payload); - // TODO add actual motor control - ESP_LOGI(TAG, "Processing HOME command: delay_ms=%d", home_payload->delay_ms); + s_motor_interface.home(home_payload->delay_ms); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_START: driver_socket_api_polar_pan_start_payload_t *polar_pan_start_payload = (driver_socket_api_polar_pan_start_payload_t *)&wrapper->payload_head; // No endianess swap needed for int8_t fields - // TODO add actual motor control - ESP_LOGI(TAG, "Processing POLAR_PAN_START command: delta_azimuth=%d, delta_altitude=%d", - polar_pan_start_payload->delta_azimuth, polar_pan_start_payload->delta_altitude); + s_motor_interface.polar_pan_start(polar_pan_start_payload->delta_azimuth, polar_pan_start_payload->delta_altitude); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_STOP: - // TODO add actual motor control - ESP_LOGI(TAG, "Processing POLAR_PAN_STOP command"); + s_motor_interface.polar_pan_stop(); break; default: ESP_LOGW(TAG, "Unknown command ID: %d", wrapper->header.cmd_id); diff --git a/components/driver_socket/include/driver_socket.h b/components/driver_socket/include/driver_socket.h index d047ac9..a67b423 100644 --- a/components/driver_socket/include/driver_socket.h +++ b/components/driver_socket/include/driver_socket.h @@ -3,6 +3,7 @@ #include "freertos/FreeRTOS.h" #include "sys/socket.h" +#include "driver_socket_api.h" /** @@ -33,13 +34,15 @@ typedef struct { * * @param[out] socket_handle Socket handle to be initialized by this function * @param[in] config Socket configuration + * @param[in] motor_interface Motor control interface to be used by driver_socket_api * @return * ESP_OK : Socket driver initialized successfully + * ESP_ERR_INVALID_ARG : Invalid argument * ESP_ERR_NO_MEM : Unable to allocate memory for task arguments or * server_ready semaphore */ esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, - const driver_socket_config_t *config); + const driver_socket_config_t *config, const driver_socket_api_motor_interface_t *motor_interface); /** * @brief Utility to log socket errors diff --git a/components/driver_socket/include/driver_socket_api.h b/components/driver_socket/include/driver_socket_api.h index 73ad680..0d294a8 100644 --- a/components/driver_socket/include/driver_socket_api.h +++ b/components/driver_socket/include/driver_socket_api.h @@ -52,6 +52,30 @@ typedef struct { int8_t delta_altitude; /** Requested change in altitude */ } __attribute__((packed)) driver_socket_api_polar_pan_start_payload_t; +typedef struct { + esp_err_t (*polar_pan)(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms); + esp_err_t (*polar_pan_start)(int8_t delta_azimuth, int8_t delta_altitude); + esp_err_t (*polar_pan_stop)(void); + esp_err_t (*home)(uint16_t delay_ms); +} driver_socket_api_motor_interface_t; + +/** + * @brief Initializes the socket API module with the provided motor control interface. + * + * This function must be called before processing any incoming messages with + * driver_socket_api_process(). It sets up the internal function pointers for + * motor control commands that will be invoked when corresponding messages are + * received. + * + * @param[in] motor_interface Pointer to a structure containing function pointers + * for motor control operations. + * + * @return + * ESP_OK : Initialization successful + * ESP_ERR_INVALID_ARG : Invalid argument (e.g., NULL pointer) + */ +esp_err_t driver_socket_api_init(const driver_socket_api_motor_interface_t *motor_interface); /** * @brief Process incoming socket API messages. This function should be called diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index 9ca21dc..ccce067 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -92,6 +92,63 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { */ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *config, EventGroupHandle_t events); +/** + * @brief Polar pan command + * + * Triggers a polar pan movement based on the specified azimuth and altitude deltas, with + * timing parameters for delay and duration. This is a high-level command that can be + * implemented to control multiple motors in coordination to achieve the desired pan movement. + * + * @param[in] delta_azimuth Azimuth delta for the pan movement + * @param[in] delta_altitude Altitude delta for the pan movement + * @param[in] delay_ms Delay before starting the movement + * @param[in] time_ms Duration of the movement + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + */ +esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms); + +/** + * @brief Polar pan start command + * + * Starts a continuous pan movement that will run until a stop command is received. + * + * @param[in] delta_azimuth Azimuth delta for the pan movement + * @param[in] delta_altitude Altitude delta for the pan movement + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + */ +esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude); + +/** + * @brief Polar pan stop command + * + * Stops any ongoing polar pan movement initiated by polar_pan_start. + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + */ +esp_err_t motorhat_polar_pan_stop(void); + +/** + * @brief Home command + * + * Moves the motors to a predefined home position after an optional delay. + * + * @param[in] delay_ms Delay before starting the homing movement + * + * @return + * - ESP_OK: Success + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + */ +esp_err_t motorhat_home(uint16_t delay_ms); + /** * @brief Set motor speed * diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index c698e76..55200b7 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -41,6 +41,33 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf return ESP_OK; } +esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms) { + // motorhat_set_motor_direction(); + ESP_LOGI(TAG, "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", + delta_azimuth, delta_altitude, delay_ms, time_ms); + return ESP_OK; +} + +esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude) { + // motorhat_set_motor_direction(); + ESP_LOGI(TAG, "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", + delta_azimuth, delta_altitude); + return ESP_OK; +} + +esp_err_t motorhat_polar_pan_stop(void) { + // motorhat_set_motor_direction(); + ESP_LOGI(TAG, "Received polar pan stop command"); + return ESP_OK; +} + +esp_err_t motorhat_home(uint16_t delay_ms) { + // motorhat_set_motor_direction(); + ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); + return ESP_OK; +} + esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, motorhat_motor_t motor, uint16_t speed) { if (handle == NULL || motor < MOTORHAT_MOTOR1 || diff --git a/main/esp_driver.c b/main/esp_driver.c index b9441c7..922b8d2 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -10,12 +10,20 @@ #include "encoder.h" #include "motorhat.h" #include "signal_bus.h" +#include "driver_socket_api.h" #include "nvs_flash.h" #include "driver/gpio.h" #define TAG "MAIN" +static const driver_socket_api_motor_interface_t motor_interface = { + .polar_pan = motorhat_polar_pan, + .polar_pan_start = motorhat_polar_pan_start, + .polar_pan_stop = motorhat_polar_pan_stop, + .home = motorhat_home, +}; + void app_main(void) { // Install GPIO interrupt service @@ -104,7 +112,7 @@ void app_main(void) { .port = CONFIG_DRIVER_SERVER_PORT, }; - ESP_ERROR_CHECK(driver_socket_init(&socket_handle, &socket_config)); + ESP_ERROR_CHECK(driver_socket_init(&socket_handle, &socket_config, &motor_interface)); int pulse_count; @@ -113,8 +121,9 @@ void app_main(void) { esp_err_t err = encoder_get_raw_count(&encoder, &pulse_count); if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); + vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop on error } - - vTaskDelay(pdMS_TO_TICKS(1000)); - } + + // This loop can also be used to add periodic tasks like reading sensors, or checking limit switches. + // However, it must not terminate or structs initialized here will disappear from stack memory, breaking modules that use them. } From ed45a5be68dd53c906b26f084ffea2c93fb1fdb4 Mon Sep 17 00:00:00 2001 From: frey808 Date: Sun, 22 Mar 2026 15:02:08 -0400 Subject: [PATCH 06/16] made polar pan start and stop --- components/motorhat/include/motorhat.h | 20 ++++++++ components/motorhat/motorhat.c | 68 +++++++++++++++++++++----- main/Kconfig.projbuild | 18 +++---- main/esp_driver.c | 5 +- 4 files changed, 87 insertions(+), 24 deletions(-) diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index ccce067..fba0f05 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -8,6 +8,9 @@ #define DEFAULT_FREQUENCY_HZ 1526.0f +/** + * @brief Motor enumeration + */ typedef enum { MOTORHAT_MOTOR1 = 0, MOTORHAT_MOTOR2, @@ -16,6 +19,23 @@ typedef enum { MOTORHAT_NUM_MOTORS } motorhat_motor_t; +/** + * @brief Axis enumeration + */ +typedef enum { + MOTORHAT_AXIS_AZIMUTH = 0, + MOTORHAT_AXIS_ALTITUDE, + MOTORHAT_NUM_AXES +} motorhat_axis_t; + +/** + * @brief Mapping of axes to motors + */ +static const motorhat_motor_t axis_motor[MOTORHAT_NUM_AXES] = { + [MOTORHAT_AXIS_AZIMUTH] = MOTORHAT_MOTOR1, + [MOTORHAT_AXIS_ALTITUDE] = MOTORHAT_MOTOR2, +}; + /** * @brief Motor direction and state control */ diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 55200b7..62897b9 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -5,6 +5,16 @@ #define TAG "motorhat" +#define POLAR_PAN_SPEED PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED) + +static motorhat_handle_t *s_handle = NULL; + +static motorhat_direction_t delta_to_direction(int8_t delta) { + if (delta > 0) return MOTORHAT_DIRECTION_FORWARD; + if (delta < 0) return MOTORHAT_DIRECTION_BACKWARD; + return MOTORHAT_DIRECTION_BRAKE; +} + void motor_stop_task(void *args) { motorhat_handle_t *handle = (motorhat_handle_t *)args; @@ -32,6 +42,7 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf return ESP_ERR_INVALID_ARG; } + s_handle = handle; handle->events = events; handle->stop_bits = FAULT_ANY; @@ -41,33 +52,66 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf return ESP_OK; } + +// Socket commands +esp_err_t motorhat_home(uint16_t delay_ms) { + ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); + // TODO: Implement homing logic using motorhat_set_motor_direction, motorhat_set_motor_speed, and limit switch state from signal bus + return ESP_OK; +} + esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, uint16_t delay_ms, uint16_t time_ms) { - // motorhat_set_motor_direction(); ESP_LOGI(TAG, "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", delta_azimuth, delta_altitude, delay_ms, time_ms); + // TODO: Implement polar pan logic using motorhat_set_motor_direction and motorhat_set_motor_speed return ESP_OK; } esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude) { - // motorhat_set_motor_direction(); - ESP_LOGI(TAG, "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", - delta_azimuth, delta_altitude); - return ESP_OK; + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + ESP_LOGI(TAG, "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", + delta_azimuth, delta_altitude); + + // Set both axes speed to 0 to prevent direction change while moving + esp_err_t err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], 0); + if (err != ESP_OK) { + return err; + } + err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], 0); + if (err != ESP_OK) { + return err; + } + + // Set directions based on deltas + err = motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], delta_to_direction(delta_azimuth)); + if (err != ESP_OK) { + return err; + } + err = motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], delta_to_direction(delta_altitude)); + if (err != ESP_OK) { + return err; + } + + // Set speeds to a default value + err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], POLAR_PAN_SPEED); + if (err != ESP_OK) { + return err; + } + return motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], POLAR_PAN_SPEED); } esp_err_t motorhat_polar_pan_stop(void) { - // motorhat_set_motor_direction(); ESP_LOGI(TAG, "Received polar pan stop command"); + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BRAKE); + } return ESP_OK; } -esp_err_t motorhat_home(uint16_t delay_ms) { - // motorhat_set_motor_direction(); - ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); - return ESP_OK; -} +// Motor control to be used by socket command functions esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, motorhat_motor_t motor, uint16_t speed) { if (handle == NULL || motor < MOTORHAT_MOTOR1 || @@ -147,6 +191,8 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, return err; } + +// Emergency stop all motors that bypasses normal logic flow esp_err_t motorhat_emergency_stop(motorhat_handle_t *handle) { if (handle == NULL) { return ESP_ERR_INVALID_ARG; diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index f4eb925..ec32166 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -23,24 +23,18 @@ menu "Talos ESP Driver Configuration" help I2C address for the MotorHAT device. + config DRIVER_MOTORHAT_PAN_SPEED + string "MotorHAT Pan Speed" + default "0.8" + help + The fraction of PCA9685's max PWM speed to use when driving polar pan. + config DRIVER_ADS1015_SDA_PIN int "ADS1015 SDA Pin" default 18 help GPIO pin number for the ADS1015 SDA line. - config DRIVER_ADS1015_SCL_PIN - int "ADS1015 SCL Pin" - default 19 - help - GPIO pin number for the ADS1015 SCL line. - - config DRIVER_ADS1015_I2C_BUS - int "ADS1015 I2C Bus Number" - default 1 - help - I2C bus number for ADS1015 communication. - config DRIVER_ADS1015_ADDRESS hex "ADS1015 I2C Address" default 0x48 diff --git a/main/esp_driver.c b/main/esp_driver.c index 922b8d2..738df6e 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -81,6 +81,7 @@ void app_main(void) { .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, }; ESP_ERROR_CHECK(encoder_init(&encoder, &encoder_config)); + ESP_ERROR_CHECK(encoder_start(&encoder)); // Initialize motor controller @@ -121,9 +122,11 @@ void app_main(void) { esp_err_t err = encoder_get_raw_count(&encoder, &pulse_count); if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); - vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop on error } + vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop + // This loop can also be used to add periodic tasks like reading sensors, or checking limit switches. // However, it must not terminate or structs initialized here will disappear from stack memory, breaking modules that use them. + } } From 14b1979ad8f7bf26e679074b676a614040d833d1 Mon Sep 17 00:00:00 2001 From: frey808 Date: Sun, 22 Mar 2026 15:05:26 -0400 Subject: [PATCH 07/16] uncommented pca9685 functions and removed logging --- components/motorhat/motorhat.c | 53 +++++++++++++++------------------- 1 file changed, 24 insertions(+), 29 deletions(-) diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 62897b9..51716ea 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -48,7 +48,7 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); - // return pca9685_init(&handle->pca9685, &config->pca9685_config); + return pca9685_init(&handle->pca9685, &config->pca9685_config); return ESP_OK; } @@ -130,7 +130,7 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, const motorhat_motor_channels_t *channels = &motor_channels[motor]; - // return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); + return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); return ESP_OK; } @@ -153,35 +153,31 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, switch (direction) { case MOTORHAT_DIRECTION_FORWARD: - // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); - // if (err != ESP_OK) - // return err; - // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); - ESP_LOGI(TAG, "Set motor %d direction to FORWARD", motor); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + if (err != ESP_OK) + return err; + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); err = ESP_OK; break; case MOTORHAT_DIRECTION_BACKWARD: - // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); - // if (err != ESP_OK) - // return err; - // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); - ESP_LOGI(TAG, "Set motor %d direction to BACKWARD", motor); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + if (err != ESP_OK) + return err; + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); err = ESP_OK; break; case MOTORHAT_DIRECTION_BRAKE: - // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); - // if (err != ESP_OK) - // return err; - // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); - ESP_LOGI(TAG, "Set motor %d direction to BRAKE", motor); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + if (err != ESP_OK) + return err; + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); err = ESP_OK; break; case MOTORHAT_DIRECTION_RELEASE: - // err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); - // if (err != ESP_OK) - // return err; - // err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); - ESP_LOGI(TAG, "Set motor %d direction to RELEASE", motor); + err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + if (err != ESP_OK) + return err; + err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); err = ESP_OK; break; default: @@ -203,15 +199,14 @@ esp_err_t motorhat_emergency_stop(motorhat_handle_t *handle) { const motorhat_motor_channels_t *ch = &motor_channels[m]; // Stop PWM cycle and release both control lines - ESP_LOGI(TAG, "Emergency stopping motor %d", m); - // esp_err_t err = pca9685_set_duty_cycle(&handle->pca9685, ch->pwm_channel, 0); - // if (err != ESP_OK && first_err == ESP_OK) first_err = err; + esp_err_t err = pca9685_set_duty_cycle(&handle->pca9685, ch->pwm_channel, 0); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; - // err = pca9685_digital_write(&handle->pca9685, ch->in1_channel, false); - // if (err != ESP_OK && first_err == ESP_OK) first_err = err; + err = pca9685_digital_write(&handle->pca9685, ch->in1_channel, false); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; - // err = pca9685_digital_write(&handle->pca9685, ch->in2_channel, false); - // if (err != ESP_OK && first_err == ESP_OK) first_err = err; + err = pca9685_digital_write(&handle->pca9685, ch->in2_channel, false); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; } return first_err; } From 3f64a30c67639827371eb2437dbb93a6e09b89de Mon Sep 17 00:00:00 2001 From: Ryan Yocum Date: Tue, 31 Mar 2026 16:01:15 -0400 Subject: [PATCH 08/16] Fix some issues + format all to google std w/ clang format --- .clang-format | 3 + components/ads1015/ads1015.c | 238 ++++++++-------- components/ads1015/include/ads1015.h | 158 ++++++----- components/ads1015/test/test_ads1015.c | 12 +- components/driver_socket/driver_socket.c | 83 +++--- components/driver_socket/driver_socket_api.c | 45 +-- .../driver_socket/include/driver_socket.h | 31 +-- .../driver_socket/include/driver_socket_api.h | 59 ++-- components/driver_wifi/driver_wifi.c | 16 +- components/driver_wifi/include/driver_wifi.h | 38 +-- .../driver_wifi/test/test_driver_wifi.c | 4 +- components/encoder/encoder.c | 259 +++++++++--------- components/encoder/include/encoder.h | 88 +++--- components/encoder/test/test_encoder.c | 71 ++--- components/encoder/test/test_runner.c | 1 - components/i2c_bus/i2c_bus.c | 4 +- components/i2c_bus/include/i2c_bus.h | 20 +- components/i2c_bus/test/test_i2c_bus.c | 5 +- .../limit_switch/include/limit_switch.h | 10 +- components/limit_switch/limit_switch.c | 48 ++-- components/motorhat/include/motorhat.h | 107 ++++---- components/motorhat/include/pca9685.h | 30 +- components/motorhat/motorhat.c | 227 ++++++++------- components/motorhat/pca9685.c | 31 ++- components/motorhat/test/test_pca9685.c | 45 +-- components/signal_bus/include/signal_bus.h | 24 +- components/signal_bus/signal_bus.c | 15 +- main/Kconfig.projbuild | 6 - main/esp_driver.c | 55 ++-- test/main/esp_driver_test.c | 35 +-- test/main/include/test_menu.h | 6 +- test/main/test_menu.c | 250 ++++++++--------- 32 files changed, 1040 insertions(+), 984 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..925c757 --- /dev/null +++ b/.clang-format @@ -0,0 +1,3 @@ +BasedOnStyle : Google +PointerAlignment: Left +DerivePointerAlignment: false diff --git a/components/ads1015/ads1015.c b/components/ads1015/ads1015.c index 5b0d48e..a948bd3 100644 --- a/components/ads1015/ads1015.c +++ b/components/ads1015/ads1015.c @@ -1,4 +1,5 @@ #include "ads1015.h" + #include "driver/i2c_master.h" #include "esp_err.h" #include "esp_log.h" @@ -7,152 +8,148 @@ #define TAG "ADS1015" -static TaskHandle_t adc_task_handle = NULL; +static TaskHandle_t adc_task_handle = NULL; -static void IRAM_ATTR ads1015_isr(void *arg){ - BaseType_t higher_priority_task_woken = pdFALSE; +static void IRAM_ATTR ads1015_isr(void* arg) { + BaseType_t higher_priority_task_woken = pdFALSE; - if (adc_task_handle){ - xTaskNotifyFromISR( - adc_task_handle, - 0, - eNoAction, - &higher_priority_task_woken - ); - } + if (adc_task_handle) { + xTaskNotifyFromISR(adc_task_handle, 0, eNoAction, + &higher_priority_task_woken); + } - if (higher_priority_task_woken) { - portYIELD_FROM_ISR(); - } + if (higher_priority_task_woken) { + portYIELD_FROM_ISR(); + } } -void adc_task(void *arg){ - ads1015_handle_t *handle = (ads1015_handle_t *)arg; - uint16_t config_reg = handle->config_reg | (1 << ADS1015_OS_BIT); // Modify so that writing will start conversions - EventGroupHandle_t events = handle->events; - bool mux_state = true; // Differential input to read (true = A2-A3, false = A0-A1) - - ads1015_write_register(handle, ADS1015_CONFIG, config_reg); // Start conversions - while (1) { - // Wait until RDY interrupt fires - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - - // Read conversion value - uint16_t raw; - if (ads1015_read_register(handle, ADS1015_CONVERSION, &raw) == ESP_OK){ - int16_t value = ((int16_t)raw) >> 4; - if (value >= CONFIG_ADS1015_HIGH_THRESH || value <= CONFIG_ADS1015_LOW_THRESH){ - xEventGroupSetBits(events, ESTOP_OVERCURRENT); - } - } - - // Switch MUX inputs and start next conversion - config_reg &= ~(0b111 << ADS1015_MUX_SHIFT); // clear MUX bits - config_reg |= (mux_state ? (ADS1015_MUX_AIN0_AIN1 << ADS1015_MUX_SHIFT) - : (ADS1015_MUX_AIN2_AIN3 << ADS1015_MUX_SHIFT)); - mux_state = !mux_state; - - ads1015_write_register(handle, ADS1015_CONFIG, config_reg); +void adc_task(void* arg) { + ads1015_handle_t* handle = (ads1015_handle_t*)arg; + uint16_t config_reg = + handle->config_reg | + (1 << ADS1015_OS_BIT); // Modify so that writing will start conversions + EventGroupHandle_t events = handle->events; + bool mux_state = + true; // Differential input to read (true = A2-A3, false = A0-A1) + + ads1015_write_register(handle, ADS1015_CONFIG, + config_reg); // Start conversions + while (1) { + // Wait until RDY interrupt fires + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); + + // Read conversion value + uint16_t raw; + if (ads1015_read_register(handle, ADS1015_CONVERSION, &raw) == ESP_OK) { + int16_t value = ((int16_t)raw) >> 4; + if (value >= CONFIG_ADS1015_HIGH_THRESH || + value <= CONFIG_ADS1015_LOW_THRESH) { + ESP_LOGI(TAG, "Overcurrent detection fired"); + xEventGroupSetBits(events, ESTOP_OVERCURRENT); + } } + + // Switch MUX inputs and start next conversion + config_reg &= ~(0b111 << ADS1015_MUX_SHIFT); // clear MUX bits + config_reg |= (mux_state ? (ADS1015_MUX_AIN0_AIN1 << ADS1015_MUX_SHIFT) + : (ADS1015_MUX_AIN2_AIN3 << ADS1015_MUX_SHIFT)); + mux_state = !mux_state; + + ads1015_write_register(handle, ADS1015_CONFIG, config_reg); + } } -esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config, EventGroupHandle_t events) { - if (handle == NULL || config == NULL) { - return ESP_ERR_INVALID_ARG; - } +esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config, + EventGroupHandle_t events) { + if (handle == NULL || config == NULL) { + return ESP_ERR_INVALID_ARG; + } - i2c_device_config_t dev_config = { + i2c_device_config_t dev_config = { .dev_addr_length = I2C_ADDR_BIT_LEN_7, .device_address = config->i2c_addr, .scl_speed_hz = config->i2c_speed_hz, - }; + }; - esp_err_t ret = i2c_master_bus_add_device(config->bus_handle, &dev_config, &handle->dev_handle); - if (ret != ESP_OK) { - return ret; - } + esp_err_t ret = i2c_master_bus_add_device(config->bus_handle, &dev_config, + &handle->dev_handle); + if (ret != ESP_OK) { + return ret; + } - // Set comparator threshold registers for RDY mode - ret = ads1015_write_register(handle, ADS1015_HIGH_THRESH_REG, 0x8000); - if (ret != ESP_OK) { - return ret; - } - ret = ads1015_write_register(handle, ADS1015_LOW_THRESH_REG, 0x0000); - if (ret != ESP_OK) { - return ret; - } + // Set comparator threshold registers for RDY mode + ret = ads1015_write_register(handle, ADS1015_HIGH_THRESH_REG, 0x8000); + if (ret != ESP_OK) { + return ret; + } + ret = ads1015_write_register(handle, ADS1015_LOW_THRESH_REG, 0x0000); + if (ret != ESP_OK) { + return ret; + } - // Write config register - uint16_t config_reg = ads1015_build_config( - ADS1015_MUX_AIN0_AIN1, - ADS1015_PGA_4_096V, - ADS1015_MODE_SINGLESHOT, - config->adc_data_rate, - ADS1015_COMP_TRADITIONAL, - ADS1015_COMP_ACTIVE_LOW, - ADS1015_COMP_NON_LATCHING, - ADS1015_COMP_ASSERT_1, - false // don't conversions immediately - ); - - ret = ads1015_write_register(handle, ADS1015_CONFIG, config_reg); - if (ret != ESP_OK) { - return ret; - } + // Write config register + uint16_t config_reg = ads1015_build_config( + ADS1015_MUX_AIN0_AIN1, ADS1015_PGA_4_096V, ADS1015_MODE_SINGLESHOT, + config->adc_data_rate, ADS1015_COMP_TRADITIONAL, ADS1015_COMP_ACTIVE_LOW, + ADS1015_COMP_NON_LATCHING, ADS1015_COMP_ASSERT_1, + false // don't conversions immediately + ); - handle->config_reg = config_reg; - handle->events = events; - - // Configure alert GPIO and interrupt service - gpio_config_t io_conf = { - .intr_type = GPIO_INTR_NEGEDGE, - .mode = GPIO_MODE_INPUT, - .pin_bit_mask = 1ULL << config->rdy_gpio, - .pull_up_en = GPIO_PULLUP_ENABLE, - .pull_down_en = GPIO_PULLDOWN_DISABLE, - }; - - ret = gpio_config(&io_conf); - if (ret != ESP_OK) { - return ret; - } - ret = gpio_isr_handler_add(config->rdy_gpio, ads1015_isr, NULL); - if (ret != ESP_OK) { - return ret; - } - - // Start waiting task - xTaskCreate(adc_task, "adc_task", 4096, handle, 10, &adc_task_handle); + ret = ads1015_write_register(handle, ADS1015_CONFIG, config_reg); + if (ret != ESP_OK) { + return ret; + } + + handle->config_reg = config_reg; + handle->events = events; + + // Configure alert GPIO and interrupt service + gpio_config_t io_conf = { + .intr_type = GPIO_INTR_NEGEDGE, + .mode = GPIO_MODE_INPUT, + .pin_bit_mask = 1ULL << config->rdy_gpio, + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + }; + + ret = gpio_config(&io_conf); + if (ret != ESP_OK) { + return ret; + } + ret = gpio_isr_handler_add(config->rdy_gpio, ads1015_isr, NULL); + if (ret != ESP_OK) { + return ret; + } + + // Start waiting task + xTaskCreate(adc_task, "adc_task", 4096, handle, 10, &adc_task_handle); - ESP_LOGI(TAG, "ADS1015 initialized"); + ESP_LOGI(TAG, "ADS1015 initialized"); - return ESP_OK; + return ESP_OK; } -esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t *data) { +esp_err_t ads1015_read_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t* data) { if (handle == NULL || data == NULL) { return ESP_ERR_INVALID_ARG; } uint8_t rx_buf[2]; - esp_err_t ret = i2c_master_transmit_receive( - handle->dev_handle, - (uint8_t *)®, - 1, - rx_buf, - 2, - ADS1015_I2C_MASTER_TIMEOUT_MS - ); - - if (ret == ESP_OK) { - *data = (rx_buf[0] << 8) | rx_buf[1]; - } + esp_err_t ret = + i2c_master_transmit_receive(handle->dev_handle, (uint8_t*)®, 1, rx_buf, + 2, ADS1015_I2C_MASTER_TIMEOUT_MS); - return ret; + if (ret == ESP_OK) { + *data = (rx_buf[0] << 8) | rx_buf[1]; + } + + return ret; } -esp_err_t ads1015_write_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t data) { +esp_err_t ads1015_write_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t data) { if (handle == NULL) { return ESP_ERR_INVALID_ARG; } @@ -161,9 +158,6 @@ esp_err_t ads1015_write_register(ads1015_handle_t *handle, ads1015_register_t re write_buf[0] = reg; write_buf[1] = (data >> 8) & 0xFF; // MSB first write_buf[2] = data & 0xFF; // LSB - return i2c_master_transmit( - handle->dev_handle, - write_buf, - sizeof(write_buf), - ADS1015_I2C_MASTER_TIMEOUT_MS); + return i2c_master_transmit(handle->dev_handle, write_buf, sizeof(write_buf), + ADS1015_I2C_MASTER_TIMEOUT_MS); } diff --git a/components/ads1015/include/ads1015.h b/components/ads1015/include/ads1015.h index 51eb270..f0fc425 100644 --- a/components/ads1015/include/ads1015.h +++ b/components/ads1015/include/ads1015.h @@ -1,148 +1,140 @@ #ifndef _ADS1015_H_ #define _ADS1015_H_ -#include "driver/i2c_types.h" #include "driver/gpio.h" +#include "driver/i2c_types.h" #include "esp_err.h" #include "freertos/FreeRTOS.h" #include "freertos/semphr.h" #define ADS1015_I2C_MASTER_TIMEOUT_MS 1000 -#define ADS1015_MUX_MASK (0x7 << 12) +#define ADS1015_MUX_MASK (0x7 << 12) // Config Register Bit Positions -#define ADS1015_OS_BIT 15 -#define ADS1015_MUX_SHIFT 12 -#define ADS1015_PGA_SHIFT 9 -#define ADS1015_MODE_BIT 8 -#define ADS1015_DR_SHIFT 5 -#define ADS1015_COMP_MODE_BIT 4 -#define ADS1015_COMP_POL_BIT 3 -#define ADS1015_COMP_LAT_BIT 2 -#define ADS1015_COMP_QUE_SHIFT 0 +#define ADS1015_OS_BIT 15 +#define ADS1015_MUX_SHIFT 12 +#define ADS1015_PGA_SHIFT 9 +#define ADS1015_MODE_BIT 8 +#define ADS1015_DR_SHIFT 5 +#define ADS1015_COMP_MODE_BIT 4 +#define ADS1015_COMP_POL_BIT 3 +#define ADS1015_COMP_LAT_BIT 2 +#define ADS1015_COMP_QUE_SHIFT 0 /** * @brief ADS1015 register addresses */ typedef enum { - ADS1015_CONVERSION = 0x00, - ADS1015_CONFIG = 0x01, - ADS1015_LOW_THRESH_REG = 0x02, - ADS1015_HIGH_THRESH_REG = 0x03, + ADS1015_CONVERSION = 0x00, + ADS1015_CONFIG = 0x01, + ADS1015_LOW_THRESH_REG = 0x02, + ADS1015_HIGH_THRESH_REG = 0x03, } ads1015_register_t; /** * @brief ADS1015 MUX register bit definitions */ typedef enum { - ADS1015_MUX_AIN0_AIN1 = 0b000, - ADS1015_MUX_AIN0_AIN3 = 0b001, - ADS1015_MUX_AIN1_AIN3 = 0b010, - ADS1015_MUX_AIN2_AIN3 = 0b011, - ADS1015_MUX_AIN0_GND = 0b100, - ADS1015_MUX_AIN1_GND = 0b101, - ADS1015_MUX_AIN2_GND = 0b110, - ADS1015_MUX_AIN3_GND = 0b111 + ADS1015_MUX_AIN0_AIN1 = 0b000, + ADS1015_MUX_AIN0_AIN3 = 0b001, + ADS1015_MUX_AIN1_AIN3 = 0b010, + ADS1015_MUX_AIN2_AIN3 = 0b011, + ADS1015_MUX_AIN0_GND = 0b100, + ADS1015_MUX_AIN1_GND = 0b101, + ADS1015_MUX_AIN2_GND = 0b110, + ADS1015_MUX_AIN3_GND = 0b111 } ads1015_mux_t; /** * @brief ADS1015 PGA register bit definitions */ typedef enum { - ADS1015_PGA_6_144V = 0b000, - ADS1015_PGA_4_096V = 0b001, - ADS1015_PGA_2_048V = 0b010, - ADS1015_PGA_1_024V = 0b011, - ADS1015_PGA_0_512V = 0b100, - ADS1015_PGA_0_256V = 0b101 + ADS1015_PGA_6_144V = 0b000, + ADS1015_PGA_4_096V = 0b001, + ADS1015_PGA_2_048V = 0b010, + ADS1015_PGA_1_024V = 0b011, + ADS1015_PGA_0_512V = 0b100, + ADS1015_PGA_0_256V = 0b101 } ads1015_pga_t; /** * @brief ADS1015 mode register bit definitions */ typedef enum { - ADS1015_MODE_CONTINUOUS = 0, - ADS1015_MODE_SINGLESHOT = 1 + ADS1015_MODE_CONTINUOUS = 0, + ADS1015_MODE_SINGLESHOT = 1 } ads1015_mode_t; /** * @brief ADS1015 data rate register bit definitions */ typedef enum { - ADS1015_DR_128SPS = 0b000, - ADS1015_DR_250SPS = 0b001, - ADS1015_DR_490SPS = 0b010, - ADS1015_DR_920SPS = 0b011, - ADS1015_DR_1600SPS = 0b100, - ADS1015_DR_2400SPS = 0b101, - ADS1015_DR_3300SPS = 0b110 + ADS1015_DR_128SPS = 0b000, + ADS1015_DR_250SPS = 0b001, + ADS1015_DR_490SPS = 0b010, + ADS1015_DR_920SPS = 0b011, + ADS1015_DR_1600SPS = 0b100, + ADS1015_DR_2400SPS = 0b101, + ADS1015_DR_3300SPS = 0b110 } ads1015_dr_t; /** * @brief ADS1015 comparator settings register bit definitions */ typedef enum { - ADS1015_COMP_TRADITIONAL = 0, - ADS1015_COMP_WINDOW = 1 + ADS1015_COMP_TRADITIONAL = 0, + ADS1015_COMP_WINDOW = 1 } ads1015_comp_mode_t; typedef enum { - ADS1015_COMP_ACTIVE_LOW = 0, - ADS1015_COMP_ACTIVE_HIGH = 1 + ADS1015_COMP_ACTIVE_LOW = 0, + ADS1015_COMP_ACTIVE_HIGH = 1 } ads1015_comp_pol_t; typedef enum { - ADS1015_COMP_NON_LATCHING = 0, - ADS1015_COMP_LATCHING = 1 + ADS1015_COMP_NON_LATCHING = 0, + ADS1015_COMP_LATCHING = 1 } ads1015_comp_lat_t; typedef enum { - ADS1015_COMP_ASSERT_1 = 0b00, - ADS1015_COMP_ASSERT_2 = 0b01, - ADS1015_COMP_ASSERT_4 = 0b10, - ADS1015_COMP_DISABLE = 0b11 + ADS1015_COMP_ASSERT_1 = 0b00, + ADS1015_COMP_ASSERT_2 = 0b01, + ADS1015_COMP_ASSERT_4 = 0b10, + ADS1015_COMP_DISABLE = 0b11 } ads1015_comp_que_t; static inline uint16_t ads1015_build_config( - ads1015_mux_t mux, - ads1015_pga_t pga, - ads1015_mode_t mode, - ads1015_dr_t dr, - ads1015_comp_mode_t comp_mode, - ads1015_comp_pol_t comp_pol, - ads1015_comp_lat_t comp_lat, - ads1015_comp_que_t comp_que, - bool start_conversion -) { - return - ((start_conversion ? 1 : 0) << ADS1015_OS_BIT) | - (mux << ADS1015_MUX_SHIFT) | - (pga << ADS1015_PGA_SHIFT) | - (mode << ADS1015_MODE_BIT) | - (dr << ADS1015_DR_SHIFT) | - (comp_mode << ADS1015_COMP_MODE_BIT) | - (comp_pol << ADS1015_COMP_POL_BIT) | - (comp_lat << ADS1015_COMP_LAT_BIT) | - (comp_que << ADS1015_COMP_QUE_SHIFT); + ads1015_mux_t mux, ads1015_pga_t pga, ads1015_mode_t mode, ads1015_dr_t dr, + ads1015_comp_mode_t comp_mode, ads1015_comp_pol_t comp_pol, + ads1015_comp_lat_t comp_lat, ads1015_comp_que_t comp_que, + bool start_conversion) { + return ((start_conversion ? 1 : 0) << ADS1015_OS_BIT) | + (mux << ADS1015_MUX_SHIFT) | (pga << ADS1015_PGA_SHIFT) | + (mode << ADS1015_MODE_BIT) | (dr << ADS1015_DR_SHIFT) | + (comp_mode << ADS1015_COMP_MODE_BIT) | + (comp_pol << ADS1015_COMP_POL_BIT) | + (comp_lat << ADS1015_COMP_LAT_BIT) | + (comp_que << ADS1015_COMP_QUE_SHIFT); } /** * @brief ADS1015 configuration structure */ typedef struct { - uint8_t i2c_addr; /**< I2C device address */ - uint32_t i2c_speed_hz; /**< I2C bus speed in Hz */ - i2c_master_bus_handle_t bus_handle; /**< I2C master bus handle */ - gpio_num_t rdy_gpio; /**< GPIO number for ALERT pin */ - uint8_t adc_data_rate; /**< Data rate setting for ADC conversions */ + uint8_t i2c_addr; /**< I2C device address */ + uint32_t i2c_speed_hz; /**< I2C bus speed in Hz */ + i2c_master_bus_handle_t bus_handle; /**< I2C master bus handle */ + gpio_num_t rdy_gpio; /**< GPIO number for ALERT pin */ + uint8_t adc_data_rate; /**< Data rate setting for ADC conversions */ } ads1015_config_t; /** * @brief ADS1015 device handle */ typedef struct { - i2c_master_dev_handle_t dev_handle; /**< I2C device handle */ - uint16_t config_reg; /**< ADS1015 config register values */ - EventGroupHandle_t events; /**< Event group handle for signaling overcurrent events */ + i2c_master_dev_handle_t dev_handle; /**< I2C device handle */ + uint16_t config_reg; /**< ADS1015 config register values */ + EventGroupHandle_t + events; /**< Event group handle for signaling overcurrent events */ } ads1015_handle_t; /** @@ -161,10 +153,12 @@ typedef struct { * frequency) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_init(ads1015_handle_t *handle, const ads1015_config_t *config, EventGroupHandle_t events); +esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config, + EventGroupHandle_t events); /** - * @brief Check the ADC value against the configured threshholds and trigger E-stop if it exceeds them + * @brief Check the ADC value against the configured threshholds and trigger + * E-stop if it exceeds them * * @param[in] value ADC value to check * @@ -185,7 +179,8 @@ esp_err_t ads1015_check_current(int16_t value, bool mux_state); * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t *data); +esp_err_t ads1015_read_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t* data); /** * @brief Write a single byte to a ADS1015 register @@ -199,6 +194,7 @@ esp_err_t ads1015_read_register(ads1015_handle_t *handle, ads1015_register_t reg * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_write_register(ads1015_handle_t *handle, ads1015_register_t reg, uint16_t data); +esp_err_t ads1015_write_register(ads1015_handle_t* handle, + ads1015_register_t reg, uint16_t data); -#endif // _ADS1015_H_ +#endif // _ADS1015_H_ diff --git a/components/ads1015/test/test_ads1015.c b/components/ads1015/test/test_ads1015.c index 9dd1323..1b4860a 100644 --- a/components/ads1015/test/test_ads1015.c +++ b/components/ads1015/test/test_ads1015.c @@ -1,6 +1,7 @@ +#include "ads1015.h" #include "driver/i2c_master.h" +#include "freertos/idf_additions.h" #include "i2c_bus.h" -#include "ads1015.h" #include "unity_fixture.h" static i2c_bus_t bus_handle; @@ -33,19 +34,20 @@ TEST(ADS1015, ADS1015_Initialization) { .bus_handle = bus_handle.handle, }; - esp_err_t err = ads1015_init(&handle, &config); + EventGroupHandle_t event_group = xEventGroupCreate(); + esp_err_t err = ads1015_init(&handle, &config, event_group); TEST_ASSERT_EQUAL(ESP_OK, err); TEST_ASSERT_NOT_EQUAL(NULL, handle.dev_handle); } TEST(ADS1015, ADS1015_Wrong_Address) { - ads1015_config_t config = { - .i2c_addr = 0x00, // Invalid address + .i2c_addr = 0x00, // Invalid address .i2c_speed_hz = 400000, .bus_handle = bus_handle.handle, }; - esp_err_t err = ads1015_init(&handle, &config); + EventGroupHandle_t event_group = xEventGroupCreate(); + esp_err_t err = ads1015_init(&handle, &config, event_group); TEST_ASSERT_NOT_EQUAL(ESP_OK, err); } diff --git a/components/driver_socket/driver_socket.c b/components/driver_socket/driver_socket.c index e0b8365..492e64f 100644 --- a/components/driver_socket/driver_socket.c +++ b/components/driver_socket/driver_socket.c @@ -1,10 +1,11 @@ #include "driver_socket.h" + +#include "driver_socket_api.h" +#include "endian.h" #include "esp_err.h" #include "esp_log.h" #include "netdb.h" #include "sys/socket.h" -#include "driver_socket_api.h" -#include "endian.h" #define TAG "driver_socket" #define INVALID_SOCK -1 @@ -20,12 +21,12 @@ */ #define YIELD_TO_ALL_MS 50 -void driver_socket_task(void *arg) { +void driver_socket_task(void* arg) { static uint8_t rx_buffer[128]; - task_args_t *task_args = (task_args_t *)arg; - SemaphoreHandle_t *server_ready = &task_args->server_ready; + task_args_t* task_args = (task_args_t*)arg; + SemaphoreHandle_t* server_ready = &task_args->server_ready; struct addrinfo hints = {.ai_socktype = SOCK_STREAM}; - struct addrinfo *address_info; + struct addrinfo* address_info; int listen_sock = INVALID_SOCK; const size_t max_socks = CONFIG_LWIP_MAX_SOCKETS - 1; static int sock[CONFIG_LWIP_MAX_SOCKETS - 1]; @@ -87,7 +88,7 @@ void driver_socket_task(void *arg) { // Main loop for accepting new connections and serving all connected clients while (1) { - struct sockaddr_storage source_addr; // Large enough for both IPv4 or IPv6 + struct sockaddr_storage source_addr; // Large enough for both IPv4 or IPv6 socklen_t addr_len = sizeof(source_addr); // Find a free socket @@ -102,13 +103,13 @@ void driver_socket_task(void *arg) { if (new_sock_index < max_socks) { // Try to accept a new connections sock[new_sock_index] = - accept(listen_sock, (struct sockaddr *)&source_addr, &addr_len); + accept(listen_sock, (struct sockaddr*)&source_addr, &addr_len); if (sock[new_sock_index] < 0) { if (errno == - EWOULDBLOCK) { // The listener socket did not accepts any connection - // continue to serve open connections and try to - // accept again upon the next iteration + EWOULDBLOCK) { // The listener socket did not accepts any + // connection continue to serve open connections and + // try to accept again upon the next iteration ESP_LOGV(TAG, "No pending connections..."); } else { log_socket_error(TAG, listen_sock, errno, @@ -135,7 +136,6 @@ void driver_socket_task(void *arg) { // We serve all the connected clients in this loop for (int i = 0; i < max_socks; ++i) { if (sock[i] != INVALID_SOCK) { - // This is an open socket -> try to serve it int len = try_receive(TAG, sock[i], rx_buffer, sizeof(rx_buffer)); if (len < 0) { @@ -168,8 +168,8 @@ void driver_socket_task(void *arg) { } } - } // one client's socket - } // for all sockets + } // one client's socket + } // for all sockets // Yield to other tasks vTaskDelay(pdMS_TO_TICKS(YIELD_TO_ALL_MS)); @@ -190,9 +190,9 @@ void driver_socket_task(void *arg) { vTaskDelete(NULL); } -esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, - const driver_socket_config_t *config, const driver_socket_api_motor_interface_t *motor_interface) { - +esp_err_t driver_socket_init( + driver_socket_handle_t* socket_handle, const driver_socket_config_t* config, + const driver_socket_api_motor_interface_t* motor_interface) { if (socket_handle == NULL || config == NULL || motor_interface == NULL) { return ESP_ERR_INVALID_ARG; } @@ -202,7 +202,7 @@ esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, return ret; } - task_args_t *task_args = malloc(sizeof(task_args_t)); + task_args_t* task_args = malloc(sizeof(task_args_t)); if (task_args == NULL) { ESP_LOGE(TAG, "Unable to allocate memory for task arguments"); return ESP_ERR_NO_MEM; @@ -228,44 +228,44 @@ esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, return ESP_OK; } -void log_socket_error(const char *tag, const int sock, const int err, - const char *message) { +void log_socket_error(const char* tag, const int sock, const int err, + const char* message) { ESP_LOGE(tag, "[sock=%d]: %s\n" "error=%d: %s", sock, message, err, strerror(err)); } -char *get_clients_address(struct sockaddr_storage *source_addr) { +char* get_clients_address(struct sockaddr_storage* source_addr) { static char address_str[128]; - char *res = NULL; + char* res = NULL; // Convert ip address to string if (source_addr->ss_family == PF_INET) { - res = inet_ntoa_r(((struct sockaddr_in *)source_addr)->sin_addr, - address_str, sizeof(address_str) - 1); + res = inet_ntoa_r(((struct sockaddr_in*)source_addr)->sin_addr, address_str, + sizeof(address_str) - 1); } #ifdef CONFIG_LWIP_IPV6 else if (source_addr->ss_family == PF_INET6) { - res = inet6_ntoa_r(((struct sockaddr_in6 *)source_addr)->sin6_addr, + res = inet6_ntoa_r(((struct sockaddr_in6*)source_addr)->sin6_addr, address_str, sizeof(address_str) - 1); } #endif if (!res) { - address_str[0] = '\0'; // Returns empty string if conversion didn't succeed + address_str[0] = '\0'; // Returns empty string if conversion didn't succeed } return address_str; } -int try_receive(const char *tag, const int sock, uint8_t *data, - size_t max_len) { +int try_receive(const char* tag, const int sock, uint8_t* data, + size_t max_len) { int len = recv(sock, data, max_len, 0); if (len < 0) { if (errno == EINPROGRESS || errno == EAGAIN || errno == EWOULDBLOCK) { - return 0; // Not an error + return 0; // Not an error } if (errno == ENOTCONN) { ESP_LOGW(tag, "[sock=%d]: Connection closed", sock); - return -2; // Socket has been disconnected + return -2; // Socket has been disconnected } log_socket_error(tag, sock, errno, "Error occurred during receiving"); return -1; @@ -274,16 +274,17 @@ int try_receive(const char *tag, const int sock, uint8_t *data, return len; } -int socket_send(const char *tag, const int sock, const uint8_t * data, const size_t len) -{ - int to_write = len; - while (to_write > 0) { - int written = send(sock, data + (len - to_write), to_write, 0); - if (written < 0 && errno != EINPROGRESS && errno != EAGAIN && errno != EWOULDBLOCK) { - log_socket_error(tag, sock, errno, "Error occurred during sending"); - return -1; - } - to_write -= written; +int socket_send(const char* tag, const int sock, const uint8_t* data, + const size_t len) { + int to_write = len; + while (to_write > 0) { + int written = send(sock, data + (len - to_write), to_write, 0); + if (written < 0 && errno != EINPROGRESS && errno != EAGAIN && + errno != EWOULDBLOCK) { + log_socket_error(tag, sock, errno, "Error occurred during sending"); + return -1; } - return len; + to_write -= written; + } + return len; } diff --git a/components/driver_socket/driver_socket_api.c b/components/driver_socket/driver_socket_api.c index 746b41a..f2660a9 100644 --- a/components/driver_socket/driver_socket_api.c +++ b/components/driver_socket/driver_socket_api.c @@ -1,4 +1,5 @@ #include "driver_socket_api.h" + #include "endian.h" #include "esp_log.h" #include "freertos//FreeRTOS.h" @@ -7,24 +8,27 @@ static driver_socket_api_motor_interface_t s_motor_interface; -static void driver_socket_api_header_swap_endianess(driver_socket_api_header_t *header) { +static void driver_socket_api_header_swap_endianess( + driver_socket_api_header_t* header) { header->msg_id = be32toh(header->msg_id); header->cmd_id = be16toh(header->cmd_id); header->len = be16toh(header->len); } -static void driver_socket_api_polar_pan_payload_swap_endianess(driver_socket_api_polar_pan_payload_t *payload) { +static void driver_socket_api_polar_pan_payload_swap_endianess( + driver_socket_api_polar_pan_payload_t* payload) { payload->delta_azimuth = be32toh(payload->delta_azimuth); payload->delta_altitude = be32toh(payload->delta_altitude); payload->delay_ms = be32toh(payload->delay_ms); payload->time_ms = be32toh(payload->time_ms); } -static void driver_socket_api_home_payload_swap_endianess(driver_socket_api_home_payload_t *payload) { +static void driver_socket_api_home_payload_swap_endianess( + driver_socket_api_home_payload_t* payload) { payload->delay_ms = be32toh(payload->delay_ms); } -static uint8_t xor_checksum(uint8_t *data, size_t len) { +static uint8_t xor_checksum(uint8_t* data, size_t len) { uint8_t result = 0; for (size_t i = 0; i < len; i++) { result ^= data[i]; @@ -32,12 +36,14 @@ static uint8_t xor_checksum(uint8_t *data, size_t len) { return result; } -static esp_err_t validate_checksum(uint8_t *data, size_t len, uint8_t checksum) { - uint8_t calculated_checksum = xor_checksum((uint8_t *)data, len); +static esp_err_t validate_checksum(uint8_t* data, size_t len, + uint8_t checksum) { + uint8_t calculated_checksum = xor_checksum((uint8_t*)data, len); return (calculated_checksum == checksum) ? ESP_OK : ESP_ERR_INVALID_CRC; } -esp_err_t driver_socket_api_init(const driver_socket_api_motor_interface_t *motor_interface) { +esp_err_t driver_socket_api_init( + const driver_socket_api_motor_interface_t* motor_interface) { if (motor_interface == NULL) { return ESP_ERR_INVALID_ARG; } @@ -45,17 +51,18 @@ esp_err_t driver_socket_api_init(const driver_socket_api_motor_interface_t *moto return ESP_OK; } -esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { +esp_err_t driver_socket_api_process(uint8_t* buffer, size_t buffer_size) { ESP_LOGI(TAG, "Received data size: %d", buffer_size); ESP_LOG_BUFFER_HEX_LEVEL(TAG, buffer, buffer_size, ESP_LOG_INFO); - esp_err_t err = validate_checksum(buffer, buffer_size - 1, buffer[buffer_size - 1]); + esp_err_t err = + validate_checksum(buffer, buffer_size - 1, buffer[buffer_size - 1]); if (err != ESP_OK) { ESP_LOGE(TAG, "Checksum validation failed"); return err; } - driver_socket_api_wrapper_t *wrapper = (driver_socket_api_wrapper_t *)buffer; + driver_socket_api_wrapper_t* wrapper = (driver_socket_api_wrapper_t*)buffer; driver_socket_api_header_swap_endianess(&wrapper->header); @@ -67,20 +74,26 @@ esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size) { ESP_LOGI(TAG, "Processing HANDSHAKE command"); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN: - driver_socket_api_polar_pan_payload_t *polar_pan_payload = (driver_socket_api_polar_pan_payload_t *)&wrapper->payload_head; + driver_socket_api_polar_pan_payload_t* polar_pan_payload = + (driver_socket_api_polar_pan_payload_t*)&wrapper->payload_head; driver_socket_api_polar_pan_payload_swap_endianess(polar_pan_payload); - s_motor_interface.polar_pan(polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, - polar_pan_payload->delay_ms, polar_pan_payload->time_ms); + s_motor_interface.polar_pan( + polar_pan_payload->delta_azimuth, polar_pan_payload->delta_altitude, + polar_pan_payload->delay_ms, polar_pan_payload->time_ms); break; case DRIVER_SOCKET_API_CMD_ID_HOME: - driver_socket_api_home_payload_t *home_payload = (driver_socket_api_home_payload_t *)&wrapper->payload_head; + driver_socket_api_home_payload_t* home_payload = + (driver_socket_api_home_payload_t*)&wrapper->payload_head; driver_socket_api_home_payload_swap_endianess(home_payload); s_motor_interface.home(home_payload->delay_ms); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_START: - driver_socket_api_polar_pan_start_payload_t *polar_pan_start_payload = (driver_socket_api_polar_pan_start_payload_t *)&wrapper->payload_head; + driver_socket_api_polar_pan_start_payload_t* polar_pan_start_payload = + (driver_socket_api_polar_pan_start_payload_t*)&wrapper->payload_head; // No endianess swap needed for int8_t fields - s_motor_interface.polar_pan_start(polar_pan_start_payload->delta_azimuth, polar_pan_start_payload->delta_altitude); + s_motor_interface.polar_pan_start( + polar_pan_start_payload->delta_azimuth, + polar_pan_start_payload->delta_altitude); break; case DRIVER_SOCKET_API_CMD_ID_POLAR_PAN_STOP: s_motor_interface.polar_pan_stop(); diff --git a/components/driver_socket/include/driver_socket.h b/components/driver_socket/include/driver_socket.h index a67b423..0a09b86 100644 --- a/components/driver_socket/include/driver_socket.h +++ b/components/driver_socket/include/driver_socket.h @@ -1,10 +1,9 @@ #ifndef _DRIVER_SOCKET_H_ #define _DRIVER_SOCKET_H_ +#include "driver_socket_api.h" #include "freertos/FreeRTOS.h" #include "sys/socket.h" -#include "driver_socket_api.h" - /** * @brief Socket driver handle and configuration structures @@ -14,11 +13,10 @@ typedef struct { } driver_socket_handle_t; typedef struct { - const char *ip; - const char *port; + const char* ip; + const char* port; } driver_socket_config_t; - /** * @brief Task arguments structure for the socket task */ @@ -34,15 +32,17 @@ typedef struct { * * @param[out] socket_handle Socket handle to be initialized by this function * @param[in] config Socket configuration - * @param[in] motor_interface Motor control interface to be used by driver_socket_api + * @param[in] motor_interface Motor control interface to be used by + * driver_socket_api * @return * ESP_OK : Socket driver initialized successfully * ESP_ERR_INVALID_ARG : Invalid argument * ESP_ERR_NO_MEM : Unable to allocate memory for task arguments or * server_ready semaphore */ -esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, - const driver_socket_config_t *config, const driver_socket_api_motor_interface_t *motor_interface); +esp_err_t driver_socket_init( + driver_socket_handle_t* socket_handle, const driver_socket_config_t* config, + const driver_socket_api_motor_interface_t* motor_interface); /** * @brief Utility to log socket errors @@ -52,14 +52,14 @@ esp_err_t driver_socket_init(driver_socket_handle_t *socket_handle, * @param[in] err Socket errno * @param[in] message Message to print */ -void log_socket_error(const char *tag, const int sock, const int err, - const char *message); +void log_socket_error(const char* tag, const int sock, const int err, + const char* message); /** * @brief Returns the string representation of client's address (accepted on * this server) */ -char *get_clients_address(struct sockaddr_storage *source_addr); +char* get_clients_address(struct sockaddr_storage* source_addr); /** * @brief Tries to receive data from specified sockets in a non-blocking way, @@ -76,8 +76,7 @@ char *get_clients_address(struct sockaddr_storage *source_addr); * -2 : Socket is not connected, to distinguish between an actual * socket error and active disconnection */ -int try_receive(const char *tag, const int sock, uint8_t *data, - size_t max_len); +int try_receive(const char* tag, const int sock, uint8_t* data, size_t max_len); /** * @brief Sends the specified data to the socket. This function blocks until all @@ -91,7 +90,7 @@ int try_receive(const char *tag, const int sock, uint8_t *data, * >0 : Size the written data * -1 : Error occurred during socket write operation */ -int socket_send(const char *tag, const int sock, const uint8_t *data, - const size_t len); +int socket_send(const char* tag, const int sock, const uint8_t* data, + const size_t len); -#endif // _DRIVER_SOCKET_H_ +#endif // _DRIVER_SOCKET_H_ diff --git a/components/driver_socket/include/driver_socket_api.h b/components/driver_socket/include/driver_socket_api.h index 0d294a8..2059549 100644 --- a/components/driver_socket/include/driver_socket_api.h +++ b/components/driver_socket/include/driver_socket_api.h @@ -1,9 +1,10 @@ #ifndef _DRIVER_SOCKET_API_H_ #define _DRIVER_SOCKET_API_H_ -#include "freertos/FreeRTOS.h" #include +#include "freertos/FreeRTOS.h" + /** Command IDs for the socket API */ typedef enum { DRIVER_SOCKET_API_CMD_ID_HANDSHAKE = 0x0000, @@ -21,9 +22,10 @@ typedef struct { uint16_t len; } __attribute__((packed)) driver_socket_api_header_t; -/** Wrapper struct for the entire message, including header and payload. The payload - * is variable length, so we can't define it as a fixed array. Instead, we can - * use a flexible array member or just treat it as a pointer when processing. +/** Wrapper struct for the entire message, including header and payload. The + * payload is variable length, so we can't define it as a fixed array. Instead, + * we can use a flexible array member or just treat it as a pointer when + * processing. */ typedef struct { driver_socket_api_header_t header; @@ -34,7 +36,6 @@ typedef struct { // uint_t checksum; } __attribute__((packed)) driver_socket_api_wrapper_t; - /** Payload structures for different command types */ typedef struct { int32_t delta_azimuth; /** Requested change in azimuth */ @@ -53,43 +54,45 @@ typedef struct { } __attribute__((packed)) driver_socket_api_polar_pan_start_payload_t; typedef struct { - esp_err_t (*polar_pan)(int16_t delta_azimuth, int16_t delta_altitude, - uint16_t delay_ms, uint16_t time_ms); - esp_err_t (*polar_pan_start)(int8_t delta_azimuth, int8_t delta_altitude); - esp_err_t (*polar_pan_stop)(void); - esp_err_t (*home)(uint16_t delay_ms); + esp_err_t (*polar_pan)(int16_t delta_azimuth, int16_t delta_altitude, + uint16_t delay_ms, uint16_t time_ms); + esp_err_t (*polar_pan_start)(int8_t delta_azimuth, int8_t delta_altitude); + esp_err_t (*polar_pan_stop)(void); + esp_err_t (*home)(uint16_t delay_ms); } driver_socket_api_motor_interface_t; /** - * @brief Initializes the socket API module with the provided motor control interface. + * @brief Initializes the socket API module with the provided motor control + * interface. * * This function must be called before processing any incoming messages with * driver_socket_api_process(). It sets up the internal function pointers for * motor control commands that will be invoked when corresponding messages are * received. * - * @param[in] motor_interface Pointer to a structure containing function pointers - * for motor control operations. + * @param[in] motor_interface Pointer to a structure containing function + * pointers for motor control operations. * * @return * ESP_OK : Initialization successful * ESP_ERR_INVALID_ARG : Invalid argument (e.g., NULL pointer) */ -esp_err_t driver_socket_api_init(const driver_socket_api_motor_interface_t *motor_interface); +esp_err_t driver_socket_api_init( + const driver_socket_api_motor_interface_t* motor_interface); /** -* @brief Process incoming socket API messages. This function should be called -* from the socket task when a complete message has been received. It will parse -* the message, validate it, and execute the corresponding command. -* @param[in] buffer Pointer to the received message buffer -* @param[in] buffer_size Size of the received message buffer -* @return -* ESP_OK : Message processed successfully -* ESP_ERR_INVALID_ARG : Invalid message format or command ID -* ESP_ERR_INVALID_SIZE : Message size does not match expected size for the command -* ESP_ERR_INVALID_CRC : Checksum validation failed (if implemented) -* ESP_ERR_FAIL : Command execution failed (e.g., invalid parameters) -*/ -esp_err_t driver_socket_api_process(uint8_t *buffer, size_t buffer_size); + * @brief Process incoming socket API messages. This function should be called + * from the socket task when a complete message has been received. It will parse + * the message, validate it, and execute the corresponding command. + * @param[in] buffer Pointer to the received message buffer + * @param[in] buffer_size Size of the received message buffer + * @return + * ESP_OK : Message processed successfully + * ESP_ERR_INVALID_ARG : Invalid message format or command ID + * ESP_ERR_INVALID_SIZE : Message size does not match expected size for + * the command ESP_ERR_INVALID_CRC : Checksum validation failed (if implemented) + * ESP_ERR_FAIL : Command execution failed (e.g., invalid parameters) + */ +esp_err_t driver_socket_api_process(uint8_t* buffer, size_t buffer_size); -#endif // _DRIVER_SOCKET_API_H_ +#endif // _DRIVER_SOCKET_API_H_ diff --git a/components/driver_wifi/driver_wifi.c b/components/driver_wifi/driver_wifi.c index 4311fbf..4dbaa10 100644 --- a/components/driver_wifi/driver_wifi.c +++ b/components/driver_wifi/driver_wifi.c @@ -1,4 +1,5 @@ #include "driver_wifi.h" + #include "esp_err.h" #include "esp_log.h" #include "esp_wifi.h" @@ -18,13 +19,13 @@ #elif CONFIG_DRIVER_WIFI_SAE_PWE_H2E #define DRIVER_WIFI_SAE_MODE WPA3_SAE_PWE_HASH_TO_ELEMENT -#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ +#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ CONFIG_DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER #elif CONFIG_DRIVER_WIFI_SAE_PWE_BOTH #define DRIVER_WIFI_SAE_MODE WPA3_SAE_PWE_BOTH -#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ +#define DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER \ CONFIG_DRIVER_WIFI_SAE_PASSWORD_IDENTIFIER #endif @@ -59,7 +60,7 @@ #endif -static const char *TAG = "DRIVER_WIFI"; +static const char* TAG = "DRIVER_WIFI"; /* FreeRTOS event group to signal when we are connected*/ static EventGroupHandle_t s_wifi_event_group; @@ -73,8 +74,8 @@ static EventGroupHandle_t s_wifi_event_group; static int s_retry_num = 0; -static void event_handler(void *arg, esp_event_base_t event_base, - int32_t event_id, void *event_data) { +static void event_handler(void* arg, esp_event_base_t event_base, + int32_t event_id, void* event_data) { if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { esp_wifi_connect(); } else if (event_base == WIFI_EVENT && @@ -88,15 +89,14 @@ static void event_handler(void *arg, esp_event_base_t event_base, } ESP_LOGI(TAG, "connect to the AP fail"); } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { - ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data; + ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data; ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip)); s_retry_num = 0; xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); } } -esp_err_t wifi_init(driver_wifi_config_t *config) { - +esp_err_t wifi_init(driver_wifi_config_t* config) { s_wifi_event_group = xEventGroupCreate(); if (s_wifi_event_group == NULL) { ESP_LOGE(TAG, "Unable to create wifi event group"); diff --git a/components/driver_wifi/include/driver_wifi.h b/components/driver_wifi/include/driver_wifi.h index dbb096a..d63d724 100644 --- a/components/driver_wifi/include/driver_wifi.h +++ b/components/driver_wifi/include/driver_wifi.h @@ -10,26 +10,26 @@ typedef struct { } driver_wifi_config_t; /** - * @brief Initializes the WiFi driver and connects to the specified network. This - * does not block until the connection is established, so you should call - * wait_for_wifi_connection() after this to block until the connection is ready. - * @param[in] config WiFi configuration (SSID and password) - * @return - * ESP_OK : WiFi initialization started successfully - * ESP_ERR_INVALID_ARG : Invalid configuration (e.g., empty SSID) - * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized - * ESP_ERR_WIFI_CONN : Failed to start WiFi connection process - */ -esp_err_t wifi_init(driver_wifi_config_t *config); + * @brief Initializes the WiFi driver and connects to the specified network. + * This does not block until the connection is established, so you should call + * wait_for_wifi_connection() after this to block until the connection is ready. + * @param[in] config WiFi configuration (SSID and password) + * @return + * ESP_OK : WiFi initialization started successfully + * ESP_ERR_INVALID_ARG : Invalid configuration (e.g., empty SSID) + * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized + * ESP_ERR_WIFI_CONN : Failed to start WiFi connection process + */ +esp_err_t wifi_init(driver_wifi_config_t* config); /** - * @brief Blocks until the WiFi connection is established. This should be called - * after wifi_init() to wait for the connection to be ready before proceeding. - * @return - * ESP_OK : WiFi connected successfully - * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized - * ESP_ERR_WIFI_CONN : Failed to connect to WiFi (e.g., wrong password) - */ + * @brief Blocks until the WiFi connection is established. This should be called + * after wifi_init() to wait for the connection to be ready before proceeding. + * @return + * ESP_OK : WiFi connected successfully + * ESP_ERR_WIFI_NOT_INIT : WiFi driver not initialized + * ESP_ERR_WIFI_CONN : Failed to connect to WiFi (e.g., wrong password) + */ esp_err_t wait_for_wifi_connection(); -#endif // _WIFI_H +#endif // _WIFI_H diff --git a/components/driver_wifi/test/test_driver_wifi.c b/components/driver_wifi/test/test_driver_wifi.c index f730f5e..819411b 100644 --- a/components/driver_wifi/test/test_driver_wifi.c +++ b/components/driver_wifi/test/test_driver_wifi.c @@ -8,6 +8,4 @@ TEST_SETUP(Driver_WiFi) {} TEST_TEAR_DOWN(Driver_WiFi) {} -TEST(Driver_WiFi, Driver_WiFi_Init) { - TEST_ASSERT_EQUAL(ESP_OK, ESP_FAIL); -} +TEST(Driver_WiFi, Driver_WiFi_Init) { TEST_ASSERT_EQUAL(ESP_OK, ESP_FAIL); } diff --git a/components/encoder/encoder.c b/components/encoder/encoder.c index 464f0e8..cad937d 100644 --- a/components/encoder/encoder.c +++ b/components/encoder/encoder.c @@ -1,158 +1,173 @@ #include "encoder.h" + +#include "driver/pulse_cnt.h" #include "esp_log.h" #include "sdkconfig.h" -#include "driver/pulse_cnt.h" - -static const char *TAG = "encoder"; -esp_err_t encoder_init(encoder_handle_t *handle, encoder_config_t *config) -{ - if (handle == NULL || config == NULL) { - return ESP_ERR_INVALID_ARG; - } - - ESP_LOGI(TAG, "install pcnt unit"); - pcnt_unit_config_t unit_config = { - .high_limit = ENCODER_HIGH_LIMIT, - .low_limit = ENCODER_LOW_LIMIT, - .flags.accum_count = 1 // accumulate the count in the event the count goes past limits (count is reset to 0) - }; - - esp_err_t ret = pcnt_new_unit(&unit_config, &handle->pcnt_unit); +static const char* TAG = "encoder"; + +esp_err_t encoder_init(encoder_handle_t* handle, encoder_config_t* config) { + if (handle == NULL || config == NULL) { + return ESP_ERR_INVALID_ARG; + } + + ESP_LOGI(TAG, "install pcnt unit"); + pcnt_unit_config_t unit_config = { + .high_limit = ENCODER_HIGH_LIMIT, + .low_limit = ENCODER_LOW_LIMIT, + .flags.accum_count = 1 // accumulate the count in the event the count + // goes past limits (count is reset to 0) + }; + + esp_err_t ret = pcnt_new_unit(&unit_config, &handle->pcnt_unit); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "set glitch filter"); + pcnt_glitch_filter_config_t filter_config = { + .max_glitch_ns = config->glitch_filter_ns, + }; + ret = pcnt_unit_set_glitch_filter(handle->pcnt_unit, &filter_config); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "install pcnt channels"); + pcnt_chan_config_t chan_a_config = { + .edge_gpio_num = config->P0_pin, + .level_gpio_num = config->P1_pin, + }; + pcnt_channel_handle_t pcnt_chan_a = NULL; + ret = pcnt_new_channel(handle->pcnt_unit, &chan_a_config, &pcnt_chan_a); + if (ret != ESP_OK) { + return ret; + } + + pcnt_chan_config_t chan_b_config = { + .edge_gpio_num = config->P1_pin, + .level_gpio_num = config->P0_pin, + }; + pcnt_channel_handle_t pcnt_chan_b = NULL; + ret = pcnt_new_channel(handle->pcnt_unit, &chan_b_config, &pcnt_chan_b); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "set edge and level actions for pcnt channels"); + if (config->invert_angle) { + ret = pcnt_channel_set_edge_action(pcnt_chan_a, + PCNT_CHANNEL_EDGE_ACTION_INCREASE, + PCNT_CHANNEL_EDGE_ACTION_DECREASE); if (ret != ESP_OK) { - return ret; + return ret; } - - ESP_LOGI(TAG, "set glitch filter"); - pcnt_glitch_filter_config_t filter_config = { - .max_glitch_ns = config->glitch_filter_ns, - }; - ret = pcnt_unit_set_glitch_filter(handle->pcnt_unit, &filter_config); + ret = pcnt_channel_set_edge_action(pcnt_chan_b, + PCNT_CHANNEL_EDGE_ACTION_DECREASE, + PCNT_CHANNEL_EDGE_ACTION_INCREASE); if (ret != ESP_OK) { - return ret; + return ret; } - - ESP_LOGI(TAG, "install pcnt channels"); - pcnt_chan_config_t chan_a_config = { - .edge_gpio_num = config->P0_pin, - .level_gpio_num = config->P1_pin, - }; - pcnt_channel_handle_t pcnt_chan_a = NULL; - ret = pcnt_new_channel(handle->pcnt_unit, &chan_a_config, &pcnt_chan_a); + } else { + ret = pcnt_channel_set_edge_action(pcnt_chan_a, + PCNT_CHANNEL_EDGE_ACTION_DECREASE, + PCNT_CHANNEL_EDGE_ACTION_INCREASE); if (ret != ESP_OK) { - return ret; + return ret; } - - pcnt_chan_config_t chan_b_config = { - .edge_gpio_num = config->P1_pin, - .level_gpio_num = config->P0_pin, - }; - pcnt_channel_handle_t pcnt_chan_b = NULL; - ret = pcnt_new_channel(handle->pcnt_unit, &chan_b_config, &pcnt_chan_b); + ret = pcnt_channel_set_edge_action(pcnt_chan_b, + PCNT_CHANNEL_EDGE_ACTION_INCREASE, + PCNT_CHANNEL_EDGE_ACTION_DECREASE); if (ret != ESP_OK) { - return ret; - } - - ESP_LOGI(TAG, "set edge and level actions for pcnt channels"); - if (config->invert_angle) { - ret = pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE); - if (ret != ESP_OK) { - return ret; - } - ret = pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE); - if (ret != ESP_OK) { - return ret; - } - } else { - ret = pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE); - if (ret != ESP_OK) { - return ret; - } - ret = pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE); - if (ret != ESP_OK) { - return ret; - } + return ret; } - ret = pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE); + } + ret = + pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, + PCNT_CHANNEL_LEVEL_ACTION_INVERSE); + if (ret != ESP_OK) { + return ret; + } + ret = + pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, + PCNT_CHANNEL_LEVEL_ACTION_INVERSE); + if (ret != ESP_OK) { + return ret; + } + + ESP_LOGI(TAG, "add limit watch points for count accumulation"); + int watch_points[] = {ENCODER_LOW_LIMIT, ENCODER_HIGH_LIMIT}; + for (size_t i = 0; i < sizeof(watch_points) / sizeof(watch_points[0]); i++) { + ret = pcnt_unit_add_watch_point(handle->pcnt_unit, watch_points[i]); if (ret != ESP_OK) { - return ret; - } - ret = pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE); - if (ret != ESP_OK) { - return ret; + return ret; } + } - ESP_LOGI(TAG, "add limit watch points for count accumulation"); - int watch_points[] = {ENCODER_LOW_LIMIT, ENCODER_HIGH_LIMIT}; - for (size_t i = 0; i < sizeof(watch_points) / sizeof(watch_points[0]); i++) { - ret = pcnt_unit_add_watch_point(handle->pcnt_unit, watch_points[i]); - if (ret != ESP_OK) {return ret;} - } - - return ESP_OK ; + return ESP_OK; } -esp_err_t encoder_start(encoder_handle_t *handle) { +esp_err_t encoder_start(encoder_handle_t* handle) { + pcnt_unit_handle_t pcnt_unit = handle->pcnt_unit; - pcnt_unit_handle_t pcnt_unit = handle->pcnt_unit; + ESP_LOGI(TAG, "enable pcnt unit"); + esp_err_t ret = pcnt_unit_enable(pcnt_unit); + if (ret != ESP_OK) { + return ret; + } - ESP_LOGI(TAG, "enable pcnt unit"); - esp_err_t ret = pcnt_unit_enable(pcnt_unit); - if (ret != ESP_OK) { - return ret; - } + ESP_LOGI(TAG, "clear pcnt unit"); + ret = pcnt_unit_clear_count(pcnt_unit); + if (ret != ESP_OK) { + return ret; + } - ESP_LOGI(TAG, "clear pcnt unit"); - ret = pcnt_unit_clear_count(pcnt_unit); - if (ret != ESP_OK) { - return ret; - } - - ESP_LOGI(TAG, "start pcnt unit"); - ret = pcnt_unit_start(pcnt_unit); - if (ret != ESP_OK) { - return ret; - } + ESP_LOGI(TAG, "start pcnt unit"); + ret = pcnt_unit_start(pcnt_unit); + if (ret != ESP_OK) { + return ret; + } - return ESP_OK ; + return ESP_OK; } - -esp_err_t encoder_get_raw_count(encoder_handle_t *handle, int *pulse_count) { - return pcnt_unit_get_count(handle->pcnt_unit, pulse_count); +esp_err_t encoder_get_raw_count(encoder_handle_t* handle, int* pulse_count) { + return pcnt_unit_get_count(handle->pcnt_unit, pulse_count); } -esp_err_t encoder_clear_count(encoder_handle_t *handle) { - return pcnt_unit_clear_count(handle->pcnt_unit); +esp_err_t encoder_clear_count(encoder_handle_t* handle) { + return pcnt_unit_clear_count(handle->pcnt_unit); } -esp_err_t encoder_get_wheel_angle(encoder_handle_t *handle, encoder_config_t *config, float *wheel_angle) { - - int pulse_count = 0; - esp_err_t ret = encoder_get_raw_count(handle, &pulse_count); - if (ret != ESP_OK) { - return ret; - } +esp_err_t encoder_get_wheel_angle(encoder_handle_t* handle, + encoder_config_t* config, + float* wheel_angle) { + int pulse_count = 0; + esp_err_t ret = encoder_get_raw_count(handle, &pulse_count); + if (ret != ESP_OK) { + return ret; + } - int resolution = config->resolution * 4; // Due to x4 quadrature encoding. + int resolution = config->resolution * 4; // Due to x4 quadrature encoding. - *wheel_angle = ((float)pulse_count * 360.0f) / resolution ; // Due to 360 degrees in 1 rotation. - - return ESP_OK ; + *wheel_angle = ((float)pulse_count * 360.0f) / + resolution; // Due to 360 degrees in 1 rotation. + return ESP_OK; } -esp_err_t encoder_get_limb_angle(encoder_handle_t *handle, encoder_config_t *config, float *limb_angle) { - float wheel_angle = 0.0f ; - esp_err_t ret = encoder_get_wheel_angle(handle, config, &wheel_angle); - if (ret != ESP_OK) { - return ret; - } +esp_err_t encoder_get_limb_angle(encoder_handle_t* handle, + encoder_config_t* config, float* limb_angle) { + float wheel_angle = 0.0f; + esp_err_t ret = encoder_get_wheel_angle(handle, config, &wheel_angle); + if (ret != ESP_OK) { + return ret; + } - float gear_ratio = config->gear_ratio ; - *limb_angle = config->limb_default ; + float gear_ratio = config->gear_ratio; + *limb_angle = config->limb_default; - *limb_angle += wheel_angle / gear_ratio ; + *limb_angle += wheel_angle / gear_ratio; - return ESP_OK ; + return ESP_OK; } \ No newline at end of file diff --git a/components/encoder/include/encoder.h b/components/encoder/include/encoder.h index 8f97d1a..4a4a761 100644 --- a/components/encoder/include/encoder.h +++ b/components/encoder/include/encoder.h @@ -1,31 +1,37 @@ -#include "esp_err.h" +#ifndef _ENCODER_H_ +#define _ENCODER_H_ -#include "driver/pulse_cnt.h" #include "driver/gpio.h" +#include "driver/pulse_cnt.h" +#include "esp_err.h" -#define ENCODER_LOW_LIMIT INT16_MIN // These are not configurable due to overflow functionality! -#define ENCODER_HIGH_LIMIT INT16_MAX // These are not configurable due to overflow functionality! - +#define ENCODER_LOW_LIMIT \ + INT16_MIN // These are not configurable due to overflow functionality! +#define ENCODER_HIGH_LIMIT \ + INT16_MAX // These are not configurable due to overflow functionality! typedef struct { - int P0_pin ; - int P1_pin ; - int glitch_filter_ns ; /**< Pulses shorter than this will not inc/dec the count */ - int resolution ; /**< Number of slots in the encoder wheel, used to calculate the real world angle */ - float gear_ratio ; /**< For every X number of rotations of the encoder wheel, the limb it controls moves once */ - float limb_default ; /**< Angle at which the limb hits the end stop switch */ - int invert_angle ; /**< Boolean flip sign of angle and count measurements */ -} encoder_config_t ; + int P0_pin; + int P1_pin; + int glitch_filter_ns; /**< Pulses shorter than this will not inc/dec the count + */ + int resolution; /**< Number of slots in the encoder wheel, used to calculate + the real world angle */ + float gear_ratio; /**< For every X number of rotations of the encoder wheel, + the limb it controls moves once */ + float limb_default; /**< Angle at which the limb hits the end stop switch */ + int invert_angle; /**< Boolean flip sign of angle and count measurements */ +} encoder_config_t; typedef struct { - pcnt_unit_handle_t pcnt_unit ; /**< specific internal PCNT hardware unit */ -} encoder_handle_t ; - + pcnt_unit_handle_t pcnt_unit; /**< specific internal PCNT hardware unit */ +} encoder_handle_t; /** * @brief Initialize an encoder * - * This function initializes the Pulse Counter hardware module for a specific encoder. + * This function initializes the Pulse Counter hardware module for a specific + * encoder. * * @param[out] handle Pointer to encoder handle structure * @param[in] config Pointer to configuration structure @@ -35,12 +41,13 @@ typedef struct { * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_init(encoder_handle_t *handle, encoder_config_t *config) ; +esp_err_t encoder_init(encoder_handle_t* handle, encoder_config_t* config); /** * @brief Start an encoder * - * Starts the Pulse Counter module for the specified encoder. The count is set to 0. + * Starts the Pulse Counter module for the specified encoder. The count is set + * to 0. * * @param[out] handle Pointer to encoder handle structure * @@ -49,13 +56,14 @@ esp_err_t encoder_init(encoder_handle_t *handle, encoder_config_t *config) ; * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_start(encoder_handle_t *handle) ; +esp_err_t encoder_start(encoder_handle_t* handle); /** * @brief Get pulse count of encoder * - * Retrieves the raw pulse count of the encoder. This count is signed, for every slot in - * the encoder wheel this count will increment or decrement (based on direction) by 4. + * Retrieves the raw pulse count of the encoder. This count is signed, for every + * slot in the encoder wheel this count will increment or decrement (based on + * direction) by 4. * * @param[out] handle Pointer to encoder handle structure * @param[out] pulse_count Pointer to pulse count value desired @@ -65,14 +73,14 @@ esp_err_t encoder_start(encoder_handle_t *handle) ; * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_get_raw_count(encoder_handle_t *handle, - int *pulse_count) ; +esp_err_t encoder_get_raw_count(encoder_handle_t* handle, int* pulse_count); /** * @brief Clear pulse count of encoder * - * Resets pulse count of the encoder to 0. This function should only be called when - * an end stop has triggered. If called otherwise, all absolute angles will be incorrect. + * Resets pulse count of the encoder to 0. This function should only be called + * when an end stop has triggered. If called otherwise, all absolute angles will + * be incorrect. * * @param[out] handle Pointer to encoder handle structure * @@ -81,14 +89,14 @@ esp_err_t encoder_get_raw_count(encoder_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_clear_count(encoder_handle_t *handle) ; +esp_err_t encoder_clear_count(encoder_handle_t* handle); /** * @brief Get wheel angle of encoder * - * Retrieves the angle of the encoder wheel. This angle is calculated using the - * current pulse count, default pulse count (0), and wheel resolution. This angle is - * signed, and can go outside 360. + * Retrieves the angle of the encoder wheel. This angle is calculated using the + * current pulse count, default pulse count (0), and wheel resolution. This + * angle is signed, and can go outside 360. * * @param[out] handle Pointer to encoder handle structure * @param[in] config Pointer to configuration structure @@ -99,16 +107,16 @@ esp_err_t encoder_clear_count(encoder_handle_t *handle) ; * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes */ -esp_err_t encoder_get_wheel_angle(encoder_handle_t *handle, - encoder_config_t *config, float *wheel_angle) ; +esp_err_t encoder_get_wheel_angle(encoder_handle_t* handle, + encoder_config_t* config, float* wheel_angle); /** * @brief Get limb angle of encoder * - * Retrieves the angle of the encoder wheel. This angle is calculated using the - * current pulse count, default pulse count (0), wheel resolution, limb gear ratio, and - * default limb angle. This angle is signed, and can go outside 360(though it may - * not depending on the robot anatomy). + * Retrieves the angle of the encoder wheel. This angle is calculated using the + * current pulse count, default pulse count (0), wheel resolution, limb gear + * ratio, and default limb angle. This angle is signed, and can go outside + * 360(though it may not depending on the robot anatomy). * * @param[out] handle Pointer to encoder handle structure * @param[in] config Pointer to configuration structure @@ -118,8 +126,8 @@ esp_err_t encoder_get_wheel_angle(encoder_handle_t *handle, * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes - */ -esp_err_t encoder_get_limb_angle(encoder_handle_t *handle, - encoder_config_t *config, float *limb_angle) ; - + */ +esp_err_t encoder_get_limb_angle(encoder_handle_t* handle, + encoder_config_t* config, float* limb_angle); +#endif /* _ENCODER_H_ */ diff --git a/components/encoder/test/test_encoder.c b/components/encoder/test/test_encoder.c index 9800710..cd3647f 100644 --- a/components/encoder/test/test_encoder.c +++ b/components/encoder/test/test_encoder.c @@ -1,34 +1,28 @@ #include "encoder.h" -#include "unity_fixture.h" #include "sdkconfig.h" +#include "unity_fixture.h" -TEST_GROUP(ENCODER) ; +TEST_GROUP(ENCODER); -static encoder_handle_t handle ; +static encoder_handle_t handle; static encoder_config_t config = { - .P0_pin = CONFIG_ENCODER_P0_TEST_PIN , - .P1_pin = CONFIG_ENCODER_P1_TEST_PIN , - .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER , - .resolution = CONFIG_ENCODER_0_RESOLUTION , - .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y , - .limb_default = CONFIG_ENCODER_0_LIMB_REFERENCE, - .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT - } ; - -TEST_SETUP(ENCODER) { - -} + .P0_pin = CONFIG_ENCODER_P0_TEST_PIN, + .P1_pin = CONFIG_ENCODER_P1_TEST_PIN, + .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, + .resolution = CONFIG_ENCODER_0_RESOLUTION, + .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / + (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y, + .limb_default = CONFIG_ENCODER_0_LIMB_REFERENCE, + .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT}; -TEST_TEAR_DOWN(ENCODER) { +TEST_SETUP(ENCODER) {} -} +TEST_TEAR_DOWN(ENCODER) {} TEST(ENCODER, Encoder_True_Initialization) { - - - esp_err_t err = encoder_init(&handle, &config); - TEST_ASSERT_EQUAL(ESP_OK, err); - TEST_ASSERT_NOT_EQUAL(NULL, &handle.pcnt_unit); + esp_err_t err = encoder_init(&handle, &config); + TEST_ASSERT_EQUAL(ESP_OK, err); + TEST_ASSERT_NOT_EQUAL(NULL, &handle.pcnt_unit); } // TEST(ENCODER, Encoder_Test_Initialization) { @@ -37,9 +31,10 @@ TEST(ENCODER, Encoder_True_Initialization) { // .P1_pin = -1 , // .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER , // .resolution = CONFIG_ENCODER_0_RESOLUTION , -// .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y , -// .limb_default = CONFIG_ENCODER_0_LIMB_REFERENCE, -// .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT +// .gear_ratio = (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_X / +// (float)CONFIG_ENCODER_0_LIMB_GEAR_RATIO_Y , .limb_default = +// CONFIG_ENCODER_0_LIMB_REFERENCE, .invert_angle = +// CONFIG_ENCODER_0_ANGLE_INVERT // } ; // esp_err_t err = encoder_init(&handle, &virtual_config); @@ -47,24 +42,18 @@ TEST(ENCODER, Encoder_True_Initialization) { // TEST_ASSERT_NOT_EQUAL(NULL, &handle.pcnt_unit); // } - - - -TEST_GROUP(ENCODER_INFINITE) ; +TEST_GROUP(ENCODER_INFINITE); TEST_SETUP(ENCODER_INFINITE) {} TEST_TEAR_DOWN(ENCODER_INFINITE) {} TEST(ENCODER_INFINITE, Encoder_Run_Infinite) { - - encoder_init(&handle, &config); - esp_err_t err = encoder_start(&handle) ; - TEST_ASSERT_EQUAL(ESP_OK, err) ; - - int pulse_count = 0; - while (true) { - TEST_ASSERT_EQUAL(encoder_get_raw_count(&handle, &pulse_count), ESP_OK); - printf("Pulse count: %d\n", pulse_count); - } - - + encoder_init(&handle, &config); + esp_err_t err = encoder_start(&handle); + TEST_ASSERT_EQUAL(ESP_OK, err); + + int pulse_count = 0; + while (true) { + TEST_ASSERT_EQUAL(encoder_get_raw_count(&handle, &pulse_count), ESP_OK); + printf("Pulse count: %d\n", pulse_count); + } } \ No newline at end of file diff --git a/components/encoder/test/test_runner.c b/components/encoder/test/test_runner.c index 6218969..83786a1 100644 --- a/components/encoder/test/test_runner.c +++ b/components/encoder/test/test_runner.c @@ -9,5 +9,4 @@ TEST_GROUP_RUNNER(ENCODER) { TEST_GROUP_RUNNER(ENCODER_INFINITE) { RUN_TEST_CASE(ENCODER_INFINITE, Encoder_Run_Infinite); - } diff --git a/components/i2c_bus/i2c_bus.c b/components/i2c_bus/i2c_bus.c index 2bc8a27..a2f334b 100644 --- a/components/i2c_bus/i2c_bus.c +++ b/components/i2c_bus/i2c_bus.c @@ -1,8 +1,8 @@ #include "i2c_bus.h" -#include "driver/i2c_master.h" -esp_err_t i2c_bus_init(i2c_bus_t *bus, const i2c_bus_config_t *config) { +#include "driver/i2c_master.h" +esp_err_t i2c_bus_init(i2c_bus_t* bus, const i2c_bus_config_t* config) { i2c_master_bus_config_t bus_config = { .i2c_port = config->port, .sda_io_num = config->sda_io_num, diff --git a/components/i2c_bus/include/i2c_bus.h b/components/i2c_bus/include/i2c_bus.h index 7041b48..f4a8218 100644 --- a/components/i2c_bus/include/i2c_bus.h +++ b/components/i2c_bus/include/i2c_bus.h @@ -2,8 +2,8 @@ #define _I2C_BUS_H_ #include "driver/i2c_types.h" -#include "soc/gpio_num.h" #include "esp_err.h" +#include "soc/gpio_num.h" /** * @brief I2C bus configuration structure @@ -12,9 +12,9 @@ * including the I2C port number and GPIO pin assignments. */ typedef struct { - i2c_port_t port; /**< I2C port number (I2C_NUM_0, I2C_NUM_1, etc.) */ - gpio_num_t sda_io_num; /**< GPIO number for SDA (data line) */ - gpio_num_t scl_io_num; /**< GPIO number for SCL (clock line) */ + i2c_port_t port; /**< I2C port number (I2C_NUM_0, I2C_NUM_1, etc.) */ + gpio_num_t sda_io_num; /**< GPIO number for SDA (data line) */ + gpio_num_t scl_io_num; /**< GPIO number for SCL (clock line) */ } i2c_bus_config_t; /** @@ -24,7 +24,7 @@ typedef struct { * for all subsequent operations on the I2C bus, including adding devices. */ typedef struct { - i2c_master_bus_handle_t handle; /**< ESP-IDF I2C master bus handle */ + i2c_master_bus_handle_t handle; /**< ESP-IDF I2C master bus handle */ } i2c_bus_t; /** @@ -40,11 +40,13 @@ typedef struct { * devices using the ESP-IDF i2c_master_bus_add_device() function. * * @param[out] bus Pointer to I2C bus structure to initialize - * @param[in] config Pointer to configuration structure with port and GPIO settings + * @param[in] config Pointer to configuration structure with port and GPIO + * settings * * @return * - ESP_OK: Success - * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer, invalid port, or invalid GPIO) + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer, invalid port, or + * invalid GPIO) * - ESP_ERR_NO_MEM: Memory allocation failed * - ESP_ERR_INVALID_STATE: I2C driver already installed on this port * - ESP_ERR_*: Other ESP-IDF error codes from I2C driver initialization @@ -68,6 +70,6 @@ typedef struct { * } * @endcode */ -esp_err_t i2c_bus_init(i2c_bus_t *bus, const i2c_bus_config_t *config); +esp_err_t i2c_bus_init(i2c_bus_t* bus, const i2c_bus_config_t* config); -#endif // _I2C_BUS_H_ +#endif // _I2C_BUS_H_ diff --git a/components/i2c_bus/test/test_i2c_bus.c b/components/i2c_bus/test/test_i2c_bus.c index 9d518b7..d56b731 100644 --- a/components/i2c_bus/test/test_i2c_bus.c +++ b/components/i2c_bus/test/test_i2c_bus.c @@ -16,8 +16,9 @@ TEST_TEAR_DOWN(I2C_Bus) { } TEST(I2C_Bus, I2C_Bus_Initialization) { - i2c_bus_config_t config = { - .port = I2C_NUM_0, .sda_io_num = CONFIG_I2CBUS_TEST_SDA_PIN, .scl_io_num = CONFIG_I2CBUS_TEST_SCL_PIN}; + i2c_bus_config_t config = {.port = I2C_NUM_0, + .sda_io_num = CONFIG_I2CBUS_TEST_SDA_PIN, + .scl_io_num = CONFIG_I2CBUS_TEST_SCL_PIN}; esp_err_t err = i2c_bus_init(&bus_handle, &config); TEST_ASSERT_EQUAL(ESP_OK, err); diff --git a/components/limit_switch/include/limit_switch.h b/components/limit_switch/include/limit_switch.h index 68bf19c..b6fa8fb 100644 --- a/components/limit_switch/include/limit_switch.h +++ b/components/limit_switch/include/limit_switch.h @@ -11,13 +11,14 @@ * @brief limit_switch configuration structure */ typedef struct { - gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ + gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ } limit_switch_config_t; /** * @brief Initialize the limit switch task * - * This function initializes the limit switch task and sets up the limit switch GPIO. + * This function initializes the limit switch task and sets up the limit switch + * GPIO. * * @param[in] config Pointer to configuration structure * @@ -25,6 +26,7 @@ typedef struct { * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument */ -esp_err_t limit_switch_init(const limit_switch_config_t *config, EventGroupHandle_t events); +esp_err_t limit_switch_init(const limit_switch_config_t* config, + EventGroupHandle_t events); -#endif // _LIMIT_SWITCH_H \ No newline at end of file +#endif // _LIMIT_SWITCH_H \ No newline at end of file diff --git a/components/limit_switch/limit_switch.c b/components/limit_switch/limit_switch.c index c116ccf..0d9070c 100644 --- a/components/limit_switch/limit_switch.c +++ b/components/limit_switch/limit_switch.c @@ -1,33 +1,37 @@ #include "limit_switch.h" + #include "esp_log.h" #include "freertos/FreeRTOS.h" #include "signal_bus.h" #define TAG "limit_switch" -static void IRAM_ATTR limit_switch_isr(void *arg){ - BaseType_t higher_priority_task_woken = pdFALSE; +static void IRAM_ATTR limit_switch_isr(void* arg) { + BaseType_t higher_priority_task_woken = pdFALSE; - xEventGroupSetBitsFromISR(g_motor_events, LIMIT_MOTOR_0, &higher_priority_task_woken); - portYIELD_FROM_ISR(higher_priority_task_woken); + xEventGroupSetBitsFromISR(g_motor_events, LIMIT_MOTOR_0, + &higher_priority_task_woken); + portYIELD_FROM_ISR(higher_priority_task_woken); } -esp_err_t limit_switch_init(const limit_switch_config_t *config, EventGroupHandle_t events){ - if (!config) { - return ESP_ERR_INVALID_ARG; - } - - // Configure GPIO interrupt service - gpio_config_t io_conf = { - .intr_type = GPIO_INTR_NEGEDGE, - .mode = GPIO_MODE_INPUT, - .pin_bit_mask = 1ULL << config->limit_gpio, - .pull_up_en = GPIO_PULLUP_ENABLE, - .pull_down_en = GPIO_PULLDOWN_DISABLE, - }; - - ESP_ERROR_CHECK(gpio_config(&io_conf)); - ESP_ERROR_CHECK(gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, NULL)); - - return ESP_OK; +esp_err_t limit_switch_init(const limit_switch_config_t* config, + EventGroupHandle_t events) { + if (!config) { + return ESP_ERR_INVALID_ARG; + } + + // Configure GPIO interrupt service + gpio_config_t io_conf = { + .intr_type = GPIO_INTR_NEGEDGE, + .mode = GPIO_MODE_INPUT, + .pin_bit_mask = 1ULL << config->limit_gpio, + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + }; + + ESP_ERROR_CHECK(gpio_config(&io_conf)); + ESP_ERROR_CHECK( + gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, NULL)); + + return ESP_OK; } \ No newline at end of file diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index fba0f05..d69b52c 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -1,14 +1,15 @@ #ifndef _MOTORHAT_H_ #define _MOTORHAT_H_ -#include "pca9685.h" #include + #include "freertos/FreeRTOS.h" #include "freertos/event_groups.h" +#include "pca9685.h" #define DEFAULT_FREQUENCY_HZ 1526.0f -/** +/** * @brief Motor enumeration */ typedef enum { @@ -19,44 +20,44 @@ typedef enum { MOTORHAT_NUM_MOTORS } motorhat_motor_t; -/** +/** * @brief Axis enumeration */ typedef enum { - MOTORHAT_AXIS_AZIMUTH = 0, - MOTORHAT_AXIS_ALTITUDE, - MOTORHAT_NUM_AXES + MOTORHAT_AXIS_AZIMUTH = 0, + MOTORHAT_AXIS_ALTITUDE, + MOTORHAT_NUM_AXES } motorhat_axis_t; -/** +/** * @brief Mapping of axes to motors */ static const motorhat_motor_t axis_motor[MOTORHAT_NUM_AXES] = { - [MOTORHAT_AXIS_AZIMUTH] = MOTORHAT_MOTOR1, - [MOTORHAT_AXIS_ALTITUDE] = MOTORHAT_MOTOR2, + [MOTORHAT_AXIS_AZIMUTH] = MOTORHAT_MOTOR3, + [MOTORHAT_AXIS_ALTITUDE] = MOTORHAT_MOTOR4, }; /** * @brief Motor direction and state control */ typedef enum { - MOTORHAT_DIRECTION_FORWARD = 0, /**< Motor rotates forward */ - MOTORHAT_DIRECTION_BACKWARD, /**< Motor rotates backward */ - MOTORHAT_DIRECTION_BRAKE, /**< Motor brakes (both inputs high) */ - MOTORHAT_DIRECTION_RELEASE /**< Motor released/coasting (both inputs low) */ + MOTORHAT_DIRECTION_FORWARD = 0, /**< Motor rotates forward */ + MOTORHAT_DIRECTION_BACKWARD, /**< Motor rotates backward */ + MOTORHAT_DIRECTION_BRAKE, /**< Motor brakes (both inputs high) */ + MOTORHAT_DIRECTION_RELEASE /**< Motor released/coasting (both inputs low) */ } motorhat_direction_t; /** * @brief Channel mapping for a single motor - * + * * Maps the three PCA9685 channels required to control one motor: * - IN1 and IN2: Direction control inputs to the H-bridge * - PWM: Speed control via pulse-width modulation */ typedef struct { - pca9685_channel_t in1_channel; /**< H-bridge IN1 control channel */ - pca9685_channel_t in2_channel; /**< H-bridge IN2 control channel */ - pca9685_channel_t pwm_channel; /**< PWM speed control channel */ + pca9685_channel_t in1_channel; /**< H-bridge IN1 control channel */ + pca9685_channel_t in2_channel; /**< H-bridge IN2 control channel */ + pca9685_channel_t pwm_channel; /**< PWM speed control channel */ } motorhat_motor_channels_t; /** @@ -64,20 +65,23 @@ typedef struct { */ typedef struct { pca9685_config_t pca9685_config; + uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ } motorhat_config_t; /** * @brief Motor HAT device handle */ typedef struct { - pca9685_handle_t pca9685; /**< Handle for the underlying PCA9685 controller */ - EventGroupHandle_t events; /**< Event group handle for motor fault signaling */ - EventBits_t stop_bits; /**< Which bits trigger motor stop */ + pca9685_handle_t pca9685; /**< Handle for the underlying PCA9685 controller */ + EventGroupHandle_t + events; /**< Event group handle for motor fault signaling */ + EventBits_t stop_bits; /**< Which bits trigger motor stop */ + uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ } motorhat_handle_t; /** * @brief Channel assignments for each motor on the Adafruit Motor HAT - * + * * This mapping is specific to the Adafruit Motor HAT v2 pinout: * - Motor 1: IN1=CH10, IN2=CH9, PWM=CH8 * - Motor 2: IN1=CH11, IN2=CH12, PWM=CH13 @@ -85,21 +89,22 @@ typedef struct { * - Motor 4: IN1=CH5, IN2=CH6, PWM=CH7 */ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { - {PCA9685_CHANNEL10, PCA9685_CHANNEL9, PCA9685_CHANNEL8}, // Motor 1 - {PCA9685_CHANNEL11, PCA9685_CHANNEL12, PCA9685_CHANNEL13}, // Motor 2 - {PCA9685_CHANNEL4, PCA9685_CHANNEL3, PCA9685_CHANNEL2}, // Motor 3 - {PCA9685_CHANNEL5, PCA9685_CHANNEL6, PCA9685_CHANNEL7} // Motor 4 + {PCA9685_CHANNEL10, PCA9685_CHANNEL9, PCA9685_CHANNEL8}, // Motor 1 + {PCA9685_CHANNEL11, PCA9685_CHANNEL12, PCA9685_CHANNEL13}, // Motor 2 + {PCA9685_CHANNEL4, PCA9685_CHANNEL3, PCA9685_CHANNEL2}, // Motor 3 + {PCA9685_CHANNEL5, PCA9685_CHANNEL6, PCA9685_CHANNEL7} // Motor 4 }; /** * @brief Initialize the Motor HAT controller * - * Initializes the underlying PCA9685 PWM controller with the provided configuration. - * After successful initialization, motors can be controlled using the speed and - * direction functions. + * Initializes the underlying PCA9685 PWM controller with the provided + * configuration. After successful initialization, motors can be controlled + * using the speed and direction functions. * * @param[out] handle Pointer to Motor HAT handle structure - * @param[in] config Pointer to configuration structure containing PCA9685 settings + * @param[in] config Pointer to configuration structure containing PCA9685 + * settings * @param[in] events Event group handle for signaling motor faults * * @return @@ -110,14 +115,17 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { * @note The handle->pca9685 pointer must be allocated and point to a valid * pca9685_handle_t structure before calling this function */ -esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *config, EventGroupHandle_t events); +esp_err_t motorhat_init(motorhat_handle_t* handle, + const motorhat_config_t* config, + EventGroupHandle_t events); /** * @brief Polar pan command * - * Triggers a polar pan movement based on the specified azimuth and altitude deltas, with - * timing parameters for delay and duration. This is a high-level command that can be - * implemented to control multiple motors in coordination to achieve the desired pan movement. + * Triggers a polar pan movement based on the specified azimuth and altitude + * deltas, with timing parameters for delay and duration. This is a high-level + * command that can be implemented to control multiple motors in coordination to + * achieve the desired pan movement. * * @param[in] delta_azimuth Azimuth delta for the pan movement * @param[in] delta_altitude Altitude delta for the pan movement @@ -126,22 +134,25 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf * * @return * - ESP_OK: Success - * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions */ esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, - uint16_t delay_ms, uint16_t time_ms); + uint16_t delay_ms, uint16_t time_ms); /** * @brief Polar pan start command * - * Starts a continuous pan movement that will run until a stop command is received. + * Starts a continuous pan movement that will run until a stop command is + * received. * * @param[in] delta_azimuth Azimuth delta for the pan movement * @param[in] delta_altitude Altitude delta for the pan movement * * @return * - ESP_OK: Success - * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions */ esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude); @@ -152,7 +163,8 @@ esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude); * * @return * - ESP_OK: Success - * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions */ esp_err_t motorhat_polar_pan_stop(void); @@ -165,7 +177,8 @@ esp_err_t motorhat_polar_pan_stop(void); * * @return * - ESP_OK: Success - * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control functions + * - ESP_ERR_*: Other ESP-IDF error codes propogated from motor control + * functions */ esp_err_t motorhat_home(uint16_t delay_ms); @@ -181,14 +194,15 @@ esp_err_t motorhat_home(uint16_t delay_ms); * * @return * - ESP_OK: Success - * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or speed > 4096) + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or + * speed > 4096) * - ESP_ERR_INVALID_STATE: Motor is in fault state (stop bits set) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations * * @note For typical usage with 0-255 speed range, scale your value: * actual_speed = (your_speed * PCA9685_PWM_MAX) / 255 */ -esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_speed(motorhat_handle_t* handle, motorhat_motor_t motor, uint16_t speed); /** @@ -197,7 +211,7 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, * Controls the motor's direction and braking state by setting the IN1 and IN2 * control signals appropriately: * - FORWARD: IN1=HIGH, IN2=LOW - * - BACKWARD: IN1=LOW, IN2=HIGH + * - BACKWARD: IN1=LOW, IN2=HIGH * - BRAKE: IN1=HIGH, IN2=HIGH (active braking) * - RELEASE: IN1=LOW, IN2=LOW (coasting/free-running) * @@ -207,14 +221,15 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, * * @return * - ESP_OK: Success - * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or invalid direction) + * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle, invalid motor, or + * invalid direction) * - ESP_ERR_INVALID_STATE: Motor is in fault state (stop bits set) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations * * @note Setting direction does not affect speed. Set speed separately using * motorhat_set_motor_speed() */ -esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, motorhat_motor_t motor, motorhat_direction_t direction); @@ -230,6 +245,6 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) */ -esp_err_t motorhat_emergency_stop(motorhat_handle_t *handle); +esp_err_t motorhat_emergency_stop(motorhat_handle_t* handle); -#endif // _MOTORHAT_H_ +#endif // _MOTORHAT_H_ diff --git a/components/motorhat/include/pca9685.h b/components/motorhat/include/pca9685.h index c05cccf..fdb2803 100644 --- a/components/motorhat/include/pca9685.h +++ b/components/motorhat/include/pca9685.h @@ -1,9 +1,10 @@ #ifndef _PCA9685_H_ #define _PCA9685_H_ +#include + #include "driver/i2c_types.h" #include "esp_err.h" -#include #define PCA9685_PWM_MAX 4096 #define PCA9685_I2C_MASTER_TIMEOUT_MS 1000 @@ -16,8 +17,8 @@ // Calculate pre-scale value for a desired PWM frequency // See PCA9685 datasheet section 7.3.5 // The + 0.5f is for rounding to nearest integer without truncation -#define PCA9685_CALC_PRE_SCALE(freq_hz) \ - ((uint8_t)((PCA9685_OSCILLATOR_FREQ_HZ / (4096.0f * (freq_hz))) - 1.0f + \ +#define PCA9685_CALC_PRE_SCALE(freq_hz) \ + ((uint8_t)((PCA9685_OSCILLATOR_FREQ_HZ / (4096.0f * (freq_hz))) - 1.0f + \ 0.5f)) /** @@ -204,13 +205,14 @@ typedef struct { * frequency) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_init(pca9685_handle_t *handle, - const pca9685_config_t *config); +esp_err_t pca9685_init(pca9685_handle_t* handle, + const pca9685_config_t* config); /** * @brief Deinitialize the PCA9685 device * - * This function releases the I2C device handle and performs any necessary cleanup. + * This function releases the I2C device handle and performs any necessary + * cleanup. * * @param[in] handle Pointer to PCA9685 handle structure * @return @@ -218,7 +220,7 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ - esp_err_t pca9685_deinit(pca9685_handle_t *handle); +esp_err_t pca9685_deinit(pca9685_handle_t* handle); /** * @brief Set PWM duty cycle for a specific channel @@ -238,7 +240,7 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, * duty cycle > 4096) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, +esp_err_t pca9685_set_duty_cycle(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t duty_cycle); @@ -257,7 +259,7 @@ esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle or invalid channel) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_digital_write(pca9685_handle_t *handle, +esp_err_t pca9685_digital_write(pca9685_handle_t* handle, pca9685_channel_t channel, bool level); /** @@ -279,7 +281,7 @@ esp_err_t pca9685_digital_write(pca9685_handle_t *handle, * values > 4096) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, +esp_err_t pca9685_write_channel_registers(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t on, uint16_t off); @@ -295,8 +297,8 @@ esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL pointer) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_read_register(pca9685_handle_t *handle, - pca9685_register_t reg, uint8_t *data); +esp_err_t pca9685_read_register(pca9685_handle_t* handle, + pca9685_register_t reg, uint8_t* data); /** * @brief Write a single byte to a PCA9685 register @@ -310,7 +312,7 @@ esp_err_t pca9685_read_register(pca9685_handle_t *handle, * - ESP_ERR_INVALID_ARG: Invalid argument (NULL handle) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t pca9685_write_register(pca9685_handle_t *handle, +esp_err_t pca9685_write_register(pca9685_handle_t* handle, pca9685_register_t reg, uint8_t data); -#endif // _PCA9685_H_ +#endif // _PCA9685_H_ diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 51716ea..04abcb5 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -1,43 +1,43 @@ #include "motorhat.h" + #include "esp_err.h" #include "esp_log.h" #include "signal_bus.h" #define TAG "motorhat" -#define POLAR_PAN_SPEED PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED) - -static motorhat_handle_t *s_handle = NULL; +static motorhat_handle_t* s_handle = NULL; static motorhat_direction_t delta_to_direction(int8_t delta) { - if (delta > 0) return MOTORHAT_DIRECTION_FORWARD; - if (delta < 0) return MOTORHAT_DIRECTION_BACKWARD; - return MOTORHAT_DIRECTION_BRAKE; + if (delta > 0) return MOTORHAT_DIRECTION_FORWARD; + if (delta < 0) return MOTORHAT_DIRECTION_BACKWARD; + + // + return MOTORHAT_DIRECTION_RELEASE; } -void motor_stop_task(void *args) { - motorhat_handle_t *handle = (motorhat_handle_t *)args; - - while (1) { - // Block indefinitely until any stop bit is set - xEventGroupWaitBits(handle->events, - handle->stop_bits, - pdFALSE, // don't clear bits on exit - pdFALSE, // any bit (OR) - portMAX_DELAY); - - motorhat_emergency_stop(handle); - - // Wait until all stop bits are cleared before watching again - xEventGroupWaitBits(handle->events, - handle->stop_bits, - pdFALSE, - pdTRUE, // all bits (AND) - portMAX_DELAY); - } +void motor_stop_task(void* args) { + motorhat_handle_t* handle = (motorhat_handle_t*)args; + + while (1) { + // Block indefinitely until any stop bit is set + xEventGroupWaitBits(handle->events, handle->stop_bits, + pdFALSE, // don't clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); + + motorhat_emergency_stop(handle); + + // Wait until all stop bits are cleared before watching again + xEventGroupWaitBits(handle->events, handle->stop_bits, pdFALSE, + pdTRUE, // all bits (AND) + portMAX_DELAY); + } } -esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *config, EventGroupHandle_t events) { +esp_err_t motorhat_init(motorhat_handle_t* handle, + const motorhat_config_t* config, + EventGroupHandle_t events) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } @@ -45,6 +45,7 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf s_handle = handle; handle->events = events; handle->stop_bits = FAULT_ANY; + handle->polar_pan_speed = config->polar_pan_speed; xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); @@ -52,67 +53,83 @@ esp_err_t motorhat_init(motorhat_handle_t *handle, const motorhat_config_t *conf return ESP_OK; } - // Socket commands esp_err_t motorhat_home(uint16_t delay_ms) { - ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); - // TODO: Implement homing logic using motorhat_set_motor_direction, motorhat_set_motor_speed, and limit switch state from signal bus - return ESP_OK; + ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); + // TODO: Implement homing logic using motorhat_set_motor_direction, + // motorhat_set_motor_speed, and limit switch state from signal bus + return ESP_OK; } esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, - uint16_t delay_ms, uint16_t time_ms) { - ESP_LOGI(TAG, "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", - delta_azimuth, delta_altitude, delay_ms, time_ms); - // TODO: Implement polar pan logic using motorhat_set_motor_direction and motorhat_set_motor_speed - return ESP_OK; + uint16_t delay_ms, uint16_t time_ms) { + ESP_LOGI(TAG, + "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, " + "delay_ms=%d, time_ms=%d", + delta_azimuth, delta_altitude, delay_ms, time_ms); + // TODO: Implement polar pan logic using motorhat_set_motor_direction and + // motorhat_set_motor_speed + return ESP_OK; } -esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude) { +esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, + int8_t delta_altitude) { if (s_handle == NULL) return ESP_ERR_INVALID_STATE; - ESP_LOGI(TAG, "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", - delta_azimuth, delta_altitude); + ESP_LOGI( + TAG, + "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", + delta_azimuth, delta_altitude); // Set both axes speed to 0 to prevent direction change while moving - esp_err_t err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], 0); + esp_err_t err = + motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], 0); if (err != ESP_OK) { return err; } - err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], 0); + err = + motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], 0); if (err != ESP_OK) { return err; } + ESP_LOGI(TAG, "Direction Azimuth %d", delta_to_direction(delta_azimuth)); + ESP_LOGI(TAG, "Direction Altitude %d", delta_to_direction(delta_altitude)); + // Set directions based on deltas - err = motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], delta_to_direction(delta_azimuth)); + err = + motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], + delta_to_direction(delta_azimuth)); if (err != ESP_OK) { return err; } - err = motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], delta_to_direction(delta_altitude)); + err = + motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], + delta_to_direction(delta_altitude)); if (err != ESP_OK) { return err; } // Set speeds to a default value - err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], POLAR_PAN_SPEED); + err = motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], + s_handle->polar_pan_speed); if (err != ESP_OK) { return err; } - return motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], POLAR_PAN_SPEED); + return motorhat_set_motor_speed(s_handle, axis_motor[MOTORHAT_AXIS_ALTITUDE], + s_handle->polar_pan_speed); } esp_err_t motorhat_polar_pan_stop(void) { - ESP_LOGI(TAG, "Received polar pan stop command"); - for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { - motorhat_set_motor_speed(s_handle, m, 0); - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BRAKE); - } - return ESP_OK; + ESP_LOGI(TAG, "Received polar pan stop command"); + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BRAKE); + } + return ESP_OK; } - // Motor control to be used by socket command functions -esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_speed(motorhat_handle_t* handle, motorhat_motor_t motor, uint16_t speed) { if (handle == NULL || motor < MOTORHAT_MOTOR1 || motor >= MOTORHAT_NUM_MOTORS) { @@ -128,13 +145,13 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t *handle, return ESP_ERR_INVALID_ARG; } - const motorhat_motor_channels_t *channels = &motor_channels[motor]; + const motorhat_motor_channels_t* channels = &motor_channels[motor]; return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); return ESP_OK; } -esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, +esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, motorhat_motor_t motor, motorhat_direction_t direction) { if (handle == NULL || motor < MOTORHAT_MOTOR1 || @@ -147,66 +164,70 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t *handle, return ESP_ERR_INVALID_STATE; } - const motorhat_motor_channels_t *channels = &motor_channels[motor]; + const motorhat_motor_channels_t* channels = &motor_channels[motor]; esp_err_t err; switch (direction) { - case MOTORHAT_DIRECTION_FORWARD: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); - err = ESP_OK; - break; - case MOTORHAT_DIRECTION_BACKWARD: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); - err = ESP_OK; - break; - case MOTORHAT_DIRECTION_BRAKE: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); - err = ESP_OK; - break; - case MOTORHAT_DIRECTION_RELEASE: - err = pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); - if (err != ESP_OK) - return err; - err = pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); - err = ESP_OK; - break; - default: - return ESP_ERR_INVALID_ARG; + case MOTORHAT_DIRECTION_FORWARD: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + err = ESP_OK; + break; + case MOTORHAT_DIRECTION_BACKWARD: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + err = ESP_OK; + break; + case MOTORHAT_DIRECTION_BRAKE: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, true); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, true); + err = ESP_OK; + break; + case MOTORHAT_DIRECTION_RELEASE: + err = + pca9685_digital_write(&handle->pca9685, channels->in1_channel, false); + if (err != ESP_OK) return err; + err = + pca9685_digital_write(&handle->pca9685, channels->in2_channel, false); + err = ESP_OK; + break; + default: + return ESP_ERR_INVALID_ARG; } return err; } - // Emergency stop all motors that bypasses normal logic flow -esp_err_t motorhat_emergency_stop(motorhat_handle_t *handle) { - if (handle == NULL) { - return ESP_ERR_INVALID_ARG; - } +esp_err_t motorhat_emergency_stop(motorhat_handle_t* handle) { + if (handle == NULL) { + return ESP_ERR_INVALID_ARG; + } - esp_err_t first_err = ESP_OK; - for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { - const motorhat_motor_channels_t *ch = &motor_channels[m]; + esp_err_t first_err = ESP_OK; + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + const motorhat_motor_channels_t* ch = &motor_channels[m]; - // Stop PWM cycle and release both control lines - esp_err_t err = pca9685_set_duty_cycle(&handle->pca9685, ch->pwm_channel, 0); - if (err != ESP_OK && first_err == ESP_OK) first_err = err; + // Stop PWM cycle and release both control lines + esp_err_t err = + pca9685_set_duty_cycle(&handle->pca9685, ch->pwm_channel, 0); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; - err = pca9685_digital_write(&handle->pca9685, ch->in1_channel, false); - if (err != ESP_OK && first_err == ESP_OK) first_err = err; + err = pca9685_digital_write(&handle->pca9685, ch->in1_channel, false); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; - err = pca9685_digital_write(&handle->pca9685, ch->in2_channel, false); - if (err != ESP_OK && first_err == ESP_OK) first_err = err; - } - return first_err; + err = pca9685_digital_write(&handle->pca9685, ch->in2_channel, false); + if (err != ESP_OK && first_err == ESP_OK) first_err = err; + } + return first_err; } diff --git a/components/motorhat/pca9685.c b/components/motorhat/pca9685.c index ba11317..1ca65d8 100644 --- a/components/motorhat/pca9685.c +++ b/components/motorhat/pca9685.c @@ -1,8 +1,10 @@ #include "pca9685.h" + +#include + #include "driver/i2c_master.h" #include "esp_err.h" #include "freertos/FreeRTOS.h" -#include static pca9685_channel_registers_t channel_regs[PCA9685_CHANNEL15 + 1] = { {PCA9685_LED0_ON_L, PCA9685_LED0_ON_H, PCA9685_LED0_OFF_L, @@ -39,8 +41,8 @@ static pca9685_channel_registers_t channel_regs[PCA9685_CHANNEL15 + 1] = { PCA9685_LED15_OFF_H}, }; -esp_err_t pca9685_init(pca9685_handle_t *handle, - const pca9685_config_t *config) { +esp_err_t pca9685_init(pca9685_handle_t* handle, + const pca9685_config_t* config) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } @@ -98,9 +100,9 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, vTaskDelay(pdMS_TO_TICKS(5)); // Enable auto increment - ret = pca9685_write_register(handle, PCA9685_MODE1, - oldmode | PCA9685_MODE1_RESTART | - PCA9685_MODE1_AI); + ret = pca9685_write_register( + handle, PCA9685_MODE1, + oldmode | PCA9685_MODE1_RESTART | PCA9685_MODE1_AI); if (ret != ESP_OK) { return ret; } @@ -108,7 +110,7 @@ esp_err_t pca9685_init(pca9685_handle_t *handle, return ESP_OK; } -esp_err_t pca9685_deinit(pca9685_handle_t *handle) { +esp_err_t pca9685_deinit(pca9685_handle_t* handle) { if (handle == NULL || handle->dev_handle == NULL) { return ESP_ERR_INVALID_ARG; } @@ -122,7 +124,7 @@ esp_err_t pca9685_deinit(pca9685_handle_t *handle) { return ESP_OK; } -esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, +esp_err_t pca9685_set_duty_cycle(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t duty_cycle) { if (handle == NULL || channel < PCA9685_CHANNEL0 || @@ -149,7 +151,7 @@ esp_err_t pca9685_set_duty_cycle(pca9685_handle_t *handle, return pca9685_write_channel_registers(handle, channel, on, off); } -esp_err_t pca9685_digital_write(pca9685_handle_t *handle, +esp_err_t pca9685_digital_write(pca9685_handle_t* handle, pca9685_channel_t channel, bool level) { if (handle == NULL || channel < PCA9685_CHANNEL0 || channel > PCA9685_CHANNEL15) { @@ -163,7 +165,7 @@ esp_err_t pca9685_digital_write(pca9685_handle_t *handle, } } -esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, +esp_err_t pca9685_write_channel_registers(pca9685_handle_t* handle, pca9685_channel_t channel, uint16_t on, uint16_t off) { if (handle == NULL || channel < PCA9685_CHANNEL0 || @@ -177,7 +179,6 @@ esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, pca9685_channel_registers_t regs = channel_regs[channel]; - // The PCA9685 uses 12-bit PWM values for each channel (0–4095). // These 12 bits are split across two 8-bit registers: // LEDn_ON_L -> bits 7..0 (low byte) @@ -207,17 +208,17 @@ esp_err_t pca9685_write_channel_registers(pca9685_handle_t *handle, PCA9685_I2C_MASTER_TIMEOUT_MS); } -esp_err_t pca9685_read_register(pca9685_handle_t *handle, - pca9685_register_t reg, uint8_t *data) { +esp_err_t pca9685_read_register(pca9685_handle_t* handle, + pca9685_register_t reg, uint8_t* data) { if (handle == NULL || data == NULL) { return ESP_ERR_INVALID_ARG; } - return i2c_master_transmit_receive(handle->dev_handle, (uint8_t *)®, 1, + return i2c_master_transmit_receive(handle->dev_handle, (uint8_t*)®, 1, data, 1, PCA9685_I2C_MASTER_TIMEOUT_MS); } -esp_err_t pca9685_write_register(pca9685_handle_t *handle, +esp_err_t pca9685_write_register(pca9685_handle_t* handle, pca9685_register_t reg, uint8_t data) { if (handle == NULL) { return ESP_ERR_INVALID_ARG; diff --git a/components/motorhat/test/test_pca9685.c b/components/motorhat/test/test_pca9685.c index fe548b1..883a82b 100644 --- a/components/motorhat/test/test_pca9685.c +++ b/components/motorhat/test/test_pca9685.c @@ -40,9 +40,8 @@ TEST(PCA9685, PCA9685_Initialization) { } TEST(PCA9685, PCA9685_Wrong_Address) { - pca9685_config_t config = { - .i2c_addr = 0x00, // Invalid address + .i2c_addr = 0x00, // Invalid address .i2c_speed_hz = 400000, .pwm_freq_hz = 1000, .bus_handle = bus_handle.handle, @@ -67,10 +66,14 @@ TEST(PCA9685, PCA9685_Set_Duty_Cycle) { // Read back the registers to verify uint8_t on_l, on_h, off_l, off_h; - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_L, &on_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_H, &on_h), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_L, &off_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_H, &off_h), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_L, &on_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_ON_H, &on_h), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_L, &off_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED0_OFF_H, &off_h), + ESP_OK); TEST_ASSERT_EQUAL(0, on_l); uint16_t on = (on_h << 8) | on_l; uint16_t off = (off_h << 8) | off_l; @@ -90,27 +93,37 @@ TEST(PCA9685, PCA9685_Digital_Write) { TEST_ASSERT_EQUAL(pca9685_init(&handle, &config), ESP_OK); // Set channel high - TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, true), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, true), + ESP_OK); // Read back the registers to verify uint8_t on_l, on_h, off_l, off_h; - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), + ESP_OK); uint16_t on = (on_h << 8) | on_l; uint16_t off = (off_h << 8) | off_l; TEST_ASSERT_EQUAL(PCA9685_PWM_MAX, on); TEST_ASSERT_EQUAL(0, off); // Set channel low - TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, false), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_digital_write(&handle, PCA9685_CHANNEL1, false), + ESP_OK); // Read back the registers to verify - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), ESP_OK); - TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_L, &on_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_ON_H, &on_h), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_L, &off_l), + ESP_OK); + TEST_ASSERT_EQUAL(pca9685_read_register(&handle, PCA9685_LED1_OFF_H, &off_h), + ESP_OK); on = (on_h << 8) | on_l; off = (off_h << 8) | off_l; TEST_ASSERT_EQUAL(0, on); diff --git a/components/signal_bus/include/signal_bus.h b/components/signal_bus/include/signal_bus.h index e9167c0..7a1122b 100644 --- a/components/signal_bus/include/signal_bus.h +++ b/components/signal_bus/include/signal_bus.h @@ -1,23 +1,23 @@ #ifndef _SIGNAL_BUS_H #define _SIGNAL_BUS_H +#include "esp_err.h" #include "freertos/FreeRTOS.h" #include "freertos/event_groups.h" -#include "esp_err.h" // Motor limit switch and fault event bits -#define LIMIT_MOTOR_0 (1 << 0) -#define LIMIT_MOTOR_1 (1 << 1) -#define LIMIT_MOTOR_2 (1 << 2) -#define LIMIT_MOTOR_3 (1 << 3) -#define LIMIT_MOTOR_4 (1 << 4) -#define LIMIT_MOTOR_5 (1 << 5) -#define LIMIT_MOTOR_6 (1 << 6) +#define LIMIT_MOTOR_0 (1 << 0) +#define LIMIT_MOTOR_1 (1 << 1) +#define LIMIT_MOTOR_2 (1 << 2) +#define LIMIT_MOTOR_3 (1 << 3) +#define LIMIT_MOTOR_4 (1 << 4) +#define LIMIT_MOTOR_5 (1 << 5) +#define LIMIT_MOTOR_6 (1 << 6) #define ESTOP_OVERCURRENT (1 << 7) -#define ESTOP_ANY (ESTOP_OVERCURRENT) -#define LIMIT_ANY (0x7F) // bits 0-6 -#define FAULT_ANY (LIMIT_ANY | ESTOP_ANY) +#define ESTOP_ANY (ESTOP_OVERCURRENT) +#define LIMIT_ANY (0x7F) // bits 0-6 +#define FAULT_ANY (LIMIT_ANY | ESTOP_ANY) extern EventGroupHandle_t g_motor_events; @@ -33,4 +33,4 @@ extern EventGroupHandle_t g_motor_events; */ esp_err_t signal_bus_init(); -#endif // _SIGNAL_BUS_H \ No newline at end of file +#endif // _SIGNAL_BUS_H \ No newline at end of file diff --git a/components/signal_bus/signal_bus.c b/components/signal_bus/signal_bus.c index 5a06f7b..f4b234b 100644 --- a/components/signal_bus/signal_bus.c +++ b/components/signal_bus/signal_bus.c @@ -1,4 +1,5 @@ #include "signal_bus.h" + #include "esp_log.h" #define TAG "signal_bus" @@ -6,13 +7,13 @@ EventGroupHandle_t g_motor_events = NULL; esp_err_t signal_bus_init(void) { - ESP_LOGI(TAG, "Initializing signal bus..."); + ESP_LOGI(TAG, "Initializing signal bus..."); - g_motor_events = xEventGroupCreate(); + g_motor_events = xEventGroupCreate(); - if (g_motor_events == NULL) { - return ESP_FAIL; - } else{ - return ESP_OK; - } + if (g_motor_events == NULL) { + return ESP_FAIL; + } else { + return ESP_OK; + } } diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index ec32166..e10dd10 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -29,12 +29,6 @@ menu "Talos ESP Driver Configuration" help The fraction of PCA9685's max PWM speed to use when driving polar pan. - config DRIVER_ADS1015_SDA_PIN - int "ADS1015 SDA Pin" - default 18 - help - GPIO pin number for the ADS1015 SDA line. - config DRIVER_ADS1015_ADDRESS hex "ADS1015 I2C Address" default 0x48 diff --git a/main/esp_driver.c b/main/esp_driver.c index 7ef030b..e2252dd 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -1,42 +1,33 @@ +#include "ads1015.h" +#include "driver/gpio.h" #include "driver_socket.h" +#include "driver_socket_api.h" #include "driver_wifi.h" +#include "encoder.h" #include "esp_err.h" #include "esp_log.h" #include "freertos/FreeRTOS.h" #include "i2c_bus.h" - -#include "ads1015.h" #include "limit_switch.h" -#include "encoder.h" #include "motorhat.h" -#include "signal_bus.h" -#include "driver_socket_api.h" #include "nvs_flash.h" -#include "limit_switch.h" - -#include "driver/gpio.h" +#include "pca9685.h" +#include "signal_bus.h" #define TAG "MAIN" -#include "encoder.h" -#define ENCODER_GLITCH_FILTER 100 //CONFIG_ENCODER_GLITCH_FILTER +#define ENCODER_GLITCH_FILTER 100 // CONFIG_ENCODER_GLITCH_FILTER #define ENCODER_STANDARD_RESOLUTION 20 +#define POLAR_PAN_SPEED static const driver_socket_api_motor_interface_t motor_interface = { - .polar_pan = motorhat_polar_pan, + .polar_pan = motorhat_polar_pan, .polar_pan_start = motorhat_polar_pan_start, - .polar_pan_stop = motorhat_polar_pan_stop, - .home = motorhat_home, + .polar_pan_stop = motorhat_polar_pan_stop, + .home = motorhat_home, }; void app_main(void) { - - // Install GPIO interrupt service - ESP_ERROR_CHECK(gpio_install_isr_service(0)); - - // Initialize signal bus - signal_bus_init(); - // Initialize NVS esp_err_t ret = nvs_flash_init(); if (ret == ESP_ERR_NVS_NO_FREE_PAGES || @@ -46,6 +37,11 @@ void app_main(void) { } ESP_ERROR_CHECK(ret); + // Install GPIO interrupt service + ESP_ERROR_CHECK(gpio_install_isr_service(0)); + + // Initialize signal bus + ESP_ERROR_CHECK(signal_bus_init()); // Initialize I2C bus i2c_bus_t bus; @@ -56,14 +52,12 @@ void app_main(void) { }; ESP_ERROR_CHECK(i2c_bus_init(&bus, &bus_config)); - // Initialize limit switches limit_switch_config_t limit_switch_config = { .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, }; ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config, g_motor_events)); - // Initialize current sensing ads1015_handle_t ads; ads1015_config_t ads_config = { @@ -75,7 +69,6 @@ void app_main(void) { }; ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config, g_motor_events)); - // Initialize encoders encoder_handle_t encoder; encoder_config_t encoder_config = { @@ -88,7 +81,6 @@ void app_main(void) { ESP_ERROR_CHECK(encoder_init(&encoder, &encoder_config)); ESP_ERROR_CHECK(encoder_start(&encoder)); - // Initialize motor controller motorhat_handle_t motorhat; motorhat_config_t motorhat_config = { @@ -99,10 +91,11 @@ void app_main(void) { .pwm_freq_hz = DEFAULT_FREQUENCY_HZ, .bus_handle = bus.handle, }, + .polar_pan_speed = + PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED), }; ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config, g_motor_events)); - // Initialize Wi-Fi and socket connection driver_wifi_config_t wifi_config = { .ssid = CONFIG_DRIVER_WIFI_SSID, @@ -119,8 +112,8 @@ void app_main(void) { .port = CONFIG_DRIVER_SERVER_PORT, }; - ESP_ERROR_CHECK(driver_socket_init(&socket_handle, &socket_config, &motor_interface)); - + ESP_ERROR_CHECK( + driver_socket_init(&socket_handle, &socket_config, &motor_interface)); int pulse_count; @@ -130,9 +123,11 @@ void app_main(void) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); } - vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop + vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop - // This loop can also be used to add periodic tasks like reading sensors, or checking limit switches. - // However, it must not terminate or structs initialized here will disappear from stack memory, breaking modules that use them. + // This loop can also be used to add periodic tasks like reading sensors, or + // checking limit switches. However, it must not terminate or structs + // initialized here will disappear from stack memory, breaking modules that + // use them. } } diff --git a/test/main/esp_driver_test.c b/test/main/esp_driver_test.c index 17aa69a..fd5aa6c 100644 --- a/test/main/esp_driver_test.c +++ b/test/main/esp_driver_test.c @@ -1,32 +1,27 @@ +#include "driver/gpio.h" +#include "test_menu.h" #include "unity.h" #include "unity_fixture.h" -#include "test_menu.h" -#include "driver/gpio.h" - // NOTE: Define Groups here, and they will be automatically added to the menu #define TEST_GROUPS \ - X(I2C_Bus) \ - X(PCA9685) \ - X(MotorHAT) \ - X(ENCODER) \ - X(ADS1015) \ - X(Driver_WiFi) \ - + X(I2C_Bus) \ + X(PCA9685) \ + X(MotorHAT) \ + X(ENCODER) \ + X(ADS1015) \ + X(Driver_WiFi) -#define X(g) static void run_##g(void) { RUN_TEST_GROUP(g); } +#define X(g) \ + static void run_##g(void) { RUN_TEST_GROUP(g); } TEST_GROUPS #undef X - -#define X(g) { #g, run_##g }, -static const test_group_t groups[] = { - TEST_GROUPS -}; +#define X(g) {#g, run_##g}, +static const test_group_t groups[] = {TEST_GROUPS}; #undef X -void app_main(void) -{ - ESP_ERROR_CHECK(gpio_install_isr_service(0)); - test_menu_run(groups, sizeof(groups) / sizeof(groups[0])); +void app_main(void) { + ESP_ERROR_CHECK(gpio_install_isr_service(0)); + test_menu_run(groups, sizeof(groups) / sizeof(groups[0])); } diff --git a/test/main/include/test_menu.h b/test/main/include/test_menu.h index 5570b37..743215d 100644 --- a/test/main/include/test_menu.h +++ b/test/main/include/test_menu.h @@ -5,10 +5,10 @@ typedef void (*test_group_runner_fn)(void); typedef struct { - const char *name; - test_group_runner_fn runner; + const char* name; + test_group_runner_fn runner; } test_group_t; -void test_menu_run(const test_group_t *groups, size_t count); +void test_menu_run(const test_group_t* groups, size_t count); #endif /* _TEST_MENU_H_ */ diff --git a/test/main/test_menu.c b/test/main/test_menu.c index c58632f..e65e94f 100644 --- a/test/main/test_menu.c +++ b/test/main/test_menu.c @@ -1,175 +1,165 @@ /* ═══════════════════════════════════════════════════════════════ * test_menu.c — Simple serial test menu * ═══════════════════════════════════════════════════════════════ */ +#include "test_menu.h" + #include #include + #include "unity.h" #include "unity_fixture.h" -#include "test_menu.h" - /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: Internal state * ─────────────────────────────────────────────────────────────── */ -static const test_group_t *s_groups = NULL; -static size_t s_group_count = 0; +static const test_group_t* s_groups = NULL; +static size_t s_group_count = 0; -static int s_last_total = 0; -static int s_last_failed = 0; +static int s_last_total = 0; +static int s_last_failed = 0; static int s_last_ignored = 0; - /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: UART input — echoes chars so cursor feels alive * ─────────────────────────────────────────────────────────────── */ -static int read_line(char *buf, size_t max) -{ - size_t i = 0; +static int read_line(char* buf, size_t max) { + size_t i = 0; - /* Print prompt once before user starts typing */ - printf("> "); - fflush(stdout); + /* Print prompt once before user starts typing */ + printf("> "); + fflush(stdout); - while (i < max - 1) { - int c = getchar(); - if (c == EOF) continue; - - if (c == '\n' || c == '\r') { - break; - } - - /* Handle backspace — erase char from terminal too */ - if (c == 0x08 || c == 0x7F) { - if (i > 0) { - i--; - printf("\b \b"); - fflush(stdout); - } - continue; - } - - buf[i++] = (char)c; - putchar(c); + while (i < max - 1) { + int c = getchar(); + if (c == EOF) continue; + + if (c == '\n' || c == '\r') { + break; + } + + /* Handle backspace — erase char from terminal too */ + if (c == 0x08 || c == 0x7F) { + if (i > 0) { + i--; + printf("\b \b"); fflush(stdout); + } + continue; } - buf[i] = '\0'; - putchar('\n'); - return (int)i; -} + buf[i++] = (char)c; + putchar(c); + fflush(stdout); + } + buf[i] = '\0'; + putchar('\n'); + return (int)i; +} /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: Run helpers * ─────────────────────────────────────────────────────────────── */ -static void capture_results(void) -{ - s_last_total = Unity.NumberOfTests; - s_last_failed = Unity.TestFailures; - s_last_ignored = Unity.TestIgnores; +static void capture_results(void) { + s_last_total = Unity.NumberOfTests; + s_last_failed = Unity.TestFailures; + s_last_ignored = Unity.TestIgnores; } -static void run_group(const test_group_t *g) -{ - printf("\n Running: %s\n", g->name); - printf(" --------\n"); - UNITY_BEGIN(); - g->runner(); - UNITY_END(); - capture_results(); +static void run_group(const test_group_t* g) { + printf("\n Running: %s\n", g->name); + printf(" --------\n"); + UNITY_BEGIN(); + g->runner(); + UNITY_END(); + capture_results(); } -static void run_all(void) -{ - printf("\n Running: all groups\n"); - printf(" --------\n"); - UNITY_BEGIN(); - for (size_t i = 0; i < s_group_count; i++) { - printf("\n [ %s ]\n", s_groups[i].name); - s_groups[i].runner(); - } - UNITY_END(); - capture_results(); +static void run_all(void) { + printf("\n Running: all groups\n"); + printf(" --------\n"); + UNITY_BEGIN(); + for (size_t i = 0; i < s_group_count; i++) { + printf("\n [ %s ]\n", s_groups[i].name); + s_groups[i].runner(); + } + UNITY_END(); + capture_results(); } - /* ─────────────────────────────────────────────────────────────── * FRAMEWORK: Menu rendering * ─────────────────────────────────────────────────────────────── */ -static void print_menu(void) -{ - printf("\n"); - printf(" --------------------------\n"); - printf(" Test Menu\n"); - printf(" --------------------------\n"); - printf(" a run all\n"); - printf(" r last results\n"); - printf(" q quit\n"); - printf(" --------------------------\n"); - - for (size_t i = 0; i < s_group_count; i++) { - printf(" %-2zu %s\n", i, s_groups[i].name); - } - - printf(" --------------------------\n"); +static void print_menu(void) { + printf("\n"); + printf(" --------------------------\n"); + printf(" Test Menu\n"); + printf(" --------------------------\n"); + printf(" a run all\n"); + printf(" r last results\n"); + printf(" q quit\n"); + printf(" --------------------------\n"); + + for (size_t i = 0; i < s_group_count; i++) { + printf(" %-2zu %s\n", i, s_groups[i].name); + } + + printf(" --------------------------\n"); } -static void print_last_results(void) -{ - printf("\n"); - if (s_last_total == 0) { - printf(" No tests run yet.\n\n"); - return; - } - - int passed = s_last_total - s_last_failed - s_last_ignored; - - printf(" --------------------------\n"); - printf(" Last results\n"); - printf(" --------------------------\n"); - printf(" Total %d\n", s_last_total); - printf(" Passed %d\n", passed); - printf(" Failed %d\n", s_last_failed); - printf(" Ignored %d\n", s_last_ignored); - printf(" --------------------------\n\n"); +static void print_last_results(void) { + printf("\n"); + if (s_last_total == 0) { + printf(" No tests run yet.\n\n"); + return; + } + + int passed = s_last_total - s_last_failed - s_last_ignored; + + printf(" --------------------------\n"); + printf(" Last results\n"); + printf(" --------------------------\n"); + printf(" Total %d\n", s_last_total); + printf(" Passed %d\n", passed); + printf(" Failed %d\n", s_last_failed); + printf(" Ignored %d\n", s_last_ignored); + printf(" --------------------------\n\n"); } - /* ─────────────────────────────────────────────────────────────── * USER: Call this from app_main — pass your groups array * ─────────────────────────────────────────────────────────────── */ -void test_menu_run(const test_group_t *groups, size_t count) -{ - s_groups = groups; - s_group_count = count; - - char buf[16]; - - print_menu(); - - while (1) { - read_line(buf, sizeof(buf)); - - if (strcmp(buf, "a") == 0) { - run_all(); - print_menu(); - - } else if (strcmp(buf, "r") == 0) { - print_last_results(); - - } else if (strcmp(buf, "q") == 0) { - printf("\n bye.\n\n"); - break; - - } else { - char *end; - long idx = strtol(buf, &end, 10); - if (end != buf && idx >= 0 && idx < (long)s_group_count) { - run_group(&s_groups[idx]); - print_menu(); - } else if (strlen(buf) > 0) { - printf(" unknown command.\n"); - } - } +void test_menu_run(const test_group_t* groups, size_t count) { + s_groups = groups; + s_group_count = count; + + char buf[16]; + + print_menu(); + + while (1) { + read_line(buf, sizeof(buf)); + + if (strcmp(buf, "a") == 0) { + run_all(); + print_menu(); + + } else if (strcmp(buf, "r") == 0) { + print_last_results(); + + } else if (strcmp(buf, "q") == 0) { + printf("\n bye.\n\n"); + break; + + } else { + char* end; + long idx = strtol(buf, &end, 10); + if (end != buf && idx >= 0 && idx < (long)s_group_count) { + run_group(&s_groups[idx]); + print_menu(); + } else if (strlen(buf) > 0) { + printf(" unknown command.\n"); + } } + } } From 0deede6237fe03d22eb8bc1a57f660983f745c99 Mon Sep 17 00:00:00 2001 From: frey808 Date: Wed, 1 Apr 2026 18:01:42 -0400 Subject: [PATCH 09/16] motors dont stop for limit switches --- components/motorhat/motorhat.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 04abcb5..96a59b7 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -12,7 +12,6 @@ static motorhat_direction_t delta_to_direction(int8_t delta) { if (delta > 0) return MOTORHAT_DIRECTION_FORWARD; if (delta < 0) return MOTORHAT_DIRECTION_BACKWARD; - // return MOTORHAT_DIRECTION_RELEASE; } @@ -44,7 +43,7 @@ esp_err_t motorhat_init(motorhat_handle_t* handle, s_handle = handle; handle->events = events; - handle->stop_bits = FAULT_ANY; + handle->stop_bits = ESTOP_ANY; handle->polar_pan_speed = config->polar_pan_speed; xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); From 9e7c6687a037a47171bcccbe663dee43bbc8a0af Mon Sep 17 00:00:00 2001 From: frey808 Date: Sun, 5 Apr 2026 15:45:55 -0400 Subject: [PATCH 10/16] set up event groups --- components/ads1015/ads1015.c | 7 +- components/ads1015/include/ads1015.h | 6 +- .../limit_switch/include/limit_switch.h | 3 +- components/limit_switch/limit_switch.c | 19 +++- components/motorhat/include/motorhat.h | 7 +- components/motorhat/motorhat.c | 87 +++++++++++++++---- components/signal_bus/include/signal_bus.h | 36 +++++--- main/esp_driver.c | 7 +- 8 files changed, 116 insertions(+), 56 deletions(-) diff --git a/components/ads1015/ads1015.c b/components/ads1015/ads1015.c index a948bd3..ab9955f 100644 --- a/components/ads1015/ads1015.c +++ b/components/ads1015/ads1015.c @@ -28,7 +28,6 @@ void adc_task(void* arg) { uint16_t config_reg = handle->config_reg | (1 << ADS1015_OS_BIT); // Modify so that writing will start conversions - EventGroupHandle_t events = handle->events; bool mux_state = true; // Differential input to read (true = A2-A3, false = A0-A1) @@ -45,7 +44,7 @@ void adc_task(void* arg) { if (value >= CONFIG_ADS1015_HIGH_THRESH || value <= CONFIG_ADS1015_LOW_THRESH) { ESP_LOGI(TAG, "Overcurrent detection fired"); - xEventGroupSetBits(events, ESTOP_OVERCURRENT); + xEventGroupSetBits(g_motor_events, CURRENT_0); } } @@ -59,8 +58,7 @@ void adc_task(void* arg) { } } -esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config, - EventGroupHandle_t events) { +esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } @@ -101,7 +99,6 @@ esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config, } handle->config_reg = config_reg; - handle->events = events; // Configure alert GPIO and interrupt service gpio_config_t io_conf = { diff --git a/components/ads1015/include/ads1015.h b/components/ads1015/include/ads1015.h index f0fc425..5bb71bd 100644 --- a/components/ads1015/include/ads1015.h +++ b/components/ads1015/include/ads1015.h @@ -133,8 +133,6 @@ typedef struct { typedef struct { i2c_master_dev_handle_t dev_handle; /**< I2C device handle */ uint16_t config_reg; /**< ADS1015 config register values */ - EventGroupHandle_t - events; /**< Event group handle for signaling overcurrent events */ } ads1015_handle_t; /** @@ -145,7 +143,6 @@ typedef struct { * * @param[out] handle Pointer to ADS1015 handle structure * @param[in] config Pointer to configuration structure - * @param[in] events Event group handle for signaling overcurrent events * * @return * - ESP_OK: Success @@ -153,8 +150,7 @@ typedef struct { * frequency) * - ESP_ERR_*: Other ESP-IDF error codes from I2C operations */ -esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config, - EventGroupHandle_t events); +esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config); /** * @brief Check the ADC value against the configured threshholds and trigger diff --git a/components/limit_switch/include/limit_switch.h b/components/limit_switch/include/limit_switch.h index b6fa8fb..0d6cf0a 100644 --- a/components/limit_switch/include/limit_switch.h +++ b/components/limit_switch/include/limit_switch.h @@ -26,7 +26,6 @@ typedef struct { * - ESP_OK: Success * - ESP_ERR_INVALID_ARG: Invalid argument */ -esp_err_t limit_switch_init(const limit_switch_config_t* config, - EventGroupHandle_t events); +esp_err_t limit_switch_init(const limit_switch_config_t* config); #endif // _LIMIT_SWITCH_H \ No newline at end of file diff --git a/components/limit_switch/limit_switch.c b/components/limit_switch/limit_switch.c index 0d9070c..b1b74de 100644 --- a/components/limit_switch/limit_switch.c +++ b/components/limit_switch/limit_switch.c @@ -9,13 +9,23 @@ static void IRAM_ATTR limit_switch_isr(void* arg) { BaseType_t higher_priority_task_woken = pdFALSE; - xEventGroupSetBitsFromISR(g_motor_events, LIMIT_MOTOR_0, + xEventGroupSetBitsFromISR(g_motor_events, LIMIT_0, &higher_priority_task_woken); portYIELD_FROM_ISR(higher_priority_task_woken); } -esp_err_t limit_switch_init(const limit_switch_config_t* config, - EventGroupHandle_t events) { +void test_task(void* arg) { + while (1) { + xEventGroupWaitBits(g_motor_events, EVENT_ANY, + pdTRUE, // don't clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); + + ESP_LOGI(TAG, "Motor event detected, bits=0x%06lX", (unsigned long)xEventGroupGetBits(g_motor_events)); + } +} + +esp_err_t limit_switch_init(const limit_switch_config_t* config) { if (!config) { return ESP_ERR_INVALID_ARG; } @@ -33,5 +43,8 @@ esp_err_t limit_switch_init(const limit_switch_config_t* config, ESP_ERROR_CHECK( gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, NULL)); + + xTaskCreate(test_task, "test_task", 4096, NULL, 7, NULL); + return ESP_OK; } \ No newline at end of file diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index d69b52c..cda6f0e 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -73,9 +73,6 @@ typedef struct { */ typedef struct { pca9685_handle_t pca9685; /**< Handle for the underlying PCA9685 controller */ - EventGroupHandle_t - events; /**< Event group handle for motor fault signaling */ - EventBits_t stop_bits; /**< Which bits trigger motor stop */ uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ } motorhat_handle_t; @@ -105,7 +102,6 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { * @param[out] handle Pointer to Motor HAT handle structure * @param[in] config Pointer to configuration structure containing PCA9685 * settings - * @param[in] events Event group handle for signaling motor faults * * @return * - ESP_OK: Success @@ -116,8 +112,7 @@ static const motorhat_motor_channels_t motor_channels[MOTORHAT_NUM_MOTORS] = { * pca9685_handle_t structure before calling this function */ esp_err_t motorhat_init(motorhat_handle_t* handle, - const motorhat_config_t* config, - EventGroupHandle_t events); + const motorhat_config_t* config); /** * @brief Polar pan command diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 96a59b7..bd1fc29 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -20,7 +20,7 @@ void motor_stop_task(void* args) { while (1) { // Block indefinitely until any stop bit is set - xEventGroupWaitBits(handle->events, handle->stop_bits, + xEventGroupWaitBits(g_motor_events, CURRENT_ANY, pdFALSE, // don't clear bits on exit pdFALSE, // any bit (OR) portMAX_DELAY); @@ -28,22 +28,19 @@ void motor_stop_task(void* args) { motorhat_emergency_stop(handle); // Wait until all stop bits are cleared before watching again - xEventGroupWaitBits(handle->events, handle->stop_bits, pdFALSE, + xEventGroupWaitBits(g_motor_events, CURRENT_ANY, pdFALSE, pdTRUE, // all bits (AND) portMAX_DELAY); } } esp_err_t motorhat_init(motorhat_handle_t* handle, - const motorhat_config_t* config, - EventGroupHandle_t events) { + const motorhat_config_t* config) { if (handle == NULL || config == NULL) { return ESP_ERR_INVALID_ARG; } s_handle = handle; - handle->events = events; - handle->stop_bits = ESTOP_ANY; handle->polar_pan_speed = config->polar_pan_speed; xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); @@ -54,18 +51,59 @@ esp_err_t motorhat_init(motorhat_handle_t* handle, // Socket commands esp_err_t motorhat_home(uint16_t delay_ms) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + ESP_LOGI(TAG, "Received home command: delay_ms=%d", delay_ms); - // TODO: Implement homing logic using motorhat_set_motor_direction, - // motorhat_set_motor_speed, and limit switch state from signal bus + + // Set homing flag + xEventGroupSetBits(g_motor_events, HOMING_FLAG); + + // Add delay + vTaskDelay(pdMS_TO_TICKS(delay_ms)); + + // Stop any movement before homing + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + } + + // Home each motor individually until we candetermine which motor triggers limit switch and current events + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + + // motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + // motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 2); + + // xEventGroupWaitBits(g_motor_events, HOME_EVENT, + // pdTRUE, // clear bits on exit + // pdFALSE, // any bit (OR) + // portMAX_DELAY); + + // TODO: React to homing event + } + + // Clear homing flag + xEventGroupClearBits(g_motor_events, HOMING_FLAG); + return ESP_OK; } esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, uint16_t delay_ms, uint16_t time_ms) { - ESP_LOGI(TAG, - "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, " - "delay_ms=%d, time_ms=%d", - delta_azimuth, delta_altitude, delay_ms, time_ms); + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + + ESP_LOGI(TAG, "Received polar pan command: delta_azimuth=%d, delta_altitude=%d, delay_ms=%d, time_ms=%d", + delta_azimuth, delta_altitude, delay_ms, time_ms); + // TODO: Implement polar pan logic using motorhat_set_motor_direction and // motorhat_set_motor_speed return ESP_OK; @@ -73,11 +111,14 @@ esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, int8_t delta_altitude) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; - ESP_LOGI( - TAG, - "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", - delta_azimuth, delta_altitude); + + ESP_LOGI(TAG, "Received polar pan start command: delta_azimuth=%d, delta_altitude=%d", delta_azimuth, delta_altitude); // Set both axes speed to 0 to prevent direction change while moving esp_err_t err = @@ -119,7 +160,15 @@ esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, } esp_err_t motorhat_polar_pan_stop(void) { + if (xEventGroupGetBits(g_motor_events) & HOMING_FLAG) { + ESP_LOGW(TAG, "Homing in progress, cannot send commands"); + return ESP_ERR_INVALID_STATE; + } + + if (s_handle == NULL) return ESP_ERR_INVALID_STATE; + ESP_LOGI(TAG, "Received polar pan stop command"); + for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { motorhat_set_motor_speed(s_handle, m, 0); motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BRAKE); @@ -135,7 +184,7 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t* handle, return ESP_ERR_INVALID_ARG; } - if (xEventGroupGetBits(handle->events) & handle->stop_bits) { + if (xEventGroupGetBits(g_motor_events) & CURRENT_ANY) { ESP_LOGW(TAG, "Motor %d in fault state, cannot set speed", motor); return ESP_ERR_INVALID_STATE; } @@ -158,7 +207,7 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, return ESP_ERR_INVALID_ARG; } - if (xEventGroupGetBits(handle->events) & handle->stop_bits) { + if (xEventGroupGetBits(g_motor_events) & CURRENT_ANY) { ESP_LOGW(TAG, "Motor %d in fault state, cannot set direction", motor); return ESP_ERR_INVALID_STATE; } @@ -207,7 +256,7 @@ esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, return err; } -// Emergency stop all motors that bypasses normal logic flow +// Emergency stop on all motors that bypasses normal logic flow esp_err_t motorhat_emergency_stop(motorhat_handle_t* handle) { if (handle == NULL) { return ESP_ERR_INVALID_ARG; diff --git a/components/signal_bus/include/signal_bus.h b/components/signal_bus/include/signal_bus.h index 7a1122b..9520ffa 100644 --- a/components/signal_bus/include/signal_bus.h +++ b/components/signal_bus/include/signal_bus.h @@ -5,19 +5,29 @@ #include "freertos/FreeRTOS.h" #include "freertos/event_groups.h" -// Motor limit switch and fault event bits -#define LIMIT_MOTOR_0 (1 << 0) -#define LIMIT_MOTOR_1 (1 << 1) -#define LIMIT_MOTOR_2 (1 << 2) -#define LIMIT_MOTOR_3 (1 << 3) -#define LIMIT_MOTOR_4 (1 << 4) -#define LIMIT_MOTOR_5 (1 << 5) -#define LIMIT_MOTOR_6 (1 << 6) -#define ESTOP_OVERCURRENT (1 << 7) - -#define ESTOP_ANY (ESTOP_OVERCURRENT) -#define LIMIT_ANY (0x7F) // bits 0-6 -#define FAULT_ANY (LIMIT_ANY | ESTOP_ANY) +// Motor event bits +#define CURRENT_0 (1 << 0) +#define CURRENT_1 (1 << 1) +#define CURRENT_2 (1 << 2) +#define CURRENT_3 (1 << 3) +#define CURRENT_4 (1 << 4) +#define CURRENT_5 (1 << 5) +#define CURRENT_6 (1 << 6) + +#define LIMIT_0 (1 << 7) +#define LIMIT_1 (1 << 8) +#define LIMIT_2 (1 << 9) +#define LIMIT_3 (1 << 10) +#define LIMIT_4 (1 << 11) +#define LIMIT_5 (1 << 12) +#define LIMIT_6 (1 << 13) + +#define HOMING_FLAG (1 << 14) + +#define CURRENT_ANY (0x007F) // bits 0-6 +#define LIMIT_ANY (0x3F80) // bits 7-13 +#define HOME_EVENT (CURRENT_ANY | LIMIT_ANY) +#define EVENT_ANY (HOME_EVENT | HOMING_FLAG) extern EventGroupHandle_t g_motor_events; diff --git a/main/esp_driver.c b/main/esp_driver.c index e2252dd..e278ef0 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -56,7 +56,7 @@ void app_main(void) { limit_switch_config_t limit_switch_config = { .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, }; - ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config, g_motor_events)); + ESP_ERROR_CHECK(limit_switch_init(&limit_switch_config)); // Initialize current sensing ads1015_handle_t ads; @@ -67,7 +67,7 @@ void app_main(void) { .bus_handle = bus.handle, .adc_data_rate = CONFIG_DRIVER_ADS1015_DATA_RATE, }; - ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config, g_motor_events)); + ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); // Initialize encoders encoder_handle_t encoder; @@ -94,7 +94,7 @@ void app_main(void) { .polar_pan_speed = PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED), }; - ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config, g_motor_events)); + ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config)); // Initialize Wi-Fi and socket connection driver_wifi_config_t wifi_config = { @@ -122,6 +122,7 @@ void app_main(void) { if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); } + // ESP_LOGI(TAG, "Encoder pulse count: %i", pulse_count); vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop From ee6b1508a2f5142ba7a07dd598ecfa0638fa9300 Mon Sep 17 00:00:00 2001 From: frey808 Date: Sun, 5 Apr 2026 16:37:51 -0400 Subject: [PATCH 11/16] fixed unittests --- components/ads1015/test/test_ads1015.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/components/ads1015/test/test_ads1015.c b/components/ads1015/test/test_ads1015.c index 1b4860a..8517eb2 100644 --- a/components/ads1015/test/test_ads1015.c +++ b/components/ads1015/test/test_ads1015.c @@ -34,8 +34,7 @@ TEST(ADS1015, ADS1015_Initialization) { .bus_handle = bus_handle.handle, }; - EventGroupHandle_t event_group = xEventGroupCreate(); - esp_err_t err = ads1015_init(&handle, &config, event_group); + esp_err_t err = ads1015_init(&handle, &config); TEST_ASSERT_EQUAL(ESP_OK, err); TEST_ASSERT_NOT_EQUAL(NULL, handle.dev_handle); } @@ -47,7 +46,6 @@ TEST(ADS1015, ADS1015_Wrong_Address) { .bus_handle = bus_handle.handle, }; - EventGroupHandle_t event_group = xEventGroupCreate(); - esp_err_t err = ads1015_init(&handle, &config, event_group); + esp_err_t err = ads1015_init(&handle, &config); TEST_ASSERT_NOT_EQUAL(ESP_OK, err); } From d2cd0a7037380a058d54ef4abb49b6befd9e07cb Mon Sep 17 00:00:00 2001 From: frey808 Date: Sun, 5 Apr 2026 20:36:27 -0400 Subject: [PATCH 12/16] homing sequence kind of done --- components/limit_switch/limit_switch.c | 32 +++---- components/motorhat/CMakeLists.txt | 2 +- components/motorhat/motorhat.c | 127 +++++++++++++++++++++---- 3 files changed, 120 insertions(+), 41 deletions(-) diff --git a/components/limit_switch/limit_switch.c b/components/limit_switch/limit_switch.c index b1b74de..76bdad8 100644 --- a/components/limit_switch/limit_switch.c +++ b/components/limit_switch/limit_switch.c @@ -7,22 +7,20 @@ #define TAG "limit_switch" static void IRAM_ATTR limit_switch_isr(void* arg) { + limit_switch_config_t* config = (limit_switch_config_t*)arg; BaseType_t higher_priority_task_woken = pdFALSE; - xEventGroupSetBitsFromISR(g_motor_events, LIMIT_0, - &higher_priority_task_woken); - portYIELD_FROM_ISR(higher_priority_task_woken); -} - -void test_task(void* arg) { - while (1) { - xEventGroupWaitBits(g_motor_events, EVENT_ANY, - pdTRUE, // don't clear bits on exit - pdFALSE, // any bit (OR) - portMAX_DELAY); - - ESP_LOGI(TAG, "Motor event detected, bits=0x%06lX", (unsigned long)xEventGroupGetBits(g_motor_events)); + int level = gpio_get_level(config->limit_gpio); + if (level) { + // Limit switch released, clear corresponding bit + xEventGroupClearBitsFromISR(g_motor_events, LIMIT_0); + } else { + // Limit switch triggered, set corresponding bit + xEventGroupSetBitsFromISR(g_motor_events, LIMIT_0, + &higher_priority_task_woken); } + + portYIELD_FROM_ISR(higher_priority_task_woken); } esp_err_t limit_switch_init(const limit_switch_config_t* config) { @@ -32,7 +30,7 @@ esp_err_t limit_switch_init(const limit_switch_config_t* config) { // Configure GPIO interrupt service gpio_config_t io_conf = { - .intr_type = GPIO_INTR_NEGEDGE, + .intr_type = GPIO_INTR_ANYEDGE, .mode = GPIO_MODE_INPUT, .pin_bit_mask = 1ULL << config->limit_gpio, .pull_up_en = GPIO_PULLUP_ENABLE, @@ -40,11 +38,7 @@ esp_err_t limit_switch_init(const limit_switch_config_t* config) { }; ESP_ERROR_CHECK(gpio_config(&io_conf)); - ESP_ERROR_CHECK( - gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, NULL)); - - - xTaskCreate(test_task, "test_task", 4096, NULL, 7, NULL); + ESP_ERROR_CHECK(gpio_isr_handler_add(config->limit_gpio, limit_switch_isr, (void*)config)); return ESP_OK; } \ No newline at end of file diff --git a/components/motorhat/CMakeLists.txt b/components/motorhat/CMakeLists.txt index 149f993..7c8ca4d 100644 --- a/components/motorhat/CMakeLists.txt +++ b/components/motorhat/CMakeLists.txt @@ -1,2 +1,2 @@ idf_component_register(SRCS "motorhat.c" "pca9685.c" - INCLUDE_DIRS "include" REQUIRES esp_driver_i2c i2c_bus signal_bus) + INCLUDE_DIRS "include" REQUIRES esp_driver_i2c i2c_bus signal_bus esp_driver_gpio) diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index bd1fc29..6d6a4d2 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -3,6 +3,7 @@ #include "esp_err.h" #include "esp_log.h" #include "signal_bus.h" +#include "driver/gpio.h" #define TAG "motorhat" @@ -16,8 +17,6 @@ static motorhat_direction_t delta_to_direction(int8_t delta) { } void motor_stop_task(void* args) { - motorhat_handle_t* handle = (motorhat_handle_t*)args; - while (1) { // Block indefinitely until any stop bit is set xEventGroupWaitBits(g_motor_events, CURRENT_ANY, @@ -25,12 +24,13 @@ void motor_stop_task(void* args) { pdFALSE, // any bit (OR) portMAX_DELAY); - motorhat_emergency_stop(handle); - - // Wait until all stop bits are cleared before watching again - xEventGroupWaitBits(g_motor_events, CURRENT_ANY, pdFALSE, - pdTRUE, // all bits (AND) - portMAX_DELAY); + // Check for homing sequence before reacting + if (!(xEventGroupGetBits(g_motor_events) & HOMING_FLAG)){ + motorhat_emergency_stop(s_handle); + } else { + // Homing sequence will handle this, yield and check again shortly + vTaskDelay(pdMS_TO_TICKS(10)); +} } } @@ -72,23 +72,112 @@ esp_err_t motorhat_home(uint16_t delay_ms) { motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); } - // Home each motor individually until we candetermine which motor triggers limit switch and current events - for (int m = MOTORHAT_MOTOR1; m < MOTORHAT_NUM_MOTORS; m++) { + // Home each axis individually until we candetermine which motor triggers limit switch and current events + for (int axis = MOTORHAT_AXIS_AZIMUTH; axis < MOTORHAT_NUM_AXES; axis++) { + int m = axis_motor[axis]; + ESP_LOGI(TAG, "Homing motor %d", m); + + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); + + EventBits_t bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, + pdTRUE, // clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); - // motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); - // motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 2); + // Stop when a homing event is detected + motorhat_set_motor_speed(s_handle, m, 0); + + if (bits & CURRENT_ANY) { + ESP_LOGI(TAG, "Motor %d hit the backward end of its range", m); + + // Small pause before going opposite direction + vTaskDelay(pdMS_TO_TICKS(200)); - // xEventGroupWaitBits(g_motor_events, HOME_EVENT, - // pdTRUE, // clear bits on exit - // pdFALSE, // any bit (OR) - // portMAX_DELAY); + // Switch directions and move towards limit switch + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); - // TODO: React to homing event + bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, + pdTRUE, // clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); + + // Stop when a homing event is detected + motorhat_set_motor_speed(s_handle, m, 0); + + if (bits & LIMIT_ANY) { + ESP_LOGI(TAG, "Motor %d hit a limit switch moving forward", m); + + // Continue forward slowly to find limit switch release + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); + + // Detect limit switch release by polling + while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { + vTaskDelay(pdMS_TO_TICKS(10)); + } + ESP_LOGI(TAG, "Motor %d finished homing", m); + + // Homing finished, idle motor and reset encoder value + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + + } else { + ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); + motorhat_emergency_stop(s_handle); + } + + } else if (bits & LIMIT_ANY) { + ESP_LOGI(TAG, "Motor %d hit a limit switch moving backward", m); + + // Keep moving to the other side of the limit switch + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 1.5); + while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { + vTaskDelay(pdMS_TO_TICKS(10)); + } + motorhat_set_motor_speed(s_handle, m, 0); + ESP_LOGI(TAG, "Motor %d went past the limit switch", m); + + // Small pause before going the opposite direction + vTaskDelay(pdMS_TO_TICKS(200)); + + // Turn around and move slowly to find limit switch release + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); + + // Wait until limit switch engages again + bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, + pdTRUE, // clear bits on exit + pdFALSE, // any bit (OR) + portMAX_DELAY); + + // Check if it's a limit switch we weren't expecting just in case + if (!(bits & LIMIT_ANY)){ + ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); + motorhat_emergency_stop(s_handle); + } + + // Detect limit switch release by polling + while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { + vTaskDelay(pdMS_TO_TICKS(10)); + } + ESP_LOGI(TAG, "Motor %d finished homing", m); + + // Homing finished, idle motor and reset encoder value + motorhat_set_motor_speed(s_handle, m, 0); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + + } else { + ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); + motorhat_emergency_stop(s_handle); + } } // Clear homing flag xEventGroupClearBits(g_motor_events, HOMING_FLAG); + ESP_LOGI(TAG, "Motor event bits=0x%06lX", (unsigned long)xEventGroupGetBits(g_motor_events)); + return ESP_OK; } @@ -132,9 +221,6 @@ esp_err_t motorhat_polar_pan_start(int8_t delta_azimuth, return err; } - ESP_LOGI(TAG, "Direction Azimuth %d", delta_to_direction(delta_azimuth)); - ESP_LOGI(TAG, "Direction Altitude %d", delta_to_direction(delta_altitude)); - // Set directions based on deltas err = motorhat_set_motor_direction(s_handle, axis_motor[MOTORHAT_AXIS_AZIMUTH], @@ -196,7 +282,6 @@ esp_err_t motorhat_set_motor_speed(motorhat_handle_t* handle, const motorhat_motor_channels_t* channels = &motor_channels[motor]; return pca9685_set_duty_cycle(&handle->pca9685, channels->pwm_channel, speed); - return ESP_OK; } esp_err_t motorhat_set_motor_direction(motorhat_handle_t* handle, From eaf23d90f19cfc4ddb6b57d2dd8c6ccc38eacee6 Mon Sep 17 00:00:00 2001 From: frey808 Date: Tue, 7 Apr 2026 19:09:26 -0400 Subject: [PATCH 13/16] set up encoder config and unreversed homing directions --- components/motorhat/motorhat.c | 10 ++++++---- main/esp_driver.c | 36 ++++++++++++++++++++++++---------- 2 files changed, 32 insertions(+), 14 deletions(-) diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 6d6a4d2..d22a11a 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -77,7 +77,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { int m = axis_motor[axis]; ESP_LOGI(TAG, "Homing motor %d", m); - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); EventBits_t bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, @@ -95,7 +95,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { vTaskDelay(pdMS_TO_TICKS(200)); // Switch directions and move towards limit switch - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, @@ -121,6 +121,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { // Homing finished, idle motor and reset encoder value motorhat_set_motor_speed(s_handle, m, 0); motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + //TODO: reset encoder } else { ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); @@ -142,7 +143,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { vTaskDelay(pdMS_TO_TICKS(200)); // Turn around and move slowly to find limit switch release - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); // Wait until limit switch engages again @@ -151,7 +152,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { pdFALSE, // any bit (OR) portMAX_DELAY); - // Check if it's a limit switch we weren't expecting just in case + // Check if it was an event we weren't expecting just in case if (!(bits & LIMIT_ANY)){ ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); motorhat_emergency_stop(s_handle); @@ -166,6 +167,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { // Homing finished, idle motor and reset encoder value motorhat_set_motor_speed(s_handle, m, 0); motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); + // TODO: Reset encoder value } else { ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); diff --git a/main/esp_driver.c b/main/esp_driver.c index e278ef0..0ae6d95 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -70,16 +70,27 @@ void app_main(void) { ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); // Initialize encoders - encoder_handle_t encoder; - encoder_config_t encoder_config = { - .P0_pin = CONFIG_ENCODER_0_P0_PIN, - .P1_pin = CONFIG_ENCODER_0_P1_PIN, + encoder_handle_t encoder_axis1; + encoder_config_t encoder_axis1_config = { + .P0_pin = 36, + .P1_pin = 39, .resolution = CONFIG_ENCODER_0_RESOLUTION, .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, }; - ESP_ERROR_CHECK(encoder_init(&encoder, &encoder_config)); - ESP_ERROR_CHECK(encoder_start(&encoder)); + ESP_ERROR_CHECK(encoder_init(&encoder_axis1, &encoder_axis1_config)); + ESP_ERROR_CHECK(encoder_start(&encoder_axis1)); + + encoder_handle_t encoder_axis2; + encoder_config_t encoder_axis2_config = { + .P0_pin = 34, + .P1_pin = 35, + .resolution = CONFIG_ENCODER_0_RESOLUTION, + .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, + .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, + }; + ESP_ERROR_CHECK(encoder_init(&encoder_axis2, &encoder_axis2_config)); + ESP_ERROR_CHECK(encoder_start(&encoder_axis2)); // Initialize motor controller motorhat_handle_t motorhat; @@ -115,16 +126,21 @@ void app_main(void) { ESP_ERROR_CHECK( driver_socket_init(&socket_handle, &socket_config, &motor_interface)); - int pulse_count; + int axis1_count; + int axis2_count; while (1) { - esp_err_t err = encoder_get_raw_count(&encoder, &pulse_count); + esp_err_t err = encoder_get_raw_count(&encoder_axis1, &axis1_count); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); + } + err = encoder_get_raw_count(&encoder_axis2, &axis2_count); if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); } - // ESP_LOGI(TAG, "Encoder pulse count: %i", pulse_count); + // ESP_LOGI(TAG, "Axis 1 count: %i Axis 2 count: %i", axis1_count, axis2_count); - vTaskDelay(pdMS_TO_TICKS(10)); // Avoid busy loop + vTaskDelay(pdMS_TO_TICKS(500)); // Avoid busy loop // This loop can also be used to add periodic tasks like reading sensors, or // checking limit switches. However, it must not terminate or structs From 5764af0ac4291a9758ddd24167f40b33910107cc Mon Sep 17 00:00:00 2001 From: frey808 Date: Thu, 9 Apr 2026 17:07:39 -0400 Subject: [PATCH 14/16] finished homing --- components/motorhat/include/motorhat.h | 9 +++ components/motorhat/motorhat.c | 76 ++++++++++++-------------- main/esp_driver.c | 36 +++++++++--- 3 files changed, 70 insertions(+), 51 deletions(-) diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index cda6f0e..43e7cfc 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -37,6 +37,11 @@ static const motorhat_motor_t axis_motor[MOTORHAT_NUM_AXES] = { [MOTORHAT_AXIS_ALTITUDE] = MOTORHAT_MOTOR4, }; +/** + * @brief Function callback for clearing encoder count + */ +typedef void (*motorhat_encoder_cb_t)(void* ctx); + /** * @brief Motor direction and state control */ @@ -66,6 +71,8 @@ typedef struct { typedef struct { pca9685_config_t pca9685_config; uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ + motorhat_encoder_cb_t encoder_cb; /**< Clear encoder count function callback */ + void* encoder_ctx; /** < Clear encoder count callback context (the encoder handle) */ } motorhat_config_t; /** @@ -74,6 +81,8 @@ typedef struct { typedef struct { pca9685_handle_t pca9685; /**< Handle for the underlying PCA9685 controller */ uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ + motorhat_encoder_cb_t encoder_cb; /**< Clear encoder count function callback */ + void* encoder_ctx; /** < Clear encoder count callback context (the encoder handle) */ } motorhat_handle_t; /** diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index d22a11a..818b8ce 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -42,6 +42,8 @@ esp_err_t motorhat_init(motorhat_handle_t* handle, s_handle = handle; handle->polar_pan_speed = config->polar_pan_speed; + handle->encoder_cb = config->encoder_cb; + handle->encoder_ctx = config->encoder_ctx; xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); @@ -77,39 +79,45 @@ esp_err_t motorhat_home(uint16_t delay_ms) { int m = axis_motor[axis]; ESP_LOGI(TAG, "Homing motor %d", m); - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); - EventBits_t bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, - pdTRUE, // clear bits on exit - pdFALSE, // any bit (OR) - portMAX_DELAY); + EventBits_t bits = xEventGroupWaitBits( + g_motor_events, + HOME_EVENT, + pdTRUE, + pdFALSE, + portMAX_DELAY + ); // Stop when a homing event is detected motorhat_set_motor_speed(s_handle, m, 0); if (bits & CURRENT_ANY) { - ESP_LOGI(TAG, "Motor %d hit the backward end of its range", m); + ESP_LOGI(TAG, "Motor %d hit the forward end of its range", m); // Small pause before going opposite direction vTaskDelay(pdMS_TO_TICKS(200)); // Switch directions and move towards limit switch - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed); - bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, - pdTRUE, // clear bits on exit - pdFALSE, // any bit (OR) - portMAX_DELAY); + bits = xEventGroupWaitBits( + g_motor_events, + HOME_EVENT, + pdTRUE, + pdFALSE, + portMAX_DELAY + ); // Stop when a homing event is detected motorhat_set_motor_speed(s_handle, m, 0); if (bits & LIMIT_ANY) { - ESP_LOGI(TAG, "Motor %d hit a limit switch moving forward", m); + ESP_LOGI(TAG, "Motor %d hit a limit switch moving backward", m); - // Continue forward slowly to find limit switch release + // Continue forward slowly until limit switch releases motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); // Detect limit switch release by polling @@ -118,10 +126,9 @@ esp_err_t motorhat_home(uint16_t delay_ms) { } ESP_LOGI(TAG, "Motor %d finished homing", m); - // Homing finished, idle motor and reset encoder value + // Homing finished, stop the motor and continue sequence motorhat_set_motor_speed(s_handle, m, 0); motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); - //TODO: reset encoder } else { ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); @@ -129,57 +136,42 @@ esp_err_t motorhat_home(uint16_t delay_ms) { } } else if (bits & LIMIT_ANY) { - ESP_LOGI(TAG, "Motor %d hit a limit switch moving backward", m); - - // Keep moving to the other side of the limit switch - motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 1.5); - while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { - vTaskDelay(pdMS_TO_TICKS(10)); - } - motorhat_set_motor_speed(s_handle, m, 0); - ESP_LOGI(TAG, "Motor %d went past the limit switch", m); + ESP_LOGI(TAG, "Motor %d hit a limit switch moving forward", m); // Small pause before going the opposite direction vTaskDelay(pdMS_TO_TICKS(200)); - // Turn around and move slowly to find limit switch release - motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_FORWARD); + // Turn around and move slowly until limit switch releases + motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_BACKWARD); motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); - // Wait until limit switch engages again - bits = xEventGroupWaitBits(g_motor_events, HOME_EVENT, - pdTRUE, // clear bits on exit - pdFALSE, // any bit (OR) - portMAX_DELAY); - - // Check if it was an event we weren't expecting just in case - if (!(bits & LIMIT_ANY)){ - ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); - motorhat_emergency_stop(s_handle); - } - // Detect limit switch release by polling while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { vTaskDelay(pdMS_TO_TICKS(10)); } ESP_LOGI(TAG, "Motor %d finished homing", m); - // Homing finished, idle motor and reset encoder value + // Homing finished, stop the motor and continue sequence motorhat_set_motor_speed(s_handle, m, 0); motorhat_set_motor_direction(s_handle, m, MOTORHAT_DIRECTION_RELEASE); - // TODO: Reset encoder value } else { ESP_LOGW(TAG, "Motor %d unexpected homing event detected", m); motorhat_emergency_stop(s_handle); } } + + // Reset encoder values + if (s_handle->encoder_cb) { + s_handle->encoder_cb(s_handle->encoder_ctx); + ESP_LOGI(TAG, "Homing successful, encoders reset"); + } else { + ESP_LOGW(TAG, "Encoder reset callback not found, homing unsuccessful"); + } // Clear homing flag xEventGroupClearBits(g_motor_events, HOMING_FLAG); - ESP_LOGI(TAG, "Motor event bits=0x%06lX", (unsigned long)xEventGroupGetBits(g_motor_events)); - return ESP_OK; } diff --git a/main/esp_driver.c b/main/esp_driver.c index 0ae6d95..6bbb344 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -27,6 +27,18 @@ static const driver_socket_api_motor_interface_t motor_interface = { .home = motorhat_home, }; +typedef struct { + encoder_handle_t* encoders; + int count; +} encoder_array_t; + +void clear_all_encoders(void* ctx) { + encoder_array_t* arr = (encoder_array_t*)ctx; + for (int i = 0; i < arr->count; i++) { + encoder_clear_count(&arr->encoders[i]); + } +} + void app_main(void) { // Initialize NVS esp_err_t ret = nvs_flash_init(); @@ -70,7 +82,7 @@ void app_main(void) { ESP_ERROR_CHECK(ads1015_init(&ads, &ads_config)); // Initialize encoders - encoder_handle_t encoder_axis1; + encoder_handle_t encoders[2]; encoder_config_t encoder_axis1_config = { .P0_pin = 36, .P1_pin = 39, @@ -78,10 +90,9 @@ void app_main(void) { .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, }; - ESP_ERROR_CHECK(encoder_init(&encoder_axis1, &encoder_axis1_config)); - ESP_ERROR_CHECK(encoder_start(&encoder_axis1)); + ESP_ERROR_CHECK(encoder_init(&encoders[0], &encoder_axis1_config)); + ESP_ERROR_CHECK(encoder_start(&encoders[0])); - encoder_handle_t encoder_axis2; encoder_config_t encoder_axis2_config = { .P0_pin = 34, .P1_pin = 35, @@ -89,8 +100,13 @@ void app_main(void) { .glitch_filter_ns = CONFIG_ENCODER_GLITCH_FILTER, .invert_angle = CONFIG_ENCODER_0_ANGLE_INVERT, }; - ESP_ERROR_CHECK(encoder_init(&encoder_axis2, &encoder_axis2_config)); - ESP_ERROR_CHECK(encoder_start(&encoder_axis2)); + ESP_ERROR_CHECK(encoder_init(&encoders[1], &encoder_axis2_config)); + ESP_ERROR_CHECK(encoder_start(&encoders[1])); + + encoder_array_t encoder_array = { + .encoders = encoders, + .count = 2, +}; // Initialize motor controller motorhat_handle_t motorhat; @@ -104,6 +120,8 @@ void app_main(void) { }, .polar_pan_speed = PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED), + .encoder_cb = (motorhat_encoder_cb_t)clear_all_encoders, + .encoder_ctx = &encoder_array, }; ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config)); @@ -130,15 +148,15 @@ void app_main(void) { int axis2_count; while (1) { - esp_err_t err = encoder_get_raw_count(&encoder_axis1, &axis1_count); + esp_err_t err = encoder_get_raw_count(&encoders[0], &axis1_count); if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); } - err = encoder_get_raw_count(&encoder_axis2, &axis2_count); + err = encoder_get_raw_count(&encoders[1], &axis2_count); if (err != ESP_OK) { ESP_LOGE(TAG, "Failed to get encoder count: %s", esp_err_to_name(err)); } - // ESP_LOGI(TAG, "Axis 1 count: %i Axis 2 count: %i", axis1_count, axis2_count); + ESP_LOGI(TAG, "Axis 1 count: %i Axis 2 count: %i", axis1_count, axis2_count); vTaskDelay(pdMS_TO_TICKS(500)); // Avoid busy loop From b6d29281b192f5e9d2e0705be595c9aefd24129d Mon Sep 17 00:00:00 2001 From: frey808 Date: Thu, 16 Apr 2026 15:50:06 -0400 Subject: [PATCH 15/16] docs and tests --- components/ads1015/include/ads1015.h | 13 +------------ .../driver_socket/include/driver_socket_api.h | 1 + components/motorhat/include/motorhat.h | 17 +++++++++++++++-- components/motorhat/test/Kconfig | 6 ++++++ main/Kconfig.projbuild | 10 +++++----- main/esp_driver.c | 2 ++ 6 files changed, 30 insertions(+), 19 deletions(-) diff --git a/components/ads1015/include/ads1015.h b/components/ads1015/include/ads1015.h index 5bb71bd..6ef8a97 100644 --- a/components/ads1015/include/ads1015.h +++ b/components/ads1015/include/ads1015.h @@ -124,7 +124,7 @@ typedef struct { uint32_t i2c_speed_hz; /**< I2C bus speed in Hz */ i2c_master_bus_handle_t bus_handle; /**< I2C master bus handle */ gpio_num_t rdy_gpio; /**< GPIO number for ALERT pin */ - uint8_t adc_data_rate; /**< Data rate setting for ADC conversions */ + uint8_t adc_data_rate; /**< Data rate setting for ADC conversions */ } ads1015_config_t; /** @@ -152,17 +152,6 @@ typedef struct { */ esp_err_t ads1015_init(ads1015_handle_t* handle, const ads1015_config_t* config); -/** - * @brief Check the ADC value against the configured threshholds and trigger - * E-stop if it exceeds them - * - * @param[in] value ADC value to check - * - * @return - * - ESP_OK: Success - */ -esp_err_t ads1015_check_current(int16_t value, bool mux_state); - /** * @brief Read one or more registers from the ADS1015 * diff --git a/components/driver_socket/include/driver_socket_api.h b/components/driver_socket/include/driver_socket_api.h index 2059549..a8b8bc2 100644 --- a/components/driver_socket/include/driver_socket_api.h +++ b/components/driver_socket/include/driver_socket_api.h @@ -53,6 +53,7 @@ typedef struct { int8_t delta_altitude; /** Requested change in altitude */ } __attribute__((packed)) driver_socket_api_polar_pan_start_payload_t; +/** Callback struct that allows the driver socket api to access motorhat motion commands */ typedef struct { esp_err_t (*polar_pan)(int16_t delta_azimuth, int16_t delta_altitude, uint16_t delay_ms, uint16_t time_ms); diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index 43e7cfc..554a391 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -148,7 +148,7 @@ esp_err_t motorhat_polar_pan(int16_t delta_azimuth, int16_t delta_altitude, * @brief Polar pan start command * * Starts a continuous pan movement that will run until a stop command is - * received. + * received. Only uses two axes currently, directly relating azimuth to axis 1 and altitude to axis 2. * * @param[in] delta_azimuth Azimuth delta for the pan movement * @param[in] delta_altitude Altitude delta for the pan movement @@ -175,7 +175,20 @@ esp_err_t motorhat_polar_pan_stop(void); /** * @brief Home command * - * Moves the motors to a predefined home position after an optional delay. + * Moves the motors to a predefined home position after an optional delay. Blocks high level movement commands + * such as polar pan, setting the motor speed and direction directly to operate around these commands. + * + * The homing sequence is modeled after the sequence demonstrated by the SCORBOT ER-V, since both robots + * have the unusual placement of their limit switches at the centers of their corresponding axis's range, + * rather than at the edges. The one difference is that this homing sequence starts with moving each motor FORWARD, + * and ends with it moving BACKWARD to release the limit switch. This is because for the vertical axis, the slowed + * down motion to release the limit switch and reach the final homed position currently stalls the motor when moving upward, + * so I've reversed the order so that it happens while moving downward. This should be fixed by replacing the + * current motor drivers with ones capable of pushing the proper amount of amps to drive the motors, + * but in the meantime reversing the order of FORWARD and BACKWArd motion in homing avoids this issue. + * + * IMPORTANT: until current sensing is fully functional, the robot can't detect reaching the edge of it's range and will + * break if you attempt to home it while the arm is on the wrong side of the limit switch. * * @param[in] delay_ms Delay before starting the homing movement * diff --git a/components/motorhat/test/Kconfig b/components/motorhat/test/Kconfig index d2d658d..f18f5af 100644 --- a/components/motorhat/test/Kconfig +++ b/components/motorhat/test/Kconfig @@ -12,4 +12,10 @@ menu "PCA9685 Configuration" hex "Test I2C address" default 0x60 + config DRIVER_LIMIT_SWITCH_PIN + int "Limit Switch Pin" + default 23 + help + GPIO pin for limit switch alerts. + endmenu diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index e10dd10..7e2c5f4 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -1,13 +1,13 @@ menu "Talos ESP Driver Configuration" config DRIVER_MOTORHAT_SDA_PIN int "MotorHAT SDA Pin" - default 16 + default 21 help GPIO pin number for MotorHAT SDA line. config DRIVER_MOTORHAT_SCL_PIN int "MotorHAT SCL Pin" - default 17 + default 22 help GPIO pin number for MotorHAT SCL line. @@ -31,13 +31,13 @@ menu "Talos ESP Driver Configuration" config DRIVER_ADS1015_ADDRESS hex "ADS1015 I2C Address" - default 0x48 + default 0x4a help I2C address for the ADS1015 device. config DRIVER_ADS1015_RDY_PIN int "ADS1015 RDY Pin" - default 21 + default 18 help GPIO pin number for the ADS1015 RDY line. @@ -52,7 +52,7 @@ menu "Talos ESP Driver Configuration" config DRIVER_LIMIT_SWITCH_PIN int "Limit Switch Pin" - default 22 + default 23 help GPIO pin for limit switch alerts. diff --git a/main/esp_driver.c b/main/esp_driver.c index 6bbb344..f1c7cce 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -144,6 +144,8 @@ void app_main(void) { ESP_ERROR_CHECK( driver_socket_init(&socket_handle, &socket_config, &motor_interface)); + + // Track encoder values int axis1_count; int axis2_count; From 785b335e08be381510da409de645596a555e642f Mon Sep 17 00:00:00 2001 From: frey808 Date: Thu, 16 Apr 2026 16:24:27 -0400 Subject: [PATCH 16/16] removed limit switch pin config item from motorhat --- components/motorhat/include/motorhat.h | 3 +++ components/motorhat/motorhat.c | 5 +++-- components/motorhat/test/Kconfig | 6 ------ main/esp_driver.c | 1 + 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/components/motorhat/include/motorhat.h b/components/motorhat/include/motorhat.h index 554a391..d280be9 100644 --- a/components/motorhat/include/motorhat.h +++ b/components/motorhat/include/motorhat.h @@ -5,6 +5,7 @@ #include "freertos/FreeRTOS.h" #include "freertos/event_groups.h" +#include "driver/gpio.h" #include "pca9685.h" #define DEFAULT_FREQUENCY_HZ 1526.0f @@ -73,6 +74,7 @@ typedef struct { uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ motorhat_encoder_cb_t encoder_cb; /**< Clear encoder count function callback */ void* encoder_ctx; /** < Clear encoder count callback context (the encoder handle) */ + gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ } motorhat_config_t; /** @@ -83,6 +85,7 @@ typedef struct { uint16_t polar_pan_speed; /**< Speed to use for polar pan movements */ motorhat_encoder_cb_t encoder_cb; /**< Clear encoder count function callback */ void* encoder_ctx; /** < Clear encoder count callback context (the encoder handle) */ + gpio_num_t limit_gpio; /**< GPIO number for limit switch pin */ } motorhat_handle_t; /** diff --git a/components/motorhat/motorhat.c b/components/motorhat/motorhat.c index 818b8ce..952e69e 100644 --- a/components/motorhat/motorhat.c +++ b/components/motorhat/motorhat.c @@ -44,6 +44,7 @@ esp_err_t motorhat_init(motorhat_handle_t* handle, handle->polar_pan_speed = config->polar_pan_speed; handle->encoder_cb = config->encoder_cb; handle->encoder_ctx = config->encoder_ctx; + handle->limit_gpio = config->limit_gpio; xTaskCreate(motor_stop_task, "motor_stop_task", 4096, handle, 8, NULL); @@ -121,7 +122,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); // Detect limit switch release by polling - while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { + while (!gpio_get_level(s_handle->limit_gpio)) { vTaskDelay(pdMS_TO_TICKS(10)); } ESP_LOGI(TAG, "Motor %d finished homing", m); @@ -146,7 +147,7 @@ esp_err_t motorhat_home(uint16_t delay_ms) { motorhat_set_motor_speed(s_handle, m, s_handle->polar_pan_speed / 3); // Detect limit switch release by polling - while (!gpio_get_level(CONFIG_DRIVER_LIMIT_SWITCH_PIN)) { + while (!gpio_get_level(s_handle->limit_gpio)) { vTaskDelay(pdMS_TO_TICKS(10)); } ESP_LOGI(TAG, "Motor %d finished homing", m); diff --git a/components/motorhat/test/Kconfig b/components/motorhat/test/Kconfig index f18f5af..d2d658d 100644 --- a/components/motorhat/test/Kconfig +++ b/components/motorhat/test/Kconfig @@ -12,10 +12,4 @@ menu "PCA9685 Configuration" hex "Test I2C address" default 0x60 - config DRIVER_LIMIT_SWITCH_PIN - int "Limit Switch Pin" - default 23 - help - GPIO pin for limit switch alerts. - endmenu diff --git a/main/esp_driver.c b/main/esp_driver.c index f1c7cce..88efafe 100644 --- a/main/esp_driver.c +++ b/main/esp_driver.c @@ -122,6 +122,7 @@ void app_main(void) { PCA9685_PWM_MAX * atof(CONFIG_DRIVER_MOTORHAT_PAN_SPEED), .encoder_cb = (motorhat_encoder_cb_t)clear_all_encoders, .encoder_ctx = &encoder_array, + .limit_gpio = CONFIG_DRIVER_LIMIT_SWITCH_PIN, }; ESP_ERROR_CHECK(motorhat_init(&motorhat, &motorhat_config));