Movement algo update

OZE_Sensor/.cproject

@@ -37,7 +37,7 @@
 							</tool>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1014930278" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.1138728485" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
-								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1368159996" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.o0" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1368159996" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.ofast" valueType="enumerated"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.496682559" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
 									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>

OZE_Sensor/Core/Inc/node-red-config.h

@@ -24,14 +24,14 @@
 #define ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT 5 // percentage value for motor slow down/speed up boundary
 #define AXE_X_MIDDLE_VALUE 50 // percentage value of position for C limiter
 #define AXE_Y_MIDDLE_VALUE 50 // percentage value of position for C limiter
-#define TIME_MS_FOR_MOTOR_SPEED_LIMIT 5000
-#define NO_MOVE_TIMEOUT_MS 5000 // timeout for no movement detection
+#define TIME_MS_FOR_MOTOR_SPEED_LIMIT 50000
+#define NO_MOVE_TIMEOUT_MS 50000 // timeout for no movement detection
 
 #define WATCHDOG_ENABLED
 //#define USER_MOCKS
 //#define SERIAL_PROTOCOL_DBG
 //#define USE_UART8_INSTEAD_UART1
-//#define DBG_POSITION
+#define DBG_POSITION
 //#define MOCK_VOLTAGES_AND_CURRENS
 
 #endif /* INC_NODE_RED_CONFIG_H_ */

OZE_Sensor/Core/Src/meas_tasks.c

@@ -128,7 +128,7 @@ void MeasTasksInit (void) {
 
     osThreadAttr_t osThreadAttrEncoderTask = { 0 };
     osThreadAttrEncoderTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
-    osThreadAttrEncoderTask.priority       = (osPriority_t)osPriorityRealtime;
+    osThreadAttrEncoderTask.priority       = (osPriority_t)osPriorityRealtime7;
     encoderXTaskHandle                     = osThreadNew (EncoderTask, &encoderXTaskArg, &osThreadAttrEncoderTask);
     encoderYTaskHandle                     = osThreadNew (EncoderTask, &encoderYTaskArg, &osThreadAttrEncoderTask);
 #ifdef MOCK_VOLTAGES_AND_CURRENS

OZE_Sensor/Core/Src/position_task.c

@@ -35,7 +35,7 @@ void PositionControlTaskInit (void) {
 //    positionSettingMutex                           = osMutexNew (NULL);
     osThreadAttr_t osThreadAttrPositionControlTask = { 0 };
     osThreadAttrPositionControlTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
-    osThreadAttrPositionControlTask.priority       = (osPriority_t)osPriorityNormal;
+    osThreadAttrPositionControlTask.priority       = (osPriority_t)osPriorityHigh;
 
     positionXControlTaskInitArg.channel1                = TIM_CHANNEL_1;
     positionXControlTaskInitArg.channel2                = TIM_CHANNEL_2;
@@ -74,7 +74,7 @@ void PositionControlTaskInit (void) {
 }
 
 void PositionControlTask (void* argument) {
-    const int32_t PositionControlTaskTimeOut   = 100;
+    const int32_t PositionControlTaskTimeOut   = 10;
     PositionControlTaskInitArg* posCtrlTaskArg = (PositionControlTaskInitArg*)argument;
     PositionControlTaskData posCtrlData  __attribute__ ((aligned (32))) = { 0 };
     uint32_t motorStatus                       = 0;
@@ -148,7 +148,11 @@ void PositionControlTask (void* argument) {
 #ifdef DBG_POSITION
                             printf ("Axe %c speed up phase\n", posCtrlTaskArg->axe);
 #endif
-                            movementPhase         = speedUpPhase;
+                            if (abs (posCtrlData.positionSettingValue - startPosition) > ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) {
+                                movementPhase = speedUpPhase;
+                            } else {
+                                movementPhase = slowDownPhase;
+                            }
                             timeLeftMS            = 0;
                             moveCmdTimeoutCounter = 0;
                         } else {