From b7d266e245f2d9b9b246e65daec4bbc596bc595d Mon Sep 17 00:00:00 2001 From: FReenen Date: Thu, 25 Jul 2024 12:26:53 +0200 Subject: [PATCH] add servo controll --- rx_esp32/lib/cli | 2 +- rx_esp32/src/CMakeLists.txt | 7 ++- rx_esp32/src/commands.c | 106 +++++++++++++++++++++++++----------- rx_esp32/src/config.h | 11 +++- rx_esp32/src/led.c | 2 +- rx_esp32/src/main.c | 22 +++++--- rx_esp32/src/servos.c | 86 +++++++++++++++++++++++++++++ rx_esp32/src/servos.h | 10 ++++ 8 files changed, 199 insertions(+), 47 deletions(-) create mode 100644 rx_esp32/src/servos.c create mode 100644 rx_esp32/src/servos.h diff --git a/rx_esp32/lib/cli b/rx_esp32/lib/cli index 8261e60..ae59563 160000 --- a/rx_esp32/lib/cli +++ b/rx_esp32/lib/cli @@ -1 +1 @@ -Subproject commit 8261e605bccf98e6f028d0ca221ffb81ebc4c0c2 +Subproject commit ae59563e7da09ba3f974be92b8a02bdfdf7d1dab diff --git a/rx_esp32/src/CMakeLists.txt b/rx_esp32/src/CMakeLists.txt index 2bf1fa4..00ecd11 100644 --- a/rx_esp32/src/CMakeLists.txt +++ b/rx_esp32/src/CMakeLists.txt @@ -9,8 +9,13 @@ idf_component_register( ./wifi_scan.c ./led.c ./commands.c + ./servos.c INCLUDE_DIRS "./" PRIV_REQUIRES - cli led_strip + # project components + cli + # idf extra components + led_strip + # idf base components spi_flash driver nvs_flash esp_wifi ) diff --git a/rx_esp32/src/commands.c b/rx_esp32/src/commands.c index 34ac845..0a4018d 100644 --- a/rx_esp32/src/commands.c +++ b/rx_esp32/src/commands.c @@ -1,7 +1,7 @@ -#include "CLI/CLI.h" -#include "CMDList/CMDList.h" -#include "config.h" +#include +#include +#include #include #include #include @@ -10,6 +10,7 @@ #include "commands.h" #include "utils.h" #include "led.h" +#include "servos.h" typedef enum { BOAT_AVAILABLE, @@ -18,9 +19,52 @@ typedef enum { } boatStatus_t; boatStatus_t BoatStatus = BOAT_AVAILABLE; +char* readInt(char* str, uint32_t* out) +{ + *out = 0; + while ((*str >= '0') && (*str <= '9')) + { + *out *= 10; + *out += *str - '0'; + str++; + } + return str; +} + int cmd_contrl(char* line, void* cli) { - return 0; + uint32_t ch_d[2]; + int out = 0; + char msg[40]; + + char* arg = getNextArg(line, ':'); + if (arg == NULL) + { + snprintf(&msg[0], 40, "no arguments given, expect 2\n"); + CLI_stringOut((CLI_t*)cli, &msg[0]); + out = -1; + } + else + { + readInt(arg, &ch_d[0]); + arg = getNextArg(arg, ','); + if (arg == NULL) + { + snprintf(&msg[0], 40, "one argument given, expect 2\n"); + CLI_stringOut((CLI_t*)cli, &msg[0]); + out = -2; + } + else + { + readInt(arg, &ch_d[1]); + + servo_set(0, (int16_t)ch_d[0]); + servo_set(1, (int16_t)ch_d[1]); + snprintf(&msg[0], 40, "channel data: 0:%lu, 1:%lu\n", ch_d[0], ch_d[1]); + CLI_stringOut((CLI_t*)cli, &msg[0]); + } + } + return out; } extern volatile bool running; @@ -38,21 +82,21 @@ int cmd_status(char* line, void* cli) if (arg != NULL) { if (strcmp(arg, "available") == 0) - { - BoatStatus = BOAT_AVAILABLE; - } - else if (strcmp(arg, "inctrl") == 0) - { - BoatStatus = BOAT_INCTRL; - } - else if (strcmp(arg, "locked") == 0) - { - BoatStatus = BOAT_LOCKED; - } - else - { - ret = -1; - } + { + BoatStatus = BOAT_AVAILABLE; + } + else if (strcmp(arg, "inctrl") == 0) + { + BoatStatus = BOAT_INCTRL; + } + else if (strcmp(arg, "locked") == 0) + { + BoatStatus = BOAT_LOCKED; + } + else + { + ret = -1; + } } char str[20]; @@ -75,21 +119,9 @@ int cmd_status(char* line, void* cli) return ret; } -char* readInt(char* str, int* out) -{ - *out = 0; - while ((*str >= '0') && (*str <= '9')) - { - *out *= 10; - *out += *str - '0'; - str++; - } - return str; -} - int setLed(char* line, void* cli) { - int r, g, b; + uint32_t r, g, b; char* arg = getNextArg(line, ':'); readInt(arg, &r); @@ -101,12 +133,20 @@ int setLed(char* line, void* cli) led_setRGB(r, g, b); char msg[30]; - snprintf(&msg[0], 30, "led: r%d g%d n%d\n", r, g, b); + snprintf(&msg[0], 30, "led: r%lu g%lu n%lu\n", r, g, b); CLI_stringOut((CLI_t*)cli, &msg[0]); return 0; } +int cmd_history(char* line, void* cli) +{ + CLI_PrintHistory((CLI_t*)cli); + return 0; +} + + const CMD_t Commands[] = { + { "history", &cmd_history }, { "d", &cmd_contrl }, { "shutdown", &cmd_shutdown }, { "status", &cmd_status }, diff --git a/rx_esp32/src/config.h b/rx_esp32/src/config.h index ad7fdce..a7cd884 100644 --- a/rx_esp32/src/config.h +++ b/rx_esp32/src/config.h @@ -14,7 +14,14 @@ // static uint8_t BoatId = 1; -// cli config -#define HISTORY +#define HISTORY // enable cli history + +#define SERVOS_CH0 {LEDC_CHANNEL_0, 5}, +#define SERVOS_CH1 {LEDC_CHANNEL_1, 6} +#define SERVOS_CH2 +#define SERVOS_CH3 +#define SERVOS_CH4 +#define SERVOS_CH5 +#define SERVOS_CH6 #endif diff --git a/rx_esp32/src/led.c b/rx_esp32/src/led.c index c20fde2..7a8449c 100644 --- a/rx_esp32/src/led.c +++ b/rx_esp32/src/led.c @@ -1,7 +1,7 @@ #include "led.h" #include -#include "led_strip.h" +#include #define BUILDIN_LED_GPIO 8 diff --git a/rx_esp32/src/main.c b/rx_esp32/src/main.c index 9e1abf9..3047095 100644 --- a/rx_esp32/src/main.c +++ b/rx_esp32/src/main.c @@ -2,19 +2,19 @@ #include #include -#include "nvs_flash.h" -#include "esp_wifi.h" -#include "esp_event.h" -#include "esp_task_wdt.h" +#include +#include #include #include #include "config.h" + #include "utils.h" #include "commands.h" #include "wifi.h" #include "led.h" +#include "servos.h" bool volatile running = true; CMDList_t* cmdList; @@ -27,7 +27,7 @@ bool rxBuffer_overflow = false; */ int charOut_uart(const char* c) { - printf(c); + printf("%c", *c); return 0; } @@ -54,7 +54,8 @@ void app_main() { printChipInfo(); led_init(); - // led_setRGB(150, 0, 0); + led_setRGB(0, 0, 20); + servo_init(); wifiInit(); wifi_connect(); @@ -71,16 +72,16 @@ void app_main() { * happened. */ if (bits & WIFI_CONNECTED_BIT) { - // led_setRGB(0, 150, 0); + led_setRGB(0, 20, 0); printf("INFO: main: connected to ap SSID '%s'\n", WIFI_SSID); } else if (bits & WIFI_FAIL_BIT) { - printf("ERROR: main: Failed to connect to SSID '%s',\n", WIFI_SSID); + led_setRGB(2, 0, 0); } else { - printf("FAITAL: main: UNEXPECTED EVENT\n"); + led_setRGB(2, 0, 0); return; } @@ -110,6 +111,9 @@ void app_main() { } } + led_setRGB(0, 0, 0); + + servo_deinit(); CLI_deinit(&cli_uart); CMDList_deinit(cmdList); diff --git a/rx_esp32/src/servos.c b/rx_esp32/src/servos.c new file mode 100644 index 0000000..c6bb520 --- /dev/null +++ b/rx_esp32/src/servos.c @@ -0,0 +1,86 @@ +#include "driver/ledc.h" +#include + +#include "config.h" + +#include "servos.h" + +#define SERVO_LEDC_TIMER LEDC_TIMER_0 +#define SERVO_LEDC_MODE LEDC_LOW_SPEED_MODE +#define SERVO_LEDC_CHANNEL LEDC_CHANNEL_0 +#define SERVO_LEDC_DUTY_RES LEDC_TIMER_20_BIT // Set duty resolution to 13 bits +// #define SERVO_DUTY_MIN ( 26214) // 2**20 * ( 500/20000) // 500us of the 20ms +#define SERVO_DUTY_MIN ( 52429) // 2**20 * (1000/20000) // 1000us of the 20ms +#define SERVO_DUTY_MAX (104858) // 2**20 * (2000/20000) // 2000us of the 20ms +// #define SERVO_DUTY_MAX (131072) // 2**20 * (2500/20000) // 2500us of the 20ms +#define SERVO_DUTY_DEFUALT ( 78643) // 2**20 * (1500/20000) // 2500us of the 20ms + +#define SERVO_DUTY_DIFF (SERVO_DUTY_MAX - SERVO_DUTY_MIN) + +typedef struct ServoCh_t { + ledc_channel_t channel; + int gpio; +} ServoCh_t; + +static ServoCh_t Server_chs[] = { + SERVOS_CH0 + SERVOS_CH1 + SERVOS_CH2 + SERVOS_CH3 + SERVOS_CH4 + SERVOS_CH5 + SERVOS_CH6 +}; + +void servo_init(void) +{ + // Prepare and then apply the LEDC PWM timer configuration + ledc_timer_config_t ledc_timer = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .timer_num = SERVO_LEDC_TIMER, + .duty_resolution = SERVO_LEDC_DUTY_RES, + .freq_hz = 50, // 20ms period + .clk_cfg = LEDC_AUTO_CLK + }; + ledc_timer_config(&ledc_timer); + + for (int i = sizeof(Server_chs)/sizeof(ServoCh_t)-1; i >= 0; i--) + { + // Prepare and then apply the LEDC PWM channel configuration + ledc_channel_config_t ledc_channel = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = Server_chs[i].channel, + .timer_sel = SERVO_LEDC_TIMER, + .intr_type = LEDC_INTR_DISABLE, + .gpio_num = Server_chs[i].gpio, + .duty = SERVO_DUTY_DEFUALT, // Set duty to 0% + .hpoint = 0 + }; + ledc_channel_config(&ledc_channel); + printf("DEBUG: inited servo channel %d (ledc: %u, gpio: %d)\n", i, Server_chs[i].channel, Server_chs[i].gpio); + } +} +void servo_deinit(void) +{ + ledc_timer_pause(LEDC_LOW_SPEED_MODE, SERVO_LEDC_TIMER); + for (int i = sizeof(Server_chs)/sizeof(ServoCh_t)-1; i > 0; i--) + { + ledc_stop(LEDC_LOW_SPEED_MODE, Server_chs[i].channel, 0); + } +} + +void servo_set(uint8_t ch, int16_t pos) +{ + uint32_t duty = (uint32_t) ((double)pos * (double)SERVO_DUTY_DIFF/(double)INT16_MAX); + duty += (SERVO_DUTY_MIN + SERVO_DUTY_DIFF/2); + if (duty < SERVO_DUTY_MIN) + { + duty = SERVO_DUTY_MIN; + } + else if (duty > SERVO_DUTY_MAX) + { + duty = SERVO_DUTY_MAX; + } + ledc_set_duty(SERVO_LEDC_MODE, Server_chs[ch].channel, duty); + ledc_update_duty(SERVO_LEDC_MODE, Server_chs[ch].channel); +} diff --git a/rx_esp32/src/servos.h b/rx_esp32/src/servos.h new file mode 100644 index 0000000..7cd458a --- /dev/null +++ b/rx_esp32/src/servos.h @@ -0,0 +1,10 @@ +#ifndef SERVOS_H +#define SERVOS_H + +#include + +void servo_init(void); +void servo_deinit(void); +void servo_set(uint8_t ch, int16_t pos); + +#endif // SERVOS_H \ No newline at end of file