From 251da124f7bb7ed15ab7cf44bea681884e86e4ab Mon Sep 17 00:00:00 2001
From: DashyFox <DashyFox@yandex.ru>
Date: Thu, 3 Oct 2024 20:20:52 +0300
Subject: [PATCH] min speed fix

---
 Core/Src/RobotFunctions.c    | 46 +++++++++++++++++++++---------------
 Core/Src/UART3_CMD_Handler.c |  8 ++++---
 Core/Src/main.c              | 30 +++++++++++------------
 3 files changed, 46 insertions(+), 38 deletions(-)

diff --git a/Core/Src/RobotFunctions.c b/Core/Src/RobotFunctions.c
index e829708..fa4c844 100644
--- a/Core/Src/RobotFunctions.c
+++ b/Core/Src/RobotFunctions.c
@@ -643,27 +643,35 @@ void setScrewkSpeed(uint8_t speed) {
 
 //(-v) 0 .. 100(stop) .. 200(+v)
 void setRollersSpeed(uint8_t up, uint8_t down) {
+	uint16_t min_speed_rpm = map(infoBlock.hwInfo.motors.speed_Rollers_min, 0, 200, 400, 800);
+	uint16_t min_speed_pwm_positive = map(min_speed_rpm, 0, 8000, 100, 200);
+	uint16_t min_speed_pwm_negative = 100 - map(min_speed_rpm, 0, 8000, 0, 100);
 
-	if (up < 100) {
-//		up = map(up, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);
-//		if (100 - up < min_speed) {
-//			up = 100 - min_speed;
-//		}
-	} else {
-//		up = map(up, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);
-//		if (up - 100 < min_speed) { // Ограничиваем положительную скорость минимальной
-//			up = 100 + min_speed;
-//		}
-	}
+    if (up == 100) {
+        up = 100;
+    } else if (up > 100) {
 
-	if (down < 100) {
-//		map(down, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);
+        if (up < min_speed_pwm_positive) {
+            up = min_speed_pwm_positive;
+        }
+    } else {
+        if (up > min_speed_pwm_negative) {
+            up = min_speed_pwm_negative;
+        }
+    }
 
-	} else {
-//		map(down, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);
+    if (down == 100) {
+        down = 100;
+    } else if (down > 100) {
+        if (down < min_speed_pwm_positive) {
+            down = min_speed_pwm_positive;
+        }
+    } else {
+        if (down > min_speed_pwm_negative) {
+            down = min_speed_pwm_negative;
+        }
+    }
 
-	}
-
-	Vz1 = 200 - up; // invert
-	Vz2 = down;
+    Vz1 = 200 - up; // Инвертируем значение скорости для одного из моторов
+    Vz2 = down;
 }
diff --git a/Core/Src/UART3_CMD_Handler.c b/Core/Src/UART3_CMD_Handler.c
index a143aa6..9fe0899 100644
--- a/Core/Src/UART3_CMD_Handler.c
+++ b/Core/Src/UART3_CMD_Handler.c
@@ -422,10 +422,13 @@ void UART3_GetStartupDelay(uint8_t *dataPtr, uint8_t len) {
 
 // 210 !!!!!!!!
 void UART3_SetMinRollerSpeed(uint8_t *dataPtr, uint8_t len) {
-	const uint8_t MIN_PARAM_LENGTH = 0;
+	const uint8_t MIN_PARAM_LENGTH = 1;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	infoBlock.hwInfo.motors.speed_Rollers_min = dataPtr[1];
+	saveInfoBlock();
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -435,8 +438,7 @@ void UART3_GetMinRollerSpeed(uint8_t *dataPtr, uint8_t len) {
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
-	uint8_t res[1];
-//		res[0] = ;
+	uint8_t res[1] = {infoBlock.hwInfo.motors.speed_Rollers_min};
 
 	SendResponse(dataPtr[0], 0, res, sizeof(res));
 }
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 32ebe38..e40d6e5 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -222,23 +222,21 @@ int main(void)
 			//          printNumber(SysTick->LOAD);
 			//          CDC_Transmit_FS(text, sizeof(text));
 
-//			char buffer[100];  // Буфер для формата строки
+			char buffer[100];
+			extern uint32_t vsk1;
+			extern uint32_t vsk2;
+			extern uint16_t timing1;
+			extern uint16_t timing2;
+			sprintf(buffer,
+					"timing1: %u ms, timing2: %u ms, vsk1: %u RPM, vsk2: %u RPM\n",
+					timing1, timing2, vsk1, vsk2);
+			CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));
 
-//			extern uint32_t vsk1;
-//			extern uint32_t vsk2;
-//			extern uint16_t timing1;
-//			extern uint16_t timing2;
-//			sprintf(buffer,
-//					"timing1: %u ms, timing2: %u ms, vsk1: %u RPM, vsk2: %u RPM\n",
-//					timing1, timing2, vsk1, vsk2);
-//
-//			CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));
-
-			char buffer[64];
-		    sprintf(buffer, "Current mode: %s, Current state: %s\n",
-		            getModeString(currentInfo.mode),
-		            getStateString(currentInfo.state));
-		    CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer));
+//			char buffer[64];
+//		    sprintf(buffer, "Current mode: %s, Current state: %s\n",
+//		            getModeString(currentInfo.mode),
+//		            getStateString(currentInfo.state));
+//		    CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer));
 
 
 		}