add servo controll

rx_esp32/lib/cli

@@ -1 +1 @@
-Subproject commit 8261e605bccf98e6f028d0ca221ffb81ebc4c0c2
+Subproject commit ae59563e7da09ba3f974be92b8a02bdfdf7d1dab

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
 )

rx_esp32/src/commands.c

@@ -1,7 +1,7 @@
-#include "CLI/CLI.h"
+#include <CLI/CLI.h>
-#include "CMDList/CMDList.h"
+#include <CMDList/CMDList.h>
-#include "config.h"
 
+#include <stdint.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <limits.h>
@@ -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;
@@ -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 },

rx_esp32/src/config.h

@@ -14,7 +14,14 @@
 
 // static uint8_t BoatId = 1;
 
-// cli config
+#define HISTORY // enable cli history
-#define 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

rx_esp32/src/led.c

@@ -1,7 +1,7 @@
 #include "led.h"
 
 #include <stddef.h>
-#include "led_strip.h"
+#include <led_strip.h>
 
 #define BUILDIN_LED_GPIO 8
 

rx_esp32/src/main.c

@@ -2,19 +2,19 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "nvs_flash.h"
+#include <nvs_flash.h>
-#include "esp_wifi.h"
+#include <esp_task_wdt.h>
-#include "esp_event.h"
-#include "esp_task_wdt.h"
 
 #include <CLI/CLI.h>
 #include <CMDList/CMDList.h>
 
 #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);
 

rx_esp32/src/servos.c

@@ -0,0 +1,86 @@
+#include "driver/ledc.h"
+#include <stdint.h>
+
+#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);
+}

rx_esp32/src/servos.h

@@ -0,0 +1,10 @@
+#ifndef SERVOS_H
+#define SERVOS_H
+
+#include <stdint.h>
+
+void servo_init(void);
+void servo_deinit(void);
+void servo_set(uint8_t ch, int16_t pos);
+
+#endif // SERVOS_H