From eebd55110c3febda3352d34a63fe23a67f9489e9 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 18 Apr 2024 00:53:45 +1200 Subject: [PATCH 01/16] Add servo gate --- .vscode/settings.json | 6 +- CMakeLists.txt | 1 + src/app.cpp | 4 +- src/eeprom.h | 1 + src/html/web_portal.html | 24 ++++- src/rest_endpoints.c | 3 + src/servo_gate.c | 157 ++++++++++++++++++++++++++++ src/servo_gate.h | 47 +++++++++ targets/raspberrypi_pico_config.h | 3 + targets/raspberrypi_pico_w_config.h | 3 + 10 files changed, 246 insertions(+), 3 deletions(-) create mode 100644 src/servo_gate.c create mode 100644 src/servo_gate.h diff --git a/.vscode/settings.json b/.vscode/settings.json index ba8a705..eed85c8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -40,6 +40,10 @@ "cleanup_mode.h": "c", "charge_mode.h": "c", "http_rest.h": "c", - "motors.h": "c" + "motors.h": "c", + "eeprom.h": "c", + "stdbool.h": "c", + "configuration.h": "c", + "uart.h": "c" } } diff --git a/CMakeLists.txt b/CMakeLists.txt index ae62884..83b0e25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,6 +99,7 @@ target_link_libraries("${TARGET_NAME}" hardware_pio hardware_spi hardware_i2c + hardware_pwm FreeRTOS-Kernel FreeRTOS-Kernel-Heap4 u8g2 diff --git a/src/app.cpp b/src/app.cpp index 2966bb8..31b2ec1 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -26,7 +26,7 @@ #include "rotary_button.h" #include "menu.h" #include "profile.h" - +#include "servo_gate.h" uint8_t software_reboot() { @@ -63,6 +63,8 @@ int main() // Initialize profile data profile_data_init(); + servo_gate_init(); + #ifdef RASPBERRYPI_PICO_W // Load wireless settings wireless_init(); diff --git a/src/eeprom.h b/src/eeprom.h index 8bd5f09..3e819c3 100644 --- a/src/eeprom.h +++ b/src/eeprom.h @@ -15,6 +15,7 @@ #define EEPROM_NEOPIXEL_LED_CONFIG_BASE_ADDR 7 * 1024 // 7k #define EEPROM_ROTARY_BUTTON_CONFIG_BASE_ADDR 8 * 1024 // 8k #define EEPROM_PROFILE_DATA_BASE_ADDR 9 * 1024 // 9k +#define EEPROM_SERVO_GATE_CONFIG_BASE_ADDR 10 * 1024 // 10k #define EEPROM_METADATA_REV 2 // 16 byte diff --git a/src/html/web_portal.html b/src/html/web_portal.html index e943d43..687dc35 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -71,11 +71,17 @@
-
Scale
+
General Control
+ +
+ + +
+ @@ -991,6 +997,22 @@

Error

}); } + function servoGateSetState(state) { + const uri = `/rest/servo_gate_state?g0=${encodeURIComponent(state)}`; + + // Call the scale action URI + fetch(uri) + .then(response => { + return response.json(); + }) + .then(data => { + // console.log(data); + }) + .catch(error => { + console.error("Error applying servo gate state"); + }); + } + // Switch pages function onNavButtonClicked(button) { const sectionTrickler = document.getElementById("sectionTrickler"); diff --git a/src/rest_endpoints.c b/src/rest_endpoints.c index 96d8419..eda8234 100644 --- a/src/rest_endpoints.c +++ b/src/rest_endpoints.c @@ -12,6 +12,7 @@ #include "neopixel_led.h" #include "profile.h" #include "cleanup_mode.h" +#include "servo_gate.h" // Generated headers by html2header.py under scripts #include "display_mirror.html.h" @@ -93,6 +94,8 @@ bool rest_endpoints_init(bool default_wizard) { rest_register_handler("/rest/neopixel_led_config", http_rest_neopixel_led_config); rest_register_handler("/rest/profile_config", http_rest_profile_config); rest_register_handler("/rest/profile_summary", http_rest_profile_summary); + rest_register_handler("/rest/servo_gate_state", http_rest_servo_gate_state); + rest_register_handler("/display_buffer", http_get_display_buffer); rest_register_handler("/display_mirror", http_display_mirror); diff --git a/src/servo_gate.c b/src/servo_gate.c new file mode 100644 index 0000000..2e9621e --- /dev/null +++ b/src/servo_gate.c @@ -0,0 +1,157 @@ +#include +#include +#include "servo_gate.h" +#include "hardware/pwm.h" +#include "hardware/clocks.h" +#include "configuration.h" +#include "eeprom.h" + +// Attributes +servo_gate_t servo_gate; + +// Const settings +const float _servo_pwm_freq = 50.0; +const uint16_t _pwm_full_scale_level = 65535; + + +const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { + .servo_gate_config_rev = EEPROM_SERVO_GATE_CONFIG_REV, + .servo_gate_enable = true, + .gate_close_duty_cycle = 0.05, + .gate_open_duty_cycle = 0.1, +}; + + +void set_servo_gate_state(gate_state_t state) { + // pwm_set_chan_level(pwm_gpio_to_slice_num(gpio), pwm_gpio_to_channel(gpio), level); + uint32_t servo0_level; + uint32_t servo1_level; + switch (state) + { + case GATE_CLOSE: + servo0_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle; + servo1_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle; + break; + case GATE_OPEN: + servo0_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle; + servo1_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle; + break; + + default: + return; + } + + uint32_t reg_level = (servo1_level) << 16 | servo0_level; + + // Write both levels to the pwm at the same time + hw_write_masked( + &pwm_hw->slice[SERVO_PWM_SLICE_NUM].cc, + reg_level, + 0xffffffff + ); +} + + +bool servo_gate_config_save(void) { + bool is_ok = eeprom_write(EEPROM_SERVO_GATE_CONFIG_BASE_ADDR, (uint8_t *) &servo_gate.eeprom_servo_gate_config, sizeof(eeprom_servo_gate_config_t)); + return is_ok; +} + +bool servo_gate_config_init() { + bool is_ok = true; + + // Read charge mode config from EEPROM + memset(&servo_gate, 0x0, sizeof(servo_gate)); + is_ok = eeprom_read(EEPROM_SERVO_GATE_CONFIG_BASE_ADDR, (uint8_t *)&servo_gate.eeprom_servo_gate_config, sizeof(eeprom_servo_gate_config_t)); + if (!is_ok) { + printf("Unable to read from EEPROM at address %x\n", EEPROM_SERVO_GATE_CONFIG_BASE_ADDR); + return false; + } + + if (servo_gate.eeprom_servo_gate_config.servo_gate_config_rev != EEPROM_SERVO_GATE_CONFIG_REV) { + memcpy(&servo_gate.eeprom_servo_gate_config, &default_eeprom_servo_gate_config, sizeof(eeprom_servo_gate_config_t)); + + // Write back + is_ok = servo_gate_config_save(); + if (!is_ok) { + printf("Unable to write to %x\n", EEPROM_SERVO_GATE_CONFIG_BASE_ADDR); + return false; + } + } + + // Register to eeprom save all + eeprom_register_handler(servo_gate_config_save); + + // Initialize settings + if (servo_gate.eeprom_servo_gate_config.servo_gate_enable) { + servo_gate.gate_state = GATE_CLOSE; + } + else { + servo_gate.gate_state = GATE_DISABLED; + } + + return is_ok; +} + + +bool servo_gate_init() { + bool is_ok = true; + + is_ok = servo_gate_config_init(); + + // Stop early if servo config failed to initialise, or disabled + if (!is_ok || servo_gate.gate_state == GATE_DISABLED) { + return false; + } + + // Initialize pins + gpio_set_function(SERVO0_PWM_PIN, GPIO_FUNC_PWM); + gpio_set_function(SERVO1_PWM_PIN, GPIO_FUNC_PWM); + + pwm_config cfg = pwm_get_default_config(); + + // Set to 50hz frequency + uint32_t sys_freq = clock_get_hz(clk_sys); + float divider = ceil(sys_freq / (4096 * _servo_pwm_freq)) / 16.0f; + uint16_t wrap = sys_freq / divider / _servo_pwm_freq - 1; + + pwm_config_set_clkdiv(&cfg, divider); + pwm_config_set_wrap(&cfg, wrap); + + pwm_init(pwm_gpio_to_slice_num(SERVO0_PWM_PIN), &cfg, true); + pwm_init(pwm_gpio_to_slice_num(SERVO1_PWM_PIN), &cfg, true); + + set_servo_gate_state(servo_gate.gate_state); + + return true; +} + +bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *params[], char *values[]) { + // Mappings + // g0 (int): gate_state_t + + static char servo_gate_json_buffer[256]; + + // Control + for (int idx = 0; idx < num_params; idx += 1) { + if (strcmp(params[idx], "g0") == 0) { + servo_gate.gate_state = (gate_state_t) atoi(values[idx]); + set_servo_gate_state(servo_gate.gate_state); + } + } + + // Response + snprintf(servo_gate_json_buffer, + sizeof(servo_gate_json_buffer), + "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\n\r\n" + "{\"g0\":%d}", + (int) servo_gate.gate_state); + + size_t data_length = strlen(servo_gate_json_buffer); + file->data = servo_gate_json_buffer; + file->len = data_length; + file->index = data_length; + file->flags = FS_FILE_FLAGS_HEADER_INCLUDED; + + return true; +} \ No newline at end of file diff --git a/src/servo_gate.h b/src/servo_gate.h new file mode 100644 index 0000000..94a6cd0 --- /dev/null +++ b/src/servo_gate.h @@ -0,0 +1,47 @@ +#ifndef SERVO_GATE_H_ +#define SERVO_GATE_H_ + +#include +#include "http_rest.h" + +#define EEPROM_SERVO_GATE_CONFIG_REV 1 // 16 byte + +typedef enum { + GATE_DISABLED = 0, + GATE_CLOSE, + GATE_OPEN, +} gate_state_t; + + +typedef struct { + uint16_t servo_gate_config_rev; + bool servo_gate_enable; + float gate_close_duty_cycle; + float gate_open_duty_cycle; +} eeprom_servo_gate_config_t; + + + +typedef struct { + eeprom_servo_gate_config_t eeprom_servo_gate_config; + gate_state_t gate_state; + + // RTOS queue +} servo_gate_t; + + +#ifdef __cplusplus +extern "C" { +#endif + +bool servo_gate_init(void); +bool servo_gate_config_save(void); +bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *params[], char *values[]); + + +#ifdef __cplusplus +} +#endif + + +#endif // SERVO_GATE_H_ \ No newline at end of file diff --git a/targets/raspberrypi_pico_config.h b/targets/raspberrypi_pico_config.h index ae1f21a..811b4e1 100644 --- a/targets/raspberrypi_pico_config.h +++ b/targets/raspberrypi_pico_config.h @@ -46,5 +46,8 @@ #define EEPROM_SCL_PIN 11 #define EEPROM_ADDR 0x0 +#define SERVO0_PWM_PIN 26 +#define SERVO1_PWM_PIN 27 +#define SERVO_PWM_SLICE_NUM 5 #endif // RASPBERRYPI_PICO_CONFIG_H_ \ No newline at end of file diff --git a/targets/raspberrypi_pico_w_config.h b/targets/raspberrypi_pico_w_config.h index a47399a..8d7b39c 100644 --- a/targets/raspberrypi_pico_w_config.h +++ b/targets/raspberrypi_pico_w_config.h @@ -48,5 +48,8 @@ #define EEPROM_SCL_PIN 11 #define EEPROM_ADDR 0x50 +#define SERVO0_PWM_PIN 26 +#define SERVO1_PWM_PIN 27 +#define SERVO_PWM_SLICE_NUM 5 #endif // RASPBERRYPI_PICO_W_CONFIG_H_ \ No newline at end of file From 0280136f6770c26403773c0c8920ab153dab6c4d Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 18 Apr 2024 21:14:45 +1200 Subject: [PATCH 02/16] Add servo config --- .vscode/settings.json | 3 +- src/app.cpp | 1 + src/rest_endpoints.c | 2 +- src/servo_gate.c | 105 ++++++++++++++++++++++++++++++++++++++---- src/servo_gate.h | 8 +++- 5 files changed, 108 insertions(+), 11 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index eed85c8..4c872bb 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -44,6 +44,7 @@ "eeprom.h": "c", "stdbool.h": "c", "configuration.h": "c", - "uart.h": "c" + "uart.h": "c", + "common.h": "c" } } diff --git a/src/app.cpp b/src/app.cpp index 31b2ec1..33bfca8 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -63,6 +63,7 @@ int main() // Initialize profile data profile_data_init(); + // Initialize the servo servo_gate_init(); #ifdef RASPBERRYPI_PICO_W diff --git a/src/rest_endpoints.c b/src/rest_endpoints.c index eda8234..352a297 100644 --- a/src/rest_endpoints.c +++ b/src/rest_endpoints.c @@ -95,7 +95,7 @@ bool rest_endpoints_init(bool default_wizard) { rest_register_handler("/rest/profile_config", http_rest_profile_config); rest_register_handler("/rest/profile_summary", http_rest_profile_summary); rest_register_handler("/rest/servo_gate_state", http_rest_servo_gate_state); - + rest_register_handler("/rest/servo_gate_config", http_rest_servo_gate_config); rest_register_handler("/display_buffer", http_get_display_buffer); rest_register_handler("/display_mirror", http_display_mirror); diff --git a/src/servo_gate.c b/src/servo_gate.c index 2e9621e..04b8f6b 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -5,6 +5,7 @@ #include "hardware/clocks.h" #include "configuration.h" #include "eeprom.h" +#include "common.h" // Attributes servo_gate_t servo_gate; @@ -22,10 +23,10 @@ const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { }; -void set_servo_gate_state(gate_state_t state) { - // pwm_set_chan_level(pwm_gpio_to_slice_num(gpio), pwm_gpio_to_channel(gpio), level); +void _servo_gate_set_state(gate_state_t state) { uint32_t servo0_level; uint32_t servo1_level; + switch (state) { case GATE_CLOSE: @@ -52,6 +53,26 @@ void set_servo_gate_state(gate_state_t state) { } +void servo_gate_set_state(gate_state_t state) { + if (servo_gate.servo_gate_control_queue) { + xQueueSend(servo_gate.servo_gate_control_queue, &state, portMAX_DELAY); + } +} + + +void servo_gate_control_task(void * p) { + while (true) { + gate_state_t new_state; + xQueueReceive(servo_gate.servo_gate_control_queue, &new_state, portMAX_DELAY); + + servo_gate.gate_state = new_state; + + // Apply the new state + _servo_gate_set_state(new_state); + } +} + + bool servo_gate_config_save(void) { bool is_ok = eeprom_write(EEPROM_SERVO_GATE_CONFIG_BASE_ADDR, (uint8_t *) &servo_gate.eeprom_servo_gate_config, sizeof(eeprom_servo_gate_config_t)); return is_ok; @@ -121,22 +142,35 @@ bool servo_gate_init() { pwm_init(pwm_gpio_to_slice_num(SERVO0_PWM_PIN), &cfg, true); pwm_init(pwm_gpio_to_slice_num(SERVO1_PWM_PIN), &cfg, true); - set_servo_gate_state(servo_gate.gate_state); + // Start the RTOS task and queue + servo_gate.servo_gate_control_queue = xQueueCreate(1, sizeof(gate_state_t)); + + xTaskCreate( + servo_gate_control_task, + "servo_gate_controller", + configMINIMAL_STACK_SIZE, + NULL, + 8, + &servo_gate.servo_gate_control_task_handler + ); + + servo_gate_set_state(servo_gate.gate_state); return true; } + bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *params[], char *values[]) { // Mappings // g0 (int): gate_state_t - - static char servo_gate_json_buffer[256]; + + static char servo_gate_json_buffer[64]; // Control for (int idx = 0; idx < num_params; idx += 1) { if (strcmp(params[idx], "g0") == 0) { - servo_gate.gate_state = (gate_state_t) atoi(values[idx]); - set_servo_gate_state(servo_gate.gate_state); + gate_state_t state = (gate_state_t) atoi(values[idx]); + servo_gate_set_state(state); } } @@ -154,4 +188,59 @@ bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *para file->flags = FS_FILE_FLAGS_HEADER_INCLUDED; return true; -} \ No newline at end of file +} + + +bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *params[], char *values[]) { + // Mappings + // c0 (bool): servo_gate_enable + // c1 (int): gate_close_duty_cycle + // c2 (int): gate_open_duty_cycle + // ee (bool): save_to_eeprom + + static char servo_gate_json_buffer[256]; + bool save_to_eeprom = false; + + + // Control + for (int idx = 0; idx < num_params; idx += 1) { + if (strcmp(params[idx], "c0") == 0) { + bool enable = string_to_boolean(values[idx]); + servo_gate.eeprom_servo_gate_config.servo_gate_enable = enable; + } + else if (strcmp(params[idx], "c1") == 0) { + float gate_close_duty_cycle = strtof(values[idx], NULL); + servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle = gate_close_duty_cycle; + } + else if (strcmp(params[idx], "c2") == 0) { + float gate_open_duty_cycle = strtof(values[idx], NULL); + servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle = gate_open_duty_cycle; + } + else if (strcmp(params[idx], "ee") == 0) { + save_to_eeprom = string_to_boolean(values[idx]); + } + } + + // Perform action + if (save_to_eeprom) { + servo_gate_config_save(); // Note: this will save settings for both + } + + // Response + snprintf(servo_gate_json_buffer, + sizeof(servo_gate_json_buffer), + "%s" + "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f}", + http_json_header, + boolean_to_string(servo_gate.eeprom_servo_gate_config.servo_gate_enable), + servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle, + servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle); + + size_t data_length = strlen(servo_gate_json_buffer); + file->data = servo_gate_json_buffer; + file->len = data_length; + file->index = data_length; + file->flags = FS_FILE_FLAGS_HEADER_INCLUDED; + + return true; +} diff --git a/src/servo_gate.h b/src/servo_gate.h index 94a6cd0..1652f56 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -2,6 +2,8 @@ #define SERVO_GATE_H_ #include +#include +#include #include "http_rest.h" #define EEPROM_SERVO_GATE_CONFIG_REV 1 // 16 byte @@ -26,7 +28,9 @@ typedef struct { eeprom_servo_gate_config_t eeprom_servo_gate_config; gate_state_t gate_state; - // RTOS queue + // RTOS control + TaskHandle_t servo_gate_control_task_handler; + QueueHandle_t servo_gate_control_queue; } servo_gate_t; @@ -37,7 +41,9 @@ extern "C" { bool servo_gate_init(void); bool servo_gate_config_save(void); bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *params[], char *values[]); +bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *params[], char *values[]); +void servo_gate_set_state(gate_state_t state); #ifdef __cplusplus } From ac14a6794ceb147ce1b15ad611613de91b79baaa Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 18 Apr 2024 22:06:49 +1200 Subject: [PATCH 03/16] Add web config --- src/html/web_portal.html | 33 ++++++++++++++++++++++++++++++++- src/servo_gate.c | 2 +- tests/web_test.py | 5 +++++ 3 files changed, 38 insertions(+), 2 deletions(-) diff --git a/src/html/web_portal.html b/src/html/web_portal.html index 687dc35..aaabf5e 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -13,7 +13,7 @@ @@ -490,6 +490,30 @@ + + diff --git a/src/servo_gate.c b/src/servo_gate.c index 0455ada..ad47cbd 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -18,31 +18,33 @@ const uint16_t _pwm_full_scale_level = 65535; const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { .servo_gate_config_rev = EEPROM_SERVO_GATE_CONFIG_REV, .servo_gate_enable = false, - .gate_close_duty_cycle = 0.05, - .gate_open_duty_cycle = 0.1, + .shutter0_close_duty_cycle = 0.05, + .shutter0_open_duty_cycle = 0.12, + .shutter1_close_duty_cycle = 0.05, + .shutter1_open_duty_cycle = 0.12, }; void _servo_gate_set_state(gate_state_t state) { - uint32_t servo0_level; - uint32_t servo1_level; + uint32_t shutter0_duty_cycle; + uint32_t shutter1_duty_cycle; switch (state) { case GATE_CLOSE: - servo0_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle; - servo1_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle; + shutter0_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle; + shutter1_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle; break; case GATE_OPEN: - servo0_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle; - servo1_level = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle; + shutter0_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle; + shutter1_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle; break; default: return; } - uint32_t reg_level = (servo1_level) << 16 | servo0_level; + uint32_t reg_level = (shutter0_duty_cycle) << 16 | shutter1_duty_cycle; // Write both levels to the pwm at the same time hw_write_masked( @@ -194,8 +196,10 @@ bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *para bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *params[], char *values[]) { // Mappings // c0 (bool): servo_gate_enable - // c1 (int): gate_close_duty_cycle - // c2 (int): gate_open_duty_cycle + // c1 (int): shutter0_close_duty_cycle + // c2 (int): shutter0_open_duty_cycle + // c3 (int): shutter1_close_duty_cycle + // c4 (int): shutter1_open_duty_cycle // ee (bool): save_to_eeprom static char servo_gate_json_buffer[256]; @@ -209,12 +213,20 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par servo_gate.eeprom_servo_gate_config.servo_gate_enable = enable; } else if (strcmp(params[idx], "c1") == 0) { - float gate_close_duty_cycle = strtof(values[idx], NULL); - servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle = gate_close_duty_cycle; + float duty_cycle = strtof(values[idx], NULL); + servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle = duty_cycle; } else if (strcmp(params[idx], "c2") == 0) { - float gate_open_duty_cycle = strtof(values[idx], NULL); - servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle = gate_open_duty_cycle; + float duty_cycle = strtof(values[idx], NULL); + servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle = duty_cycle; + } + else if (strcmp(params[idx], "c3") == 0) { + float duty_cycle = strtof(values[idx], NULL); + servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle = duty_cycle; + } + else if (strcmp(params[idx], "c4") == 0) { + float duty_cycle = strtof(values[idx], NULL); + servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle = duty_cycle; } else if (strcmp(params[idx], "ee") == 0) { save_to_eeprom = string_to_boolean(values[idx]); @@ -230,11 +242,13 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par snprintf(servo_gate_json_buffer, sizeof(servo_gate_json_buffer), "%s" - "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f}", + "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f}", http_json_header, boolean_to_string(servo_gate.eeprom_servo_gate_config.servo_gate_enable), - servo_gate.eeprom_servo_gate_config.gate_close_duty_cycle, - servo_gate.eeprom_servo_gate_config.gate_open_duty_cycle); + servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle, + servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle, + servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle, + servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle); size_t data_length = strlen(servo_gate_json_buffer); file->data = servo_gate_json_buffer; diff --git a/src/servo_gate.h b/src/servo_gate.h index 1652f56..c47fe6f 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -6,7 +6,7 @@ #include #include "http_rest.h" -#define EEPROM_SERVO_GATE_CONFIG_REV 1 // 16 byte +#define EEPROM_SERVO_GATE_CONFIG_REV 2 // 16 byte typedef enum { GATE_DISABLED = 0, @@ -18,12 +18,13 @@ typedef enum { typedef struct { uint16_t servo_gate_config_rev; bool servo_gate_enable; - float gate_close_duty_cycle; - float gate_open_duty_cycle; + float shutter0_close_duty_cycle; + float shutter0_open_duty_cycle; + float shutter1_close_duty_cycle; + float shutter1_open_duty_cycle; } eeprom_servo_gate_config_t; - typedef struct { eeprom_servo_gate_config_t eeprom_servo_gate_config; gate_state_t gate_state; From c9431f0e909187700c2c60b2e579aaaaf72155ba Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Mon, 29 Apr 2024 21:15:15 +1200 Subject: [PATCH 05/16] Update default settings --- .vscode/settings.json | 5 ++++- src/servo_gate.c | 8 ++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 4c872bb..c9fa85f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -45,6 +45,9 @@ "stdbool.h": "c", "configuration.h": "c", "uart.h": "c", - "common.h": "c" + "common.h": "c", + "clocks.h": "c", + "pwm.h": "c", + "servo_gate.h": "c" } } diff --git a/src/servo_gate.c b/src/servo_gate.c index ad47cbd..efbcf9e 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -18,10 +18,10 @@ const uint16_t _pwm_full_scale_level = 65535; const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { .servo_gate_config_rev = EEPROM_SERVO_GATE_CONFIG_REV, .servo_gate_enable = false, - .shutter0_close_duty_cycle = 0.05, - .shutter0_open_duty_cycle = 0.12, + .shutter0_close_duty_cycle = 0.09, + .shutter0_open_duty_cycle = 0.05, .shutter1_close_duty_cycle = 0.05, - .shutter1_open_duty_cycle = 0.12, + .shutter1_open_duty_cycle = 0.09, }; @@ -107,7 +107,7 @@ bool servo_gate_config_init() { // Initialize settings if (servo_gate.eeprom_servo_gate_config.servo_gate_enable) { - servo_gate.gate_state = GATE_CLOSE; + servo_gate.gate_state = GATE_OPEN; } else { servo_gate.gate_state = GATE_DISABLED; From 63c5510470c7a929ad634090a069aacebac86b20 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Mon, 29 Apr 2024 23:56:32 +1200 Subject: [PATCH 06/16] Replace task with mutex --- .vscode/settings.json | 3 +- src/html/web_portal.html | 5 +++ src/servo_gate.c | 74 ++++++++++++++++++---------------------- src/servo_gate.h | 12 ++++--- 4 files changed, 47 insertions(+), 47 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index c9fa85f..1020abe 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -48,6 +48,7 @@ "common.h": "c", "clocks.h": "c", "pwm.h": "c", - "servo_gate.h": "c" + "servo_gate.h": "c", + "semphr.h": "c" } } diff --git a/src/html/web_portal.html b/src/html/web_portal.html index 7789d5e..f37529f 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -520,6 +520,11 @@ +
+ Servo Gate Dwell Time (ms) + +
+ diff --git a/src/servo_gate.c b/src/servo_gate.c index efbcf9e..3e04709 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -1,11 +1,11 @@ #include #include -#include "servo_gate.h" #include "hardware/pwm.h" #include "hardware/clocks.h" #include "configuration.h" #include "eeprom.h" #include "common.h" +#include "servo_gate.h" // Attributes servo_gate_t servo_gate; @@ -22,14 +22,15 @@ const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { .shutter0_open_duty_cycle = 0.05, .shutter1_close_duty_cycle = 0.05, .shutter1_open_duty_cycle = 0.09, + .servo_gate_dwell_time_ms = 200ul, }; -void _servo_gate_set_state(gate_state_t state) { +void _servo_gate_set_current_state() { uint32_t shutter0_duty_cycle; uint32_t shutter1_duty_cycle; - switch (state) + switch (servo_gate.gate_state) { case GATE_CLOSE: shutter0_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle; @@ -55,22 +56,21 @@ void _servo_gate_set_state(gate_state_t state) { } -void servo_gate_set_state(gate_state_t state) { - if (servo_gate.servo_gate_control_queue) { - xQueueSend(servo_gate.servo_gate_control_queue, &state, portMAX_DELAY); - } -} +void servo_gate_set_state(gate_state_t state, bool block_for_dwell) { + if (servo_gate.servo_gate_control_mux) { + xSemaphoreTake(servo_gate.servo_gate_control_mux, portMAX_DELAY); + servo_gate.gate_state = state; -void servo_gate_control_task(void * p) { - while (true) { - gate_state_t new_state; - xQueueReceive(servo_gate.servo_gate_control_queue, &new_state, portMAX_DELAY); + // Apply the state + _servo_gate_set_current_state(); - servo_gate.gate_state = new_state; + if (block_for_dwell) { + BaseType_t scheduler_state = xTaskGetSchedulerState(); + delay_ms(servo_gate.eeprom_servo_gate_config.servo_gate_dwell_time_ms, scheduler_state); + } - // Apply the new state - _servo_gate_set_state(new_state); + xSemaphoreGive(servo_gate.servo_gate_control_mux); } } @@ -145,18 +145,9 @@ bool servo_gate_init() { pwm_init(pwm_gpio_to_slice_num(SERVO1_PWM_PIN), &cfg, true); // Start the RTOS task and queue - servo_gate.servo_gate_control_queue = xQueueCreate(1, sizeof(gate_state_t)); - - xTaskCreate( - servo_gate_control_task, - "servo_gate_controller", - configMINIMAL_STACK_SIZE, - NULL, - 8, - &servo_gate.servo_gate_control_task_handler - ); + servo_gate.servo_gate_control_mux = xSemaphoreCreateMutex(); - servo_gate_set_state(servo_gate.gate_state); + servo_gate_set_state(servo_gate.gate_state, false); return true; } @@ -172,7 +163,7 @@ bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *para for (int idx = 0; idx < num_params; idx += 1) { if (strcmp(params[idx], "g0") == 0) { gate_state_t state = (gate_state_t) atoi(values[idx]); - servo_gate_set_state(state); + servo_gate_set_state(state, false); } } @@ -196,10 +187,11 @@ bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *para bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *params[], char *values[]) { // Mappings // c0 (bool): servo_gate_enable - // c1 (int): shutter0_close_duty_cycle - // c2 (int): shutter0_open_duty_cycle - // c3 (int): shutter1_close_duty_cycle - // c4 (int): shutter1_open_duty_cycle + // c1 (float): shutter0_close_duty_cycle + // c2 (float): shutter0_open_duty_cycle + // c3 (float): shutter1_close_duty_cycle + // c4 (float): shutter1_open_duty_cycle + // c5 (int | uint32_t): servo_gate_dwell_time_ms // ee (bool): save_to_eeprom static char servo_gate_json_buffer[256]; @@ -213,20 +205,19 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par servo_gate.eeprom_servo_gate_config.servo_gate_enable = enable; } else if (strcmp(params[idx], "c1") == 0) { - float duty_cycle = strtof(values[idx], NULL); - servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle = duty_cycle; + servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle = strtof(values[idx], NULL);; } else if (strcmp(params[idx], "c2") == 0) { - float duty_cycle = strtof(values[idx], NULL); - servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle = duty_cycle; + servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle = strtof(values[idx], NULL);; } else if (strcmp(params[idx], "c3") == 0) { - float duty_cycle = strtof(values[idx], NULL); - servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle = duty_cycle; + servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle = strtof(values[idx], NULL);; } else if (strcmp(params[idx], "c4") == 0) { - float duty_cycle = strtof(values[idx], NULL); - servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle = duty_cycle; + servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle = strtof(values[idx], NULL);; + } + else if (strcmp(params[idx], "c5") == 0) { + servo_gate.eeprom_servo_gate_config.servo_gate_dwell_time_ms = strtol(values[idx], NULL, 10);; } else if (strcmp(params[idx], "ee") == 0) { save_to_eeprom = string_to_boolean(values[idx]); @@ -242,13 +233,14 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par snprintf(servo_gate_json_buffer, sizeof(servo_gate_json_buffer), "%s" - "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f}", + "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f,\"c5\":%ld}", http_json_header, boolean_to_string(servo_gate.eeprom_servo_gate_config.servo_gate_enable), servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle, - servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle); + servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle, + servo_gate.eeprom_servo_gate_config.servo_gate_dwell_time_ms); size_t data_length = strlen(servo_gate_json_buffer); file->data = servo_gate_json_buffer; diff --git a/src/servo_gate.h b/src/servo_gate.h index c47fe6f..b8df7cb 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -3,10 +3,10 @@ #include #include -#include +#include #include "http_rest.h" -#define EEPROM_SERVO_GATE_CONFIG_REV 2 // 16 byte +#define EEPROM_SERVO_GATE_CONFIG_REV 1 // 16 byte typedef enum { GATE_DISABLED = 0, @@ -22,6 +22,9 @@ typedef struct { float shutter0_open_duty_cycle; float shutter1_close_duty_cycle; float shutter1_open_duty_cycle; + + // Time taken to open or close the servo gate + uint32_t servo_gate_dwell_time_ms; } eeprom_servo_gate_config_t; @@ -30,8 +33,7 @@ typedef struct { gate_state_t gate_state; // RTOS control - TaskHandle_t servo_gate_control_task_handler; - QueueHandle_t servo_gate_control_queue; + xSemaphoreHandle servo_gate_control_mux; } servo_gate_t; @@ -44,7 +46,7 @@ bool servo_gate_config_save(void); bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *params[], char *values[]); bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *params[], char *values[]); -void servo_gate_set_state(gate_state_t state); +void servo_gate_set_state(gate_state_t, bool); #ifdef __cplusplus } From f8d5586c2e5924d2fdb008a66db5128247baa606 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Wed, 1 May 2024 00:52:47 +1200 Subject: [PATCH 07/16] Add charge mode integration --- src/charge_mode.cpp | 26 ++++++++ src/html/web_portal.html | 5 -- src/servo_gate.c | 127 +++++++++++++++++++++++++++------------ src/servo_gate.h | 10 +-- 4 files changed, 121 insertions(+), 47 deletions(-) diff --git a/src/charge_mode.cpp b/src/charge_mode.cpp index d43d53c..c5dccbb 100644 --- a/src/charge_mode.cpp +++ b/src/charge_mode.cpp @@ -20,6 +20,7 @@ #include "neopixel_led.h" #include "profile.h" #include "common.h" +#include "servo_gate.h" uint8_t charge_weight_digits[] = {0, 0, 0, 0, 0}; @@ -28,6 +29,8 @@ charge_mode_config_t charge_mode_config; // Scale related extern scale_config_t scale_config; +extern servo_gate_t servo_gate; + const eeprom_charge_mode_data_t default_charge_mode_data = { .charge_mode_data_rev = EEPROM_CHARGE_MODE_DATA_REV, @@ -167,6 +170,10 @@ void charge_mode_wait_for_complete() { true ); + if (servo_gate.gate_state != GATE_DISABLED) { + servo_gate_set_state(GATE_OPEN, false); + } + // Update current status char target_weight_string[WEIGHT_STRING_LEN]; float_to_string(target_weight_string, charge_mode_config.target_charge_weight, charge_mode_config.eeprom_charge_mode_data.decimal_places); @@ -229,6 +236,11 @@ void charge_mode_wait_for_complete() { motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 0); // TODO: When tuning off the coarse trickler, also move reverse to back off some powder + + // FIXME: Temporary solution to implement precharge + if (servo_gate.gate_state != GATE_DISABLED) { + servo_gate_set_state(GATE_OPEN_HALF, false); + } } // Update PID variables @@ -262,6 +274,19 @@ void charge_mode_wait_for_complete() { vTaskDelay(pdMS_TO_TICKS(20)); // Wait for other tasks to complete + // FIXME: Temporary solution to implement precharge + if (servo_gate.gate_state != GATE_DISABLED) { + servo_gate_set_state(GATE_CLOSE, true); + + BaseType_t scheduler_state = xTaskGetSchedulerState(); + delay_ms(1000, scheduler_state); + motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 2); + + delay_ms(1000, scheduler_state); + + motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 0); + } + charge_mode_config.charge_mode_state = CHARGE_MODE_WAIT_FOR_CUP_REMOVAL; } @@ -365,6 +390,7 @@ void charge_mode_wait_for_cup_return() { snprintf(title_string, sizeof(title_string), "Return Cup"); + FloatRingBuffer data_buffer(5); while (true) { diff --git a/src/html/web_portal.html b/src/html/web_portal.html index f37529f..7789d5e 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -520,11 +520,6 @@ -
- Servo Gate Dwell Time (ms) - -
- diff --git a/src/servo_gate.c b/src/servo_gate.c index 3e04709..ad23944 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -22,30 +22,11 @@ const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { .shutter0_open_duty_cycle = 0.05, .shutter1_close_duty_cycle = 0.05, .shutter1_open_duty_cycle = 0.09, - .servo_gate_dwell_time_ms = 200ul, }; -void _servo_gate_set_current_state() { - uint32_t shutter0_duty_cycle; - uint32_t shutter1_duty_cycle; - - switch (servo_gate.gate_state) - { - case GATE_CLOSE: - shutter0_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle; - shutter1_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle; - break; - case GATE_OPEN: - shutter0_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle; - shutter1_duty_cycle = _pwm_full_scale_level * servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle; - break; - - default: - return; - } - - uint32_t reg_level = (shutter0_duty_cycle) << 16 | shutter1_duty_cycle; +void _set_duty_cycle(uint16_t shutter0_duty_cycle, uint16_t shutter1_duty_cycle) { + uint32_t reg_level = ((uint32_t) shutter0_duty_cycle) << 16 | shutter1_duty_cycle; // Write both levels to the pwm at the same time hw_write_masked( @@ -56,21 +37,86 @@ void _servo_gate_set_current_state() { } -void servo_gate_set_state(gate_state_t state, bool block_for_dwell) { - if (servo_gate.servo_gate_control_mux) { - xSemaphoreTake(servo_gate.servo_gate_control_mux, portMAX_DELAY); +void _servo_gate_set_current_state(float open_ratio) { + uint32_t shutter0_duty_cycle; + uint32_t shutter1_duty_cycle; - servo_gate.gate_state = state; + float shutter0_range = servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle - servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle; + float shutter1_range = servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle - servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle; + shutter0_duty_cycle = _pwm_full_scale_level * (servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle + shutter0_range * open_ratio); + shutter1_duty_cycle = _pwm_full_scale_level * (servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle + shutter1_range * open_ratio); + + _set_duty_cycle(shutter0_duty_cycle, shutter1_duty_cycle); +} + + +void servo_gate_set_state(gate_state_t state, bool block_wait) { + if (servo_gate.servo_gate_control_queue) { + xQueueSend(servo_gate.servo_gate_control_queue, &state, portMAX_DELAY); + } + + if (block_wait) { + xSemaphoreTake(servo_gate.move_ready_semphore, portMAX_DELAY); + } +} + + +void servo_gate_control_task(void * p) { + float prev_open_ratio = -1.0f; + float accel = 2.0f; + + while (true) { + gate_state_t new_state; + xQueueReceive(servo_gate.servo_gate_control_queue, &new_state, portMAX_DELAY); // Apply the state - _servo_gate_set_current_state(); - if (block_for_dwell) { - BaseType_t scheduler_state = xTaskGetSchedulerState(); - delay_ms(servo_gate.eeprom_servo_gate_config.servo_gate_dwell_time_ms, scheduler_state); + float new_open_ratio; + switch (new_state) { + case GATE_OPEN: + new_open_ratio = 0.0f; + break; + case GATE_OPEN_HALF: + new_open_ratio = 0.5f; + break; + case GATE_OPEN_QUARTER: + new_open_ratio = 0.25f; + break; + case GATE_CLOSE: + new_open_ratio = 1.0f; + break; + default: + break; + } + + if (prev_open_ratio < 0) { + _servo_gate_set_current_state(new_open_ratio); + } + else { + float delta = new_open_ratio - prev_open_ratio; + uint32_t ramp_time_us = fabs(delta / accel) * 1e6; + + uint32_t start_time = time_us_32(); + uint32_t stop_time = start_time + ramp_time_us; + while (true) { + uint32_t current_time = time_us_32(); + if (current_time > stop_time) { + break; + } + + float percentage = (current_time - start_time) / (float) ramp_time_us; + float current_ratio = prev_open_ratio + delta * percentage; + + _servo_gate_set_current_state(current_ratio); + } + + _servo_gate_set_current_state(new_open_ratio); } - xSemaphoreGive(servo_gate.servo_gate_control_mux); + // Signal the motion is ready + xSemaphoreGive(servo_gate.move_ready_semphore); + + prev_open_ratio = new_open_ratio; } } @@ -145,7 +191,17 @@ bool servo_gate_init() { pwm_init(pwm_gpio_to_slice_num(SERVO1_PWM_PIN), &cfg, true); // Start the RTOS task and queue - servo_gate.servo_gate_control_mux = xSemaphoreCreateMutex(); + servo_gate.servo_gate_control_queue = xQueueCreate(1, sizeof(gate_state_t)); + servo_gate.move_ready_semphore = xSemaphoreCreateBinary(); + + xTaskCreate( + servo_gate_control_task, + "servo_gate_controller", + configMINIMAL_STACK_SIZE, + NULL, + 8, + &servo_gate.servo_gate_control_task_handler + ); servo_gate_set_state(servo_gate.gate_state, false); @@ -191,7 +247,6 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par // c2 (float): shutter0_open_duty_cycle // c3 (float): shutter1_close_duty_cycle // c4 (float): shutter1_open_duty_cycle - // c5 (int | uint32_t): servo_gate_dwell_time_ms // ee (bool): save_to_eeprom static char servo_gate_json_buffer[256]; @@ -216,9 +271,6 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par else if (strcmp(params[idx], "c4") == 0) { servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle = strtof(values[idx], NULL);; } - else if (strcmp(params[idx], "c5") == 0) { - servo_gate.eeprom_servo_gate_config.servo_gate_dwell_time_ms = strtol(values[idx], NULL, 10);; - } else if (strcmp(params[idx], "ee") == 0) { save_to_eeprom = string_to_boolean(values[idx]); } @@ -233,14 +285,13 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par snprintf(servo_gate_json_buffer, sizeof(servo_gate_json_buffer), "%s" - "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f,\"c5\":%ld}", + "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f}", http_json_header, boolean_to_string(servo_gate.eeprom_servo_gate_config.servo_gate_enable), servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle, - servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle, - servo_gate.eeprom_servo_gate_config.servo_gate_dwell_time_ms); + servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle); size_t data_length = strlen(servo_gate_json_buffer); file->data = servo_gate_json_buffer; diff --git a/src/servo_gate.h b/src/servo_gate.h index b8df7cb..967467f 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -3,6 +3,7 @@ #include #include +#include #include #include "http_rest.h" @@ -12,6 +13,8 @@ typedef enum { GATE_DISABLED = 0, GATE_CLOSE, GATE_OPEN, + GATE_OPEN_HALF, + GATE_OPEN_QUARTER, } gate_state_t; @@ -22,9 +25,6 @@ typedef struct { float shutter0_open_duty_cycle; float shutter1_close_duty_cycle; float shutter1_open_duty_cycle; - - // Time taken to open or close the servo gate - uint32_t servo_gate_dwell_time_ms; } eeprom_servo_gate_config_t; @@ -33,7 +33,9 @@ typedef struct { gate_state_t gate_state; // RTOS control - xSemaphoreHandle servo_gate_control_mux; + TaskHandle_t servo_gate_control_task_handler; + QueueHandle_t servo_gate_control_queue; + SemaphoreHandle_t move_ready_semphore; } servo_gate_t; From e59ac9e6d0f8422d951ce7271c1c341517671934 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 2 May 2024 01:29:21 +1200 Subject: [PATCH 08/16] remove code to half close the gate --- src/charge_mode.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/charge_mode.cpp b/src/charge_mode.cpp index c5dccbb..7d9d0a9 100644 --- a/src/charge_mode.cpp +++ b/src/charge_mode.cpp @@ -236,11 +236,6 @@ void charge_mode_wait_for_complete() { motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 0); // TODO: When tuning off the coarse trickler, also move reverse to back off some powder - - // FIXME: Temporary solution to implement precharge - if (servo_gate.gate_state != GATE_DISABLED) { - servo_gate_set_state(GATE_OPEN_HALF, false); - } } // Update PID variables From e71bd009ba6d3cf2c2c24e4747ba6e950fb8be8d Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 2 May 2024 13:51:16 +1200 Subject: [PATCH 09/16] Add precharge time --- src/html/web_portal.html | 13 +++++++++---- src/servo_gate.c | 39 +++++++++++++++++++-------------------- src/servo_gate.h | 7 +++---- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/src/html/web_portal.html b/src/html/web_portal.html index 7789d5e..931191d 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -502,22 +502,27 @@
Shutter 0 Close Duty Cycle - +
Shutter 0 Open Duty Cycle - +
Shutter 1 Close Duty Cycle - +
Shutter 1 Open Duty Cycle - + +
+ +
+ Shutter Speed (pct/s) +
diff --git a/src/servo_gate.c b/src/servo_gate.c index ad23944..fe529a3 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -18,14 +18,15 @@ const uint16_t _pwm_full_scale_level = 65535; const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { .servo_gate_config_rev = EEPROM_SERVO_GATE_CONFIG_REV, .servo_gate_enable = false, - .shutter0_close_duty_cycle = 0.09, - .shutter0_open_duty_cycle = 0.05, - .shutter1_close_duty_cycle = 0.05, - .shutter1_open_duty_cycle = 0.09, + .shutter0_close_duty_cycle = 0.09f, + .shutter0_open_duty_cycle = 0.05f, + .shutter1_close_duty_cycle = 0.05f, + .shutter1_open_duty_cycle = 0.09f, + .shutter_speed_pct_s = 1.0f, }; -void _set_duty_cycle(uint16_t shutter0_duty_cycle, uint16_t shutter1_duty_cycle) { +static void inline _set_duty_cycle(uint16_t shutter0_duty_cycle, uint16_t shutter1_duty_cycle) { uint32_t reg_level = ((uint32_t) shutter0_duty_cycle) << 16 | shutter1_duty_cycle; // Write both levels to the pwm at the same time @@ -52,8 +53,8 @@ void _servo_gate_set_current_state(float open_ratio) { void servo_gate_set_state(gate_state_t state, bool block_wait) { - if (servo_gate.servo_gate_control_queue) { - xQueueSend(servo_gate.servo_gate_control_queue, &state, portMAX_DELAY); + if (servo_gate.control_queue) { + xQueueSend(servo_gate.control_queue, &state, portMAX_DELAY); } if (block_wait) { @@ -64,11 +65,10 @@ void servo_gate_set_state(gate_state_t state, bool block_wait) { void servo_gate_control_task(void * p) { float prev_open_ratio = -1.0f; - float accel = 2.0f; while (true) { gate_state_t new_state; - xQueueReceive(servo_gate.servo_gate_control_queue, &new_state, portMAX_DELAY); + xQueueReceive(servo_gate.control_queue, &new_state, portMAX_DELAY); // Apply the state float new_open_ratio; @@ -76,12 +76,6 @@ void servo_gate_control_task(void * p) { case GATE_OPEN: new_open_ratio = 0.0f; break; - case GATE_OPEN_HALF: - new_open_ratio = 0.5f; - break; - case GATE_OPEN_QUARTER: - new_open_ratio = 0.25f; - break; case GATE_CLOSE: new_open_ratio = 1.0f; break; @@ -94,7 +88,7 @@ void servo_gate_control_task(void * p) { } else { float delta = new_open_ratio - prev_open_ratio; - uint32_t ramp_time_us = fabs(delta / accel) * 1e6; + uint32_t ramp_time_us = fabs(delta / servo_gate.eeprom_servo_gate_config.shutter_speed_pct_s) * 1e6; uint32_t start_time = time_us_32(); uint32_t stop_time = start_time + ramp_time_us; @@ -191,7 +185,7 @@ bool servo_gate_init() { pwm_init(pwm_gpio_to_slice_num(SERVO1_PWM_PIN), &cfg, true); // Start the RTOS task and queue - servo_gate.servo_gate_control_queue = xQueueCreate(1, sizeof(gate_state_t)); + servo_gate.control_queue = xQueueCreate(1, sizeof(gate_state_t)); servo_gate.move_ready_semphore = xSemaphoreCreateBinary(); xTaskCreate( @@ -200,7 +194,7 @@ bool servo_gate_init() { configMINIMAL_STACK_SIZE, NULL, 8, - &servo_gate.servo_gate_control_task_handler + &servo_gate.control_task_handler ); servo_gate_set_state(servo_gate.gate_state, false); @@ -247,6 +241,7 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par // c2 (float): shutter0_open_duty_cycle // c3 (float): shutter1_close_duty_cycle // c4 (float): shutter1_open_duty_cycle + // c5 (float): shutter_speed_pct_s // ee (bool): save_to_eeprom static char servo_gate_json_buffer[256]; @@ -271,6 +266,9 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par else if (strcmp(params[idx], "c4") == 0) { servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle = strtof(values[idx], NULL);; } + else if (strcmp(params[idx], "c5") == 0) { + servo_gate.eeprom_servo_gate_config.shutter_speed_pct_s = strtof(values[idx], NULL);; + } else if (strcmp(params[idx], "ee") == 0) { save_to_eeprom = string_to_boolean(values[idx]); } @@ -285,13 +283,14 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par snprintf(servo_gate_json_buffer, sizeof(servo_gate_json_buffer), "%s" - "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f}", + "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f,\"c5\":%0.3f}", http_json_header, boolean_to_string(servo_gate.eeprom_servo_gate_config.servo_gate_enable), servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle, - servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle); + servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle, + servo_gate.eeprom_servo_gate_config.shutter_speed_pct_s); size_t data_length = strlen(servo_gate_json_buffer); file->data = servo_gate_json_buffer; diff --git a/src/servo_gate.h b/src/servo_gate.h index 967467f..8bcec13 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -13,8 +13,6 @@ typedef enum { GATE_DISABLED = 0, GATE_CLOSE, GATE_OPEN, - GATE_OPEN_HALF, - GATE_OPEN_QUARTER, } gate_state_t; @@ -25,6 +23,7 @@ typedef struct { float shutter0_open_duty_cycle; float shutter1_close_duty_cycle; float shutter1_open_duty_cycle; + float shutter_speed_pct_s; // Per shutter speed (percentage per second) } eeprom_servo_gate_config_t; @@ -33,8 +32,8 @@ typedef struct { gate_state_t gate_state; // RTOS control - TaskHandle_t servo_gate_control_task_handler; - QueueHandle_t servo_gate_control_queue; + TaskHandle_t control_task_handler; + QueueHandle_t control_queue; SemaphoreHandle_t move_ready_semphore; } servo_gate_t; From 9254131cfc4887a6cb5fe5f488c3c01510a2f09e Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 2 May 2024 16:10:13 +1200 Subject: [PATCH 10/16] Integrate the control of pre-charge to the charge mode --- src/charge_mode.cpp | 39 +++++++++++++++++++++++++++++---------- src/charge_mode.h | 7 ++++++- src/html/web_portal.html | 20 ++++++++++++++++++++ src/servo_gate.c | 13 +++++++++---- 4 files changed, 64 insertions(+), 15 deletions(-) diff --git a/src/charge_mode.cpp b/src/charge_mode.cpp index 7d9d0a9..e0a7494 100644 --- a/src/charge_mode.cpp +++ b/src/charge_mode.cpp @@ -43,6 +43,11 @@ const eeprom_charge_mode_data_t default_charge_mode_data = { .decimal_places = DP_2, + // Precharges + .precharge_enable = false, + .precharge_time_ms = 1000, + .precharge_speed_rps = 2, + // LED related .neopixel_normal_charge_colour = urgb_u32(0, 0xFF, 0), // green .neopixel_under_charge_colour = urgb_u32(0xFF, 0xFF, 0), // yellow @@ -170,6 +175,7 @@ void charge_mode_wait_for_complete() { true ); + // If the servo gate is used then it has to be opened if (servo_gate.gate_state != GATE_DISABLED) { servo_gate_set_state(GATE_OPEN, false); } @@ -269,15 +275,12 @@ void charge_mode_wait_for_complete() { vTaskDelay(pdMS_TO_TICKS(20)); // Wait for other tasks to complete - // FIXME: Temporary solution to implement precharge - if (servo_gate.gate_state != GATE_DISABLED) { + // Precharge + if (charge_mode_config.eeprom_charge_mode_data.precharge_enable && servo_gate.gate_state != GATE_DISABLED) { servo_gate_set_state(GATE_CLOSE, true); - BaseType_t scheduler_state = xTaskGetSchedulerState(); - delay_ms(1000, scheduler_state); - motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 2); - - delay_ms(1000, scheduler_state); + motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, charge_mode_config.eeprom_charge_mode_data.precharge_speed_rps); + vTaskDelay(pdMS_TO_TICKS(charge_mode_config.eeprom_charge_mode_data.precharge_time_ms)); motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 0); } @@ -549,6 +552,9 @@ bool http_rest_charge_mode_config(struct fs_file *file, int num_params, char *pa // c7 (float): set_point_sd_margin // c8 (float): set_point_mean_margin // c9 (int): decimal point enum + // c10 (bool): precharge_enable + // c11 (int): precharge_time_ms + // c12 (float): precharge_speed_rps // ee (bool): save to eeprom @@ -572,6 +578,17 @@ bool http_rest_charge_mode_config(struct fs_file *file, int num_params, char *pa else if (strcmp(params[idx], "c9") == 0) { charge_mode_config.eeprom_charge_mode_data.decimal_places = (decimal_places_t) atoi(values[idx]); } + + // Pre charge related settings + else if (strcmp(params[idx], "c10") == 0) { + charge_mode_config.eeprom_charge_mode_data.precharge_enable = string_to_boolean(values[idx]); + } + else if (strcmp(params[idx], "c11") == 0) { + charge_mode_config.eeprom_charge_mode_data.precharge_time_ms = strtol(values[idx], NULL, 10); + } + else if (strcmp(params[idx], "c12") == 0) { + charge_mode_config.eeprom_charge_mode_data.precharge_speed_rps = strtof(values[idx], NULL); + } // LED related settings else if (strcmp(params[idx], "c1") == 0) { @@ -601,17 +618,19 @@ bool http_rest_charge_mode_config(struct fs_file *file, int num_params, char *pa sizeof(charge_mode_json_buffer), "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\n\r\n" "{\"c1\":\"#%06lx\",\"c2\":\"#%06lx\",\"c3\":\"#%06lx\",\"c4\":\"#%06lx\"," - "\"c5\":%.3f,\"c6\":%.3f,\"c7\":%.3f,\"c8\":%.3f,\"c9\":%d}", + "\"c5\":%.3f,\"c6\":%.3f,\"c7\":%.3f,\"c8\":%.3f,\"c9\":%d,\"c10\":\"%s\",\"c11\":%ld,\"c12\":%0.3f}", charge_mode_config.eeprom_charge_mode_data.neopixel_normal_charge_colour, charge_mode_config.eeprom_charge_mode_data.neopixel_under_charge_colour, charge_mode_config.eeprom_charge_mode_data.neopixel_over_charge_colour, charge_mode_config.eeprom_charge_mode_data.neopixel_not_ready_colour, - charge_mode_config.eeprom_charge_mode_data.coarse_stop_threshold, charge_mode_config.eeprom_charge_mode_data.fine_stop_threshold, charge_mode_config.eeprom_charge_mode_data.set_point_sd_margin, charge_mode_config.eeprom_charge_mode_data.set_point_mean_margin, - charge_mode_config.eeprom_charge_mode_data.decimal_places); + charge_mode_config.eeprom_charge_mode_data.decimal_places, + boolean_to_string(charge_mode_config.eeprom_charge_mode_data.precharge_enable), + charge_mode_config.eeprom_charge_mode_data.precharge_time_ms, + charge_mode_config.eeprom_charge_mode_data.precharge_speed_rps); size_t data_length = strlen(charge_mode_json_buffer); file->data = charge_mode_json_buffer; diff --git a/src/charge_mode.h b/src/charge_mode.h index 05bf2ae..f87e006 100644 --- a/src/charge_mode.h +++ b/src/charge_mode.h @@ -6,7 +6,7 @@ #include "common.h" -#define EEPROM_CHARGE_MODE_DATA_REV 7 // 16 byte +#define EEPROM_CHARGE_MODE_DATA_REV 8 // 16 byte #define WEIGHT_STRING_LEN 8 @@ -29,6 +29,11 @@ typedef struct { decimal_places_t decimal_places; + // Precharge + bool precharge_enable; + uint32_t precharge_time_ms; + float precharge_speed_rps; + // LED related settings uint32_t neopixel_normal_charge_colour; uint32_t neopixel_under_charge_colour; diff --git a/src/html/web_portal.html b/src/html/web_portal.html index 931191d..1d0fba4 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -257,6 +257,26 @@ +
+ +
+ Enable Pre-Charge (requires servo gate) + +
+ +
+ Pre-Charge Duration (ms) + +
+ +
+ Pre-Charge Speed (rps) + +
+ diff --git a/src/servo_gate.c b/src/servo_gate.c index fe529a3..430b605 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -53,12 +53,17 @@ void _servo_gate_set_current_state(float open_ratio) { void servo_gate_set_state(gate_state_t state, bool block_wait) { + // Skip if not initialized if (servo_gate.control_queue) { xQueueSend(servo_gate.control_queue, &state, portMAX_DELAY); - } - if (block_wait) { - xSemaphoreTake(servo_gate.move_ready_semphore, portMAX_DELAY); + if (block_wait) { + xSemaphoreTake(servo_gate.move_ready_semphore, portMAX_DELAY); + + // Still wait for another second to ensure the gate is really closed + BaseType_t scheduler_state = xTaskGetSchedulerState(); + delay_ms(1000, scheduler_state); + } } } @@ -197,7 +202,7 @@ bool servo_gate_init() { &servo_gate.control_task_handler ); - servo_gate_set_state(servo_gate.gate_state, false); + // No, we don't set the servo gate state return true; } From 10b6ddaba5781600cf57b6b76ba809f50308063e Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 2 May 2024 20:57:07 +1200 Subject: [PATCH 11/16] Fix a bug where task is incorrectly blocked --- src/charge_mode.cpp | 5 +++-- src/servo_gate.c | 10 +++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/charge_mode.cpp b/src/charge_mode.cpp index e0a7494..9330ef7 100644 --- a/src/charge_mode.cpp +++ b/src/charge_mode.cpp @@ -273,8 +273,6 @@ void charge_mode_wait_for_complete() { last_error = error; } - vTaskDelay(pdMS_TO_TICKS(20)); // Wait for other tasks to complete - // Precharge if (charge_mode_config.eeprom_charge_mode_data.precharge_enable && servo_gate.gate_state != GATE_DISABLED) { servo_gate_set_state(GATE_CLOSE, true); @@ -284,6 +282,9 @@ void charge_mode_wait_for_complete() { motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, 0); } + else { + vTaskDelay(pdMS_TO_TICKS(20)); // Wait for other tasks to complete + } charge_mode_config.charge_mode_state = CHARGE_MODE_WAIT_FOR_CUP_REMOVAL; } diff --git a/src/servo_gate.c b/src/servo_gate.c index 430b605..80c5cc0 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -55,14 +55,13 @@ void _servo_gate_set_current_state(float open_ratio) { void servo_gate_set_state(gate_state_t state, bool block_wait) { // Skip if not initialized if (servo_gate.control_queue) { + // Clear the semaphore state + xSemaphoreTake(servo_gate.move_ready_semphore, 0); + xQueueSend(servo_gate.control_queue, &state, portMAX_DELAY); if (block_wait) { xSemaphoreTake(servo_gate.move_ready_semphore, portMAX_DELAY); - - // Still wait for another second to ensure the gate is really closed - BaseType_t scheduler_state = xTaskGetSchedulerState(); - delay_ms(1000, scheduler_state); } } } @@ -74,8 +73,8 @@ void servo_gate_control_task(void * p) { while (true) { gate_state_t new_state; xQueueReceive(servo_gate.control_queue, &new_state, portMAX_DELAY); - // Apply the state + // Calculate the new gate open ratio float new_open_ratio; switch (new_state) { case GATE_OPEN: @@ -88,6 +87,7 @@ void servo_gate_control_task(void * p) { break; } + // First time if (prev_open_ratio < 0) { _servo_gate_set_current_state(new_open_ratio); } From 3d73f9358d54a7448b597f4c0964cfe0c8048806 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Thu, 2 May 2024 21:33:59 +1200 Subject: [PATCH 12/16] Implement different open and close speed --- src/html/web_portal.html | 7 ++++++- src/servo_gate.c | 20 ++++++++++++++------ src/servo_gate.h | 3 ++- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/html/web_portal.html b/src/html/web_portal.html index 1d0fba4..14582ca 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -541,10 +541,15 @@
- Shutter Speed (pct/s) + Shutter Close Speed (pct/s)
+
+ Shutter Open Speed (pct/s) + +
+ diff --git a/src/servo_gate.c b/src/servo_gate.c index 80c5cc0..99ab43a 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -22,7 +22,8 @@ const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { .shutter0_open_duty_cycle = 0.05f, .shutter1_close_duty_cycle = 0.05f, .shutter1_open_duty_cycle = 0.09f, - .shutter_speed_pct_s = 1.0f, + .shutter_open_speed_pct_s = 5.0f, + .shutter_close_speed_pct_s = 3.0f, }; @@ -93,7 +94,9 @@ void servo_gate_control_task(void * p) { } else { float delta = new_open_ratio - prev_open_ratio; - uint32_t ramp_time_us = fabs(delta / servo_gate.eeprom_servo_gate_config.shutter_speed_pct_s) * 1e6; + float speed = delta < 0 ? servo_gate.eeprom_servo_gate_config.shutter_open_speed_pct_s : + servo_gate.eeprom_servo_gate_config.shutter_close_speed_pct_s; + uint32_t ramp_time_us = fabs(delta / speed) * 1e6; uint32_t start_time = time_us_32(); uint32_t stop_time = start_time + ramp_time_us; @@ -246,7 +249,8 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par // c2 (float): shutter0_open_duty_cycle // c3 (float): shutter1_close_duty_cycle // c4 (float): shutter1_open_duty_cycle - // c5 (float): shutter_speed_pct_s + // c5 (float): shutter_close_speed_pct_s + // c6 (float): shutter_open_speed_pct_s // ee (bool): save_to_eeprom static char servo_gate_json_buffer[256]; @@ -272,7 +276,10 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle = strtof(values[idx], NULL);; } else if (strcmp(params[idx], "c5") == 0) { - servo_gate.eeprom_servo_gate_config.shutter_speed_pct_s = strtof(values[idx], NULL);; + servo_gate.eeprom_servo_gate_config.shutter_close_speed_pct_s = strtof(values[idx], NULL);; + } + else if (strcmp(params[idx], "c6") == 0) { + servo_gate.eeprom_servo_gate_config.shutter_open_speed_pct_s = strtof(values[idx], NULL);; } else if (strcmp(params[idx], "ee") == 0) { save_to_eeprom = string_to_boolean(values[idx]); @@ -288,14 +295,15 @@ bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *par snprintf(servo_gate_json_buffer, sizeof(servo_gate_json_buffer), "%s" - "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f,\"c5\":%0.3f}", + "{\"c0\":%s,\"c1\":%0.3f,\"c2\":%0.3f,\"c3\":%0.3f,\"c4\":%0.3f,\"c5\":%0.3f,\"c6\":%0.3f}", http_json_header, boolean_to_string(servo_gate.eeprom_servo_gate_config.servo_gate_enable), servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle, servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle, - servo_gate.eeprom_servo_gate_config.shutter_speed_pct_s); + servo_gate.eeprom_servo_gate_config.shutter_close_speed_pct_s, + servo_gate.eeprom_servo_gate_config.shutter_open_speed_pct_s); size_t data_length = strlen(servo_gate_json_buffer); file->data = servo_gate_json_buffer; diff --git a/src/servo_gate.h b/src/servo_gate.h index 8bcec13..bda6160 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -23,7 +23,8 @@ typedef struct { float shutter0_open_duty_cycle; float shutter1_close_duty_cycle; float shutter1_open_duty_cycle; - float shutter_speed_pct_s; // Per shutter speed (percentage per second) + float shutter_close_speed_pct_s; // Per shutter speed (percentage per second) + float shutter_open_speed_pct_s; // Per shutter speed (percentage per second) } eeprom_servo_gate_config_t; From 4fe752b037eaaa48a49af3aae5940954f05c6e27 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Fri, 3 May 2024 22:22:16 +1200 Subject: [PATCH 13/16] Integrate the servo gate state to the cleanup mode, add control to the servo gate to the mini 12864 display --- src/cleanup_mode.cpp | 8 +++++++- src/mui_menu.c | 36 ++++++++++++++++++++++++++++++++++-- src/servo_gate.c | 12 ++++++++++++ src/servo_gate.h | 1 + 4 files changed, 54 insertions(+), 3 deletions(-) diff --git a/src/cleanup_mode.cpp b/src/cleanup_mode.cpp index 7c6f32a..cf3d080 100644 --- a/src/cleanup_mode.cpp +++ b/src/cleanup_mode.cpp @@ -13,12 +13,13 @@ #include "common.h" #include "charge_mode.h" #include "cleanup_mode.h" +#include "servo_gate.h" // Memory from other modules extern QueueHandle_t encoder_event_queue; extern charge_mode_config_t charge_mode_config; - +extern servo_gate_t servo_gate; extern AppState_t exit_state; extern QueueHandle_t encoder_event_queue; @@ -78,6 +79,11 @@ void cleanup_render_task(void *p) { u8g2_SetFont(display_handler, u8g2_font_profont11_tf); u8g2_DrawStr(display_handler, 5, 45, buf); + memset(buf, 0x0, sizeof(buf)); + sprintf(buf, "Servo Gate: %s", gate_state_to_string(servo_gate.gate_state)); + u8g2_SetFont(display_handler, u8g2_font_profont11_tf); + u8g2_DrawStr(display_handler, 5, 55, buf); + u8g2_SendBuffer(display_handler); vTaskDelayUntil(&last_render_tick, pdMS_TO_TICKS(20)); diff --git a/src/mui_menu.c b/src/mui_menu.c index 5c63c99..643e7b1 100644 --- a/src/mui_menu.c +++ b/src/mui_menu.c @@ -13,13 +13,14 @@ #include "version.h" #include "common.h" #include "profile.h" +#include "servo_gate.h" +// External modules/varaibles extern uint8_t charge_weight_digits[]; extern AppState_t exit_state; extern charge_mode_config_t charge_mode_config; - -// Imported from and_scale module +extern servo_gate_t servo_gate; extern scale_config_t scale_config; extern eeprom_profile_data_t profile_data; @@ -189,6 +190,22 @@ uint8_t render_profile_misc_details(mui_t *ui, uint8_t msg) { } +uint8_t render_servo_gate_state_with_action(mui_t *ui, uint8_t msg) { + uint8_t return_value = mui_u8g2_u8_radio_wm_pi(ui, msg); + + switch(msg) { + case MUIF_MSG_CURSOR_SELECT: + { + uint8_t *value = (uint8_t *)muif_get_data(ui->uif); + servo_gate_set_state((gate_state_t) *value, false); + break; + } + } + + return return_value; +} + + muif_t muif_list[] = { /* normal text style */ @@ -229,6 +246,9 @@ muif_t muif_list[] = { // Render version MUIF_RO("VE", render_version_page), + // Render servo gate state + MUIF_VARIABLE("RB",&servo_gate.gate_state, render_servo_gate_state_with_action), + // input for a number between 0 to 9 // MUIF_U8G2_U8_MIN_MAX("N4", &charge_weight_digits[4], 0, 9, mui_u8g2_u8_min_max_wm_mud_pi), MUIF_U8G2_U8_MIN_MAX("N3", &charge_weight_digits[3], 0, 9, mui_u8g2_u8_min_max_wm_mud_pi), @@ -359,6 +379,7 @@ fds_t fds_data[] = { MUI_31 "Scale|" MUI_32 "Profile Manager|" MUI_37 "EEPROM|" + MUI_39 "Servo Gate|" MUI_35 "Reboot|" MUI_36 "Version|" MUI_1 "<-Return" // Back to main menu @@ -477,6 +498,17 @@ fds_t fds_data[] = { MUI_XYA("GC", 5, 49, 2) MUI_XYA("GC", 5, 61, 3) + // Servo Gate submenu + MUI_FORM(39) + MUI_STYLE(1) + MUI_LABEL(5,10, "Servo Gate Control") + MUI_XY("HL", 0,13) + + MUI_XYAT("RB", 5, 25, 0, "Disable") + MUI_XYAT("RB", 5, 37, 1, "Open") + MUI_XYAT("RB", 5, 49, 2, "Close") + MUI_XYAT("BN", 64, 59, 30, " OK ") // Jump to form 30 + // Wirelss submenu MUI_FORM(40) #ifdef RASPBERRYPI_PICO_W diff --git a/src/servo_gate.c b/src/servo_gate.c index 99ab43a..75e9ab1 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -27,6 +27,16 @@ const eeprom_servo_gate_config_t default_eeprom_servo_gate_config = { }; +const char * _gate_state_string[] = { + "Disabled", + "Close", + "Open" +}; +const char * gate_state_to_string(gate_state_t state) { + return _gate_state_string[state]; +} + + static void inline _set_duty_cycle(uint16_t shutter0_duty_cycle, uint16_t shutter1_duty_cycle) { uint32_t reg_level = ((uint32_t) shutter0_duty_cycle) << 16 | shutter1_duty_cycle; @@ -118,7 +128,9 @@ void servo_gate_control_task(void * p) { // Signal the motion is ready xSemaphoreGive(servo_gate.move_ready_semphore); + // Update state prev_open_ratio = new_open_ratio; + servo_gate.gate_state = new_state; } } diff --git a/src/servo_gate.h b/src/servo_gate.h index bda6160..ee29512 100644 --- a/src/servo_gate.h +++ b/src/servo_gate.h @@ -47,6 +47,7 @@ bool servo_gate_init(void); bool servo_gate_config_save(void); bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *params[], char *values[]); bool http_rest_servo_gate_config(struct fs_file *file, int num_params, char *params[], char *values[]); +const char * gate_state_to_string(gate_state_t); void servo_gate_set_state(gate_state_t, bool); From 7fe9aa1cca4416d40c48bdf7dc22b75cb31a75a4 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Fri, 3 May 2024 22:24:51 +1200 Subject: [PATCH 14/16] rename pct to % --- src/html/web_portal.html | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/html/web_portal.html b/src/html/web_portal.html index 14582ca..06f086b 100644 --- a/src/html/web_portal.html +++ b/src/html/web_portal.html @@ -541,12 +541,12 @@
- Shutter Close Speed (pct/s) + Shutter Close Speed (%/s)
- Shutter Open Speed (pct/s) + Shutter Open Speed (%/s)
From 3373a3748b9f00c874155694c82b049411e84763 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Sun, 5 May 2024 13:24:12 +1200 Subject: [PATCH 15/16] Close the gate if the servo gate is enabled regardless --- src/charge_mode.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/charge_mode.cpp b/src/charge_mode.cpp index 9330ef7..de8232a 100644 --- a/src/charge_mode.cpp +++ b/src/charge_mode.cpp @@ -273,10 +273,13 @@ void charge_mode_wait_for_complete() { last_error = error; } - // Precharge - if (charge_mode_config.eeprom_charge_mode_data.precharge_enable && servo_gate.gate_state != GATE_DISABLED) { + // Close the gate if the servo gate is present + if (servo_gate.gate_state != GATE_DISABLED) { servo_gate_set_state(GATE_CLOSE, true); + } + // Precharge + if (charge_mode_config.eeprom_charge_mode_data.precharge_enable && servo_gate.gate_state != GATE_DISABLED) { motor_set_speed(SELECT_COARSE_TRICKLER_MOTOR, charge_mode_config.eeprom_charge_mode_data.precharge_speed_rps); vTaskDelay(pdMS_TO_TICKS(charge_mode_config.eeprom_charge_mode_data.precharge_time_ms)); From 363ee36be3d704a1437f138e34fb751157318108 Mon Sep 17 00:00:00 2001 From: Ran Bao Date: Sun, 5 May 2024 23:26:48 +1200 Subject: [PATCH 16/16] Fix type --- src/servo_gate.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/servo_gate.c b/src/servo_gate.c index 75e9ab1..c3a88bd 100644 --- a/src/servo_gate.c +++ b/src/servo_gate.c @@ -50,8 +50,8 @@ static void inline _set_duty_cycle(uint16_t shutter0_duty_cycle, uint16_t shutte void _servo_gate_set_current_state(float open_ratio) { - uint32_t shutter0_duty_cycle; - uint32_t shutter1_duty_cycle; + uint16_t shutter0_duty_cycle; + uint16_t shutter1_duty_cycle; float shutter0_range = servo_gate.eeprom_servo_gate_config.shutter0_close_duty_cycle - servo_gate.eeprom_servo_gate_config.shutter0_open_duty_cycle; float shutter1_range = servo_gate.eeprom_servo_gate_config.shutter1_close_duty_cycle - servo_gate.eeprom_servo_gate_config.shutter1_open_duty_cycle; @@ -240,8 +240,9 @@ bool http_rest_servo_gate_state(struct fs_file *file, int num_params, char *para // Response snprintf(servo_gate_json_buffer, sizeof(servo_gate_json_buffer), - "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\n\r\n" + "%s" "{\"g0\":%d}", + http_json_header, (int) servo_gate.gate_state); size_t data_length = strlen(servo_gate_json_buffer);