From 3fecb101b700885eb658245cc2c5f3e270a776e4 Mon Sep 17 00:00:00 2001
From: FReenen <git@finnvanreenen.nl>
Date: Mon, 9 Sep 2024 10:17:58 +0200
Subject: [PATCH] add edits from vesting dagen

---
 rx_esp32/src/servos.c | 40 ++++++++++++++++++++++++++++------------
 rx_esp32/src/servos.h |  3 +++
 rx_esp32/src/ws.c     |  4 ++--
 3 files changed, 33 insertions(+), 14 deletions(-)

diff --git a/rx_esp32/src/servos.c b/rx_esp32/src/servos.c
index 6155087..1947eb3 100644
--- a/rx_esp32/src/servos.c
+++ b/rx_esp32/src/servos.c
@@ -27,19 +27,25 @@ void servo_init(void)
 	Servos[0].ledc_ch = LEDC_CHANNEL_0;
 	Servos[0].pulse_min = SERVO_DUTY_MIN;
 	Servos[0].pulse_mid = SERVO_DUTY_MID;
-	Servos[0].pulse_max = SERVO_DUTY_MAX - 100;
-	Servos[0].mid_delay = 10;
+	Servos[0].pulse_max = SERVO_DUTY_MAX;
+	Servos[0].in_min = 0;
+	Servos[0].in_mid = 128;
+	Servos[0].in_max = 255;
+	Servos[0].mid_delay = 0;
 	Servos[0].mid_delay_timer = 0;
 	Servos[0].reversed = false;
 
 	Servos[1].pin = SERVOS_CH1_PIN;
 	Servos[1].ledc_ch = LEDC_CHANNEL_1;
-	Servos[1].pulse_min = SERVO_DUTY_MIN;
-	Servos[1].pulse_mid = SERVO_DUTY_MID;
-	Servos[1].pulse_max = SERVO_DUTY_MAX;
-	Servos[1].mid_delay = 0;
+	Servos[1].pulse_min = SERVO_DUTY_MIN-35;
+	Servos[1].pulse_mid = SERVO_DUTY_MID-35;
+	Servos[1].pulse_max = SERVO_DUTY_MAX-35;
+	Servos[1].in_min = 0;
+	Servos[1].in_mid = 103;
+	Servos[1].in_max = 255;
+	Servos[1].mid_delay = 5;
 	Servos[1].mid_delay_timer = 0;
-	Servos[1].reversed = false;
+	Servos[1].reversed = true;
 
 	// Prepare and then apply the LEDC PWM timer configuration
 	ledc_timer_config_t ledc_timer = {
@@ -90,12 +96,22 @@ void servo_set(uint8_t ch, uint8_t pos)
 		}
 	}
 
-   if (Servos[ch].reversed)
+	double pos_double = ((double) pos) - Servos[ch].in_mid;
+
+	if (pos_double > 0.0)
 	{
-		pos = -((int16_t)pos) + 255;
+		pos_double = pos_double/(Servos[ch].in_max - Servos[ch].in_mid);
+	}
+	else 
+	{
+		pos_double = pos_double/(Servos[ch].in_mid - Servos[ch].in_min);
+	}
+
+   if (Servos[ch].reversed)
+	{
+		pos_double = -pos_double;
 	}
 
-	double pos_double = ((double) pos) - 128;
 
 	if (pos_double < 0)
 	{
@@ -125,11 +141,11 @@ void servo_set(uint8_t ch, uint8_t pos)
 	uint32_t duty;
 	if (pos_double > 0.0)
 	{
-		duty = (pos_double/128.0 * (Servos[ch].pulse_max - Servos[ch].pulse_mid)) + Servos[ch].pulse_mid;
+		duty = (pos_double * (Servos[ch].pulse_max - Servos[ch].pulse_mid)) + Servos[ch].pulse_mid;
 	}
 	else 
 	{
-      duty = (pos_double/128.0 * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid;
+      duty = (pos_double * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid;
    }
 
 	if (duty < SERVO_DUTY_MIN)
diff --git a/rx_esp32/src/servos.h b/rx_esp32/src/servos.h
index d8afd91..c680505 100644
--- a/rx_esp32/src/servos.h
+++ b/rx_esp32/src/servos.h
@@ -12,6 +12,9 @@ typedef struct {
 	uint16_t pulse_min;
 	uint16_t pulse_mid;
 	uint16_t pulse_max;
+	uint8_t in_min;
+	uint8_t in_mid;
+	uint8_t in_max;
 	uint8_t mid_delay;
 	int16_t mid_delay_timer;
 	bool reversed;
diff --git a/rx_esp32/src/ws.c b/rx_esp32/src/ws.c
index 99c3dc6..bf42dc0 100644
--- a/rx_esp32/src/ws.c
+++ b/rx_esp32/src/ws.c
@@ -42,7 +42,7 @@ static void ws_event_handler(void* handler_args, esp_event_base_t base, int32_t
 			break;
 
 		case WEBSOCKET_EVENT_DATA:
-			// LOG_D("ws_event_handler: data recieved opcode %d", data->op_code);
+			LOG_D("ws_event_handler: data recieved opcode %d", data->op_code);
 			if (data->op_code == 0x02)
 			{
 				LOG_D("ws_event_handler: binary data recieved");
@@ -212,7 +212,7 @@ void ws_sendData(ws_client_t client)
 {
 	if ((client->txBuffer_wp > 0) && esp_websocket_client_is_connected(client->handle))
 	{
-		// LOG_D("ws_sendData: (%d b) '%.*s'", client->txBuffer_wp, client->txBuffer_wp, (char *)&client->txBuffer[0]);
+		LOG_D("ws_sendData: (%d b) '%.*s'", client->txBuffer_wp, client->txBuffer_wp, (char *)&client->txBuffer[0]);
 		esp_websocket_client_send_text(client->handle, (char *)&client->txBuffer[0], client->txBuffer_wp, 1000 * portTICK_PERIOD_MS);
 		client->txBuffer_wp = 0;
 	}