New PV positioning

OZE_Main/Core/Inc/measurements.h

@@ -39,6 +39,10 @@ struct _SesnorsInfo {
     uint8_t limitYSwitchDown;
     uint8_t limitYSwitchCenter;
     uint8_t powerSupplyFailMask;
+    float currentXPosition;
+    float currentYPosition;
+    uint8_t positionXWeak;
+    uint8_t positionYWeak;
 };
 
 typedef struct _RESMeasurements RESMeasurements;

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

@@ -27,5 +27,6 @@
 #define MEASURMENTS_SCHEDULER_INTERVAL_MS 1000
 
 //#define USER_MOCKS
+//#define USE_UART8_INSTEAD_UART1
 
 #endif /* INC_NODE_RED_CONFIG_H_ */

OZE_Main/Core/Inc/serial_protocol.h

@@ -45,6 +45,8 @@ enum _SerialProtocolCommands {
     spSetCurrentMeasGains,
     spSetCurrentMeasOffsets,
 	spResetSystem,
+	spSetPositonX,
+	spSetPositonY,
     spUnknown
 };
 

OZE_Main/Core/Src/mqtt_client.c

@@ -24,9 +24,9 @@
 #define MQTT_BUFSIZE	1024
 
 char* const subscribeTopicNames[MASTER_BOARD + SLAVES_COUNT] = { "Set/0", "Set/1", "Set/2", "Set/3", "Set/4" };
-#define MAX_COMMANDS_IN_MQTT_PAYLOAD 15
+#define MAX_COMMANDS_IN_MQTT_PAYLOAD 17
 char* const topicCommands[MAX_COMMANDS_IN_MQTT_PAYLOAD] = { "fanSpeed", "motorXon", "motorYon", "diode", "motorXMaxCurrent", "motorYMaxCurrent", "clearPeakElectricalMeasurements", "mainBoardRelay",
-    "setEncoderXValue", "setEncoderYValue", "setVoltageMeasGains", "setVoltageMeasOffsets", "setCurrentMeasGains", "setCurrentMeasOffsets", "resetSystem" };
+    "setEncoderXValue", "setEncoderYValue", "setVoltageMeasGains", "setVoltageMeasOffsets", "setCurrentMeasGains", "setCurrentMeasOffsets", "resetSystem", "setPositionX", "setPositionY" };
 
 enum _Topics
 {
@@ -45,6 +45,8 @@ enum _Topics
 	setCurrentMeasGains,
 	setCurrentMeasOffsets,
 	resetSystem,
+	setPositionX,
+	setPositionY
 };
 
 enum _BoardNoOverTopic
@@ -168,6 +170,10 @@ void MqttClientPubTask (void* argument) {
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"limitYSwitchUp\":%d, ", sensors->limitYSwitchUp);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"limitYSwitchDown\":%d, ", sensors->limitYSwitchDown);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"limitYSwitchCenter\":%d, ", sensors->limitYSwitchCenter);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"currentXPosition\":%.2f, ", sensors->currentXPosition);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"currentYPosition\":%.2f, ", sensors->currentYPosition);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"positionXWeak\":%d, ", sensors->positionXWeak);
+                        bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"positionYWeak\":%d, ", sensors->positionYWeak);
                         bytesInBuffer += sprintf (&messageBuffer[bytesInBuffer], "\"powerSupplyFailMask\":%d}", sensors->powerSupplyFailMask);
                         osMutexRelease (sensorsInfoMutex);
                     } else {
@@ -418,6 +424,19 @@ void MqttMessageArrived (MessageData* msg) {
                             break;
                         }
                     }
+                    break;
+                case setPositionX:
+                    if (cJSON_IsNumber (objectItem)) {
+                        data.spCommand                   = spSetPositonX;
+                        data.values.flaotValues.value[0] = objectItem->valuedouble;
+                    }
+                	break;
+                case setPositionY:
+                    if (cJSON_IsNumber (objectItem)) {
+                        data.spCommand                   = spSetPositonY;
+                        data.values.flaotValues.value[0] = objectItem->valuedouble;
+                    }
+                	break;
                 default: break;
                 }
             }
@@ -438,9 +457,15 @@ void MqttMessageArrived (MessageData* msg) {
     switch (topicForBoard) {
     case main_board: break;
     case board_1:
+#ifdef USE_UART8_INSTEAD_UART1
+        if (uart8TaskData.sendCmdToSlaveQueue != NULL) {
+            osMessageQueuePut (uart8TaskData.sendCmdToSlaveQueue, &data, 0, (TickType_t)100);
+        }
+#else
         if (uart1TaskData.sendCmdToSlaveQueue != NULL) {
             osMessageQueuePut (uart1TaskData.sendCmdToSlaveQueue, &data, 0, (TickType_t)100);
         }
+#endif
         printf ("Send cmd to board 1\n");
         break;
     case board_2:

OZE_Main/Core/Src/uart_tasks.c

@@ -45,8 +45,11 @@ uint8_t uart8RxBuffer[UART8_RX_BUFF_SIZE]        = { 0 };
 uint8_t uart8TxBuffer[UART8_TX_BUFF_SIZE]        = { 0 };
 uint8_t uart8TaskFrameData[INPUT_DATA_BUFF_SIZE] = { 0 };
 
-//uint8_t boardToUartNumberMap[SLAVES_COUNT] = { 8, 3, 6, 2 };
+#ifdef USE_UART8_INSTEAD_UART1
+uint8_t boardToUartNumberMap[SLAVES_COUNT] = { 8, 3, 6, 2 };
+#else
 uint8_t boardToUartNumberMap[SLAVES_COUNT] = { 1, 3, 6, 2 };
+#endif
 
 UartTaskData uart1TaskData = { 0 }; // Board 1
 UartTaskData uart3TaskData = { 0 }; // Board 2
@@ -54,8 +57,11 @@ UartTaskData uart6TaskData = { 0 }; // Board 3
 UartTaskData uart2TaskData = { 0 }; // Board 4
 UartTaskData uart8TaskData = { 0 }; // Debug
 
-//UartTaskData* uartTasks[] = { &uart8TaskData, &uart3TaskData, &uart6TaskData, &uart2TaskData, NULL, };
+#ifdef USE_UART8_INSTEAD_UART1
+UartTaskData* uartTasks[] = { &uart8TaskData, &uart3TaskData, &uart6TaskData, &uart2TaskData, NULL, };
+#else
 UartTaskData* uartTasks[] = { &uart1TaskData, &uart3TaskData, &uart6TaskData, &uart2TaskData, NULL, };
+#endif
 
 uint8_t outputDataBuffer[OUTPUT_DATA_BUFF_SIZE];
 uint16_t outputDataBufferPos = 0;
@@ -377,6 +383,11 @@ void UartTxTask (void* argument) {
                 WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[2], sizeof (float));
                 break;
             case spResetSystem: break;
+            case spSetPositonX:
+            case spSetPositonY:
+                WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &data.values.flaotValues.value[0], sizeof (float));
+                break;
+                break;
             default: continue; break;
             }
 
@@ -433,6 +444,10 @@ void UartTxTask (void* argument) {
                         ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitYSwitchDown);
                         ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->limitYSwitchCenter);
                         ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->powerSupplyFailMask);
+                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->currentXPosition);
+                        ReadFloatFromBuffer (frameData.dataBuffer, &inputDataBufferPos, &sensors->currentYPosition);
+                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionXWeak);
+                        ReadByteFromBufer (frameData.dataBuffer, &inputDataBufferPos, &sensors->positionYWeak);
                         osMutexRelease (sensorsInfoMutex);
                         break;
                     default: break;

OZE_Sensor/Core/Inc/measurements.h

@@ -38,6 +38,10 @@ struct _SesnorsInfo {
     uint8_t limitYSwitchDown;
     uint8_t limitYSwitchCenter;
     uint8_t powerSupplyFailMask;
+    float currentXPosition;
+    float currentYPosition;
+    uint8_t positionXWeak;
+    uint8_t positionYWeak;
 };
 
 typedef struct _RESMeasurements RESMeasurements;

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

@@ -18,6 +18,19 @@
 #define ENCODER_X_IMP_PER_TURN 20
 #define ENCODER_Y_IMP_PER_TURN 20
 
+#define MAX_X_AXE_ANGLE 360	// Max angle value for 100% range
+#define MAX_Y_AXE_ANGLE 360 // Max angle value for 100% range
+#define MOTOR_START_STOP_PWM_VALUE 60
+#define MOTOR_HIGH_SPEED_PWM_VALUE 100
+
+#define ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT 5
+#define TIME_MS_FOR_MOTOR_SPEED_LIMIT 1000
+#define NO_MOVE_TIMEOUT_MS 10000
+
+#define WATCHDOG_ENABLED
 //#define USER_MOCKS
+//#define SERIAL_PROTOCOL_DBG
+//#define USE_UART8_INSTEAD_UART1
+//#define DBG_POSITION
 
 #endif /* INC_NODE_RED_CONFIG_H_ */

OZE_Sensor/Core/Inc/peripherial.h

@@ -54,8 +54,8 @@ void DbgLEDToggle(uint8_t ledNumber);
 void EnableCurrentSensors(void);
 void DisableCurrentSensors(void);
 void SelectCurrentSensorGain(CurrentSensor sensor, CurrentSensorGain gain);
-void motorAction(TIM_HandleTypeDef *tim, TIM_OC_InitTypeDef *timerConf, uint32_t channel1, uint32_t channel2, MotorDriverState setState, uint32_t pulse);
-uint8_t motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, uint8_t channel1, uint8_t channel2, osTimerId_t motorTimerHandle,
+void MotorAction(TIM_HandleTypeDef *tim, TIM_OC_InitTypeDef *timerConf, uint32_t channel1, uint32_t channel2, MotorDriverState setState, uint32_t pulse);
+uint8_t MotorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, uint8_t channel1, uint8_t channel2, osTimerId_t motorTimerHandle,
 		int32_t motorPWMPulse, int32_t motorTimerPeriod, uint8_t switchLimiterUpStat, uint8_t switchLimiterDownStat) ;
 
 #endif /* INC_PERIPHERIAL_H_ */

OZE_Sensor/Core/Inc/position_task.h

@@ -0,0 +1,59 @@
+/*
+ * position_task.h
+ *
+ *  Created on: Nov 6, 2024
+ *      Author: jakubski
+ */
+
+#ifndef INC_POSITION_TASK_H_
+#define INC_POSITION_TASK_H_
+
+#include "cmsis_os.h"
+#include "main.h"
+
+enum _MovementPhases
+{
+	idlePhase,
+	startPhase,
+	speedUpPhase,
+	movePhase,
+	slowDownPhase,
+	stopPhase
+};
+
+typedef enum _MovementPhases MovementPhases;
+
+struct _PositionControlTaskInitArg
+{
+	TIM_HandleTypeDef* htim;
+	TIM_OC_InitTypeDef* motorTimerConfigOC;
+	uint8_t channel1;
+	uint8_t channel2;
+	osTimerId_t motorTimerHandle;
+	osMessageQueueId_t positionSettingQueue;
+	uint8_t* switchLimiterUpStat;
+	uint8_t* switchLimiterDownStat;
+	uint8_t* switchLimiterCenterStat;
+	float* currentPosition;
+	float* positionSetting;
+	uint8_t* motorStatus;
+	float* motorPeakCurrent;
+	char axe;
+};
+
+typedef struct _PositionControlTaskInitArg PositionControlTaskInitArg;
+
+struct _PositionControlTaskData
+{
+	float positionSettingValue;
+};
+
+typedef struct _PositionControlTaskData PositionControlTaskData;
+
+extern PositionControlTaskInitArg positionXControlTaskInitArg;
+extern PositionControlTaskInitArg positionYControlTaskInitArg;
+
+void PositionControlTaskInit(void);
+void PositionControlTask (void* argument);
+
+#endif /* INC_POSITION_TASK_H_ */

OZE_Sensor/Core/Inc/serial_protocol.h

@@ -45,6 +45,8 @@ enum _SerialProtocolCommands {
     spSetCurrentMeasGains,
     spSetCurrentMeasOffsets,
 	spResetSystem,
+	spSetPositonX,
+	spSetPositonY,
     spUnknown
 };
 

OZE_Sensor/Core/Inc/uart_tasks.h

@@ -68,7 +68,7 @@ struct _UartTaskData {
 
 typedef struct _UartTaskData UartTaskData;
 
-void Uart8TasksInit (void);
+//void Uart8TasksInit (void);
 void UartTasksInit(void);
 void UartTaskCreate (UartTaskData* uartTaskData);
 

OZE_Sensor/Core/Src/main.c

@@ -31,6 +31,7 @@
 #include "meas_tasks.h"
 #include "peripherial.h"
 #include "measurements.h"
+#include "position_task.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -163,8 +164,8 @@ void motorYTimerCallback(void *argument);
 int __io_putchar(int ch)
 {
 #if UART_TASK_LOGS
-  HAL_UART_Transmit(&huart8, (uint8_t *)&ch, 1, 0xFFFF); // Use UART8 as debug interface
-//  ITM_SendChar(ch);	// Use SWV as debug interface
+//  HAL_UART_Transmit(&huart8, (uint8_t *)&ch, 1, 0xFFFF); // Use UART8 as debug interface
+  ITM_SendChar(ch);	// Use SWV as debug interface
 #endif
   return ch;
 }
@@ -236,9 +237,13 @@ int main(void)
   MX_COMP1_Init();
   MX_TIM4_Init();
   MX_TIM8_Init();
+#ifdef WATCHDOG_ENABLED
   MX_IWDG1_Init();
+#endif
   /* USER CODE BEGIN 2 */
-//  HAL_IWDG_Refresh(&hiwdg1);
+#ifdef WATCHDOG_ENABLED
+  HAL_IWDG_Refresh(&hiwdg1);
+#endif
   /* USER CODE END 2 */
 
   /* Init scheduler */
@@ -279,13 +284,16 @@ int main(void)
 
   /* USER CODE BEGIN RTOS_THREADS */
   /* add threads, ... */
+#ifdef WATCHDOG_ENABLED
   HAL_IWDG_Refresh(&hiwdg1);
+#endif
   UartTasksInit();
 #ifdef USER_MOCKS
   MockMeasurmetsTaskInit();
 #else
   MeasTasksInit();
 #endif
+  PositionControlTaskInit();
   /* USER CODE END RTOS_THREADS */
 
   /* USER CODE BEGIN RTOS_EVENTS */
@@ -1502,13 +1510,17 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
 void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
+#ifdef WATCHDOG_ENABLED
   HAL_IWDG_Refresh(&hiwdg1);
+#endif
   SelectCurrentSensorGain(CurrentSensorL1, csGain3);
   SelectCurrentSensorGain(CurrentSensorL2, csGain3);
   SelectCurrentSensorGain(CurrentSensorL3, csGain3);
   EnableCurrentSensors();
   osDelay(pdMS_TO_TICKS(100));
+#ifdef WATCHDOG_ENABLED
   HAL_IWDG_Refresh(&hiwdg1);
+#endif
   if(HAL_TIM_Base_Start(&htim8) != HAL_OK)
   {
 	  Error_Handler();
@@ -1550,12 +1562,16 @@ void StartDefaultTask(void *argument)
 	  Error_Handler();
   }
   HAL_COMP_Start(&hcomp1);
+#ifdef WATCHDOG_ENABLED
   HAL_IWDG_Refresh(&hiwdg1);
+#endif
   /* Infinite loop */
   for(;;)
   {
 	 osDelay(pdMS_TO_TICKS(100));
+#ifdef WATCHDOG_ENABLED
 	 HAL_IWDG_Refresh(&hiwdg1);
+#endif
 	 if(HAL_TIM_GetChannelState(&htim3, TIM_CHANNEL_1) == HAL_TIM_CHANNEL_STATE_READY &&
 			 HAL_TIM_GetChannelState(&htim3, TIM_CHANNEL_2) == HAL_TIM_CHANNEL_STATE_READY)
 	 {
@@ -1598,7 +1614,7 @@ void fanTimerCallback(void *argument)
 void motorXTimerCallback(void *argument)
 {
   /* USER CODE BEGIN motorXTimerCallback */
-	motorAction(&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, HiZ, 0);
+	MotorAction(&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, HiZ, 0);
 	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
 	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_2);
   /* USER CODE END motorXTimerCallback */
@@ -1608,7 +1624,7 @@ void motorXTimerCallback(void *argument)
 void motorYTimerCallback(void *argument)
 {
   /* USER CODE BEGIN motorYTimerCallback */
-	motorAction(&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, HiZ, 0);
+	MotorAction(&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, HiZ, 0);
 	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_3);
 	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_4);
   /* USER CODE END motorYTimerCallback */

OZE_Sensor/Core/Src/meas_tasks.c

@@ -52,6 +52,7 @@ extern TIM_HandleTypeDef htim3;
 extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
 extern osTimerId_t motorXTimerHandle;
 extern osTimerId_t motorYTimerHandle;
+//extern osMutexId_t positionSettingMutex;
 
 
 void MeasTasksInit (void) {
@@ -254,24 +255,71 @@ void LimiterSwitchTask (void* arg) {
         limiterSwitchData.pinState = HAL_GPIO_ReadPin (GPIOD, limiterSwitchData.gpioPin);
         osMessageQueuePut (limiterSwitchDataQueue, &limiterSwitchData, 0, 0);
         limiterSwitchData.gpioPin = limiterSwitchData.gpioPin << 1;
+        if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+        	sensorsInfo.positionXWeak = 1;
+        	sensorsInfo.positionYWeak = 1;
+        	osMutexRelease (sensorsInfoMutex);
+        }
     }
     while (pdTRUE) {
         osMessageQueueGet (limiterSwitchDataQueue, &limiterSwitchData, 0, osWaitForever);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
             switch (limiterSwitchData.gpioPin) {
-            case GPIO_PIN_8: sensorsInfo.limitYSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0; break;
-            case GPIO_PIN_9: sensorsInfo.limitYSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0; break;
-            case GPIO_PIN_10: sensorsInfo.limitXSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0; break;
-            case GPIO_PIN_11: sensorsInfo.limitYSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0; break;
-            case GPIO_PIN_12: sensorsInfo.limitXSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0; break;
-            case GPIO_PIN_13: sensorsInfo.limitXSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0; break;
+            case GPIO_PIN_8:
+            	sensorsInfo.limitYSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
+            	if (sensorsInfo.limitYSwitchCenter == 1)
+            	{
+            		sensorsInfo.currentYPosition = 50;
+            		sensorsInfo.positionYWeak = 0;
+            	}
+            	break;
+            case GPIO_PIN_9:
+            	sensorsInfo.limitYSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
+            	if (sensorsInfo.limitYSwitchDown == 1)
+            	{
+            		sensorsInfo.currentYPosition = 0;
+            		sensorsInfo.positionYWeak = 0;
+            	}
+            	break;
+            case GPIO_PIN_10:
+            	sensorsInfo.limitXSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
+            	if (sensorsInfo.limitXSwitchCenter == 1)
+				{
+					sensorsInfo.currentXPosition = 50;
+					sensorsInfo.positionXWeak = 0;
+				}
+            	break;
+            case GPIO_PIN_11:
+            	sensorsInfo.limitYSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
+            	if (sensorsInfo.limitYSwitchUp == 1)
+				{
+					sensorsInfo.currentYPosition = 100;
+					sensorsInfo.positionYWeak = 0;
+				}
+            	break;
+            case GPIO_PIN_12:
+            	sensorsInfo.limitXSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
+            	if (sensorsInfo.limitXSwitchUp == 1)
+				{
+					sensorsInfo.currentXPosition = 100;
+					sensorsInfo.positionXWeak = 0;
+				}
+            	break;
+            case GPIO_PIN_13:
+            	sensorsInfo.limitXSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
+            	if (sensorsInfo.limitXSwitchDown == 1)
+				{
+					sensorsInfo.currentXPosition = 0;
+					sensorsInfo.positionXWeak = 0;
+				}
+            	break;
             default: break;
             }
             if ((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) {
-                sensorsInfo.motorXStatus = motorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, 0, 0, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
+                sensorsInfo.motorXStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, 0, 0, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
             }
             if ((sensorsInfo.limitYSwitchDown == 1) || (sensorsInfo.limitYSwitchUp == 1)) {
-                sensorsInfo.motorYStatus = motorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, 0, 0, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
+                sensorsInfo.motorYStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, 0, 0, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
             }
             osMutexRelease (sensorsInfoMutex);
         }
@@ -290,6 +338,9 @@ void EncoderTask (void* arg) {
                 } else {
                     sensorsInfo.pvEncoderX -= 360.0 / ENCODER_X_IMP_PER_TURN;
                 }
+                float currentPercentPos = 100 * sensorsInfo.pvEncoderX / MAX_X_AXE_ANGLE;
+                currentPercentPos = currentPercentPos < 0 ? 0 : currentPercentPos;
+                sensorsInfo.currentXPosition = currentPercentPos > 100 ? 100 : currentPercentPos;
                 DbgLEDToggle(DBG_LED2);
             } else {
                 if (encoderData.direction == encoderCW) {
@@ -297,6 +348,9 @@ void EncoderTask (void* arg) {
                 } else {
                     sensorsInfo.pvEncoderY -= 360.0 / ENCODER_Y_IMP_PER_TURN;
                 }
+                float currentPercentPos = 100 * sensorsInfo.pvEncoderY / MAX_X_AXE_ANGLE;
+				currentPercentPos = currentPercentPos < 0 ? 0 : currentPercentPos;
+				sensorsInfo.currentXPosition = currentPercentPos > 100 ? 100 : currentPercentPos;
                 DbgLEDToggle(DBG_LED3);
             }
             osMutexRelease (sensorsInfoMutex);

OZE_Sensor/Core/Src/peripherial.c

@@ -48,9 +48,9 @@ void SelectCurrentSensorGain (CurrentSensor sensor, CurrentSensorGain gain) {
 }
 
 uint8_t
-motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, uint8_t channel1, uint8_t channel2, osTimerId_t motorTimerHandle, int32_t motorPWMPulse, int32_t motorTimerPeriod, uint8_t switchLimiterUpStat, uint8_t switchLimiterDownStat) {
+MotorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, uint8_t channel1, uint8_t channel2, osTimerId_t motorTimerHandle, int32_t motorPWMPulse, int32_t motorTimerPeriod, uint8_t switchLimiterUpStat, uint8_t switchLimiterDownStat) {
     uint32_t motorStatus            = 0;
-    MotorDriverState setMotorYState = HiZ;
+    MotorDriverState setMotorState = HiZ;
 
     HAL_TIM_PWM_Stop (htim, channel1);
     HAL_TIM_PWM_Stop (htim, channel2);
@@ -58,8 +58,8 @@ motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, u
         if (motorPWMPulse > 0) {
             // Forward
             if (switchLimiterUpStat == 0) {
-                setMotorYState = Forward;
-                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
+                setMotorState = Forward;
+                MotorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorState, abs (motorPWMPulse) * 10);
                 HAL_TIM_PWM_Start (htim, channel1);
                 motorStatus = 1;
             } else {
@@ -69,8 +69,8 @@ motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, u
         } else if (motorPWMPulse < 0) {
             // Reverse
             if (switchLimiterDownStat == 0) {
-                setMotorYState = Reverse;
-                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
+                setMotorState = Reverse;
+                MotorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorState, abs (motorPWMPulse) * 10);
                 HAL_TIM_PWM_Start (htim, channel2);
                 motorStatus = 1;
             } else {
@@ -79,15 +79,15 @@ motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, u
             HAL_TIM_PWM_Stop (htim, channel1);
         } else {
             // Brake
-            setMotorYState = Brake;
-            motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
+            setMotorState = Brake;
+            MotorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorState, abs (motorPWMPulse) * 10);
             HAL_TIM_PWM_Start (htim, channel1);
             HAL_TIM_PWM_Start (htim, channel2);
             motorStatus = 0;
         }
         osTimerStart (motorTimerHandle, motorTimerPeriod * 1000);
     } else if ((motorTimerPeriod == 0) && (motorPWMPulse == 0)) {
-        motorAction (htim, motorTimerConfigOC, channel1, channel2, HiZ, abs (motorPWMPulse) * 10);
+        MotorAction (htim, motorTimerConfigOC, channel1, channel2, HiZ, abs (motorPWMPulse) * 10);
         HAL_TIM_PWM_Stop (htim, channel1);
         HAL_TIM_PWM_Stop (htim, channel2);
         osTimerStop (motorTimerHandle);
@@ -96,8 +96,8 @@ motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, u
         if (motorPWMPulse > 0) {
             // Forward
             if (switchLimiterUpStat == 0) {
-                setMotorYState = Forward;
-                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
+                setMotorState = Forward;
+                MotorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorState, abs (motorPWMPulse) * 10);
                 HAL_TIM_PWM_Start (htim, channel1);
                 motorStatus = 1;
             } else {
@@ -107,8 +107,8 @@ motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, u
         } else {
             // Reverse
             if (switchLimiterDownStat == 0) {
-                setMotorYState = Reverse;
-                motorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorYState, abs (motorPWMPulse) * 10);
+                setMotorState = Reverse;
+                MotorAction (htim, motorTimerConfigOC, channel1, channel2, setMotorState, abs (motorPWMPulse) * 10);
                 HAL_TIM_PWM_Start (htim, channel2);
                 motorStatus = 1;
             } else {
@@ -120,7 +120,7 @@ motorControl (TIM_HandleTypeDef* htim, TIM_OC_InitTypeDef* motorTimerConfigOC, u
     return motorStatus;
 }
 
-void motorAction (TIM_HandleTypeDef* tim, TIM_OC_InitTypeDef* timerConf, uint32_t channel1, uint32_t channel2, MotorDriverState setState, uint32_t pulse) {
+void MotorAction (TIM_HandleTypeDef* tim, TIM_OC_InitTypeDef* timerConf, uint32_t channel1, uint32_t channel2, MotorDriverState setState, uint32_t pulse) {
     timerConf->Pulse = pulse;
     switch (setState) {
     case Forward:

OZE_Sensor/Core/Src/position_task.c

@@ -0,0 +1,216 @@
+/*
+ * position_task.c
+ *
+ *  Created on: Nov 6, 2024
+ *      Author: jakubski
+ */
+
+#include "position_task.h"
+#include "meas_tasks.h"
+#include "measurements.h"
+#include "node-red-config.h"
+#include "peripherial.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+float positionXSetting = 0.0;
+float positionYSetting = 0.0;
+
+osMutexId_t positionSettingMutex;
+
+osThreadId_t positionXControlTaskHandle = NULL;
+osThreadId_t positionYControlTaskHandle = NULL;
+
+PositionControlTaskInitArg positionXControlTaskInitArg = { 0 };
+PositionControlTaskInitArg positionYControlTaskInitArg = { 0 };
+
+extern osTimerId_t motorXTimerHandle;
+extern osTimerId_t motorYTimerHandle;
+extern TIM_HandleTypeDef htim3;
+extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
+
+void PositionControlTaskInit (void) {
+    positionSettingMutex                           = osMutexNew (NULL);
+    osThreadAttr_t osThreadAttrPositionControlTask = { 0 };
+    osThreadAttrPositionControlTask.stack_size     = configMINIMAL_STACK_SIZE * 2;
+    osThreadAttrPositionControlTask.priority       = (osPriority_t)osPriorityNormal;
+
+    positionXControlTaskInitArg.channel1                = TIM_CHANNEL_1;
+    positionXControlTaskInitArg.channel2                = TIM_CHANNEL_2;
+    positionXControlTaskInitArg.htim                    = &htim3;
+    positionXControlTaskInitArg.motorTimerConfigOC      = &motorXYTimerConfigOC;
+    positionXControlTaskInitArg.motorTimerHandle        = motorXTimerHandle;
+    positionXControlTaskInitArg.positionSettingQueue    = osMessageQueueNew (16, sizeof (PositionControlTaskData), NULL);
+    positionXControlTaskInitArg.switchLimiterCenterStat = &(sensorsInfo.limitXSwitchCenter);
+    positionXControlTaskInitArg.switchLimiterUpStat     = &(sensorsInfo.limitXSwitchUp);
+    positionXControlTaskInitArg.switchLimiterDownStat   = &(sensorsInfo.limitXSwitchDown);
+    positionXControlTaskInitArg.currentPosition         = &(sensorsInfo.currentXPosition);
+    positionXControlTaskInitArg.motorStatus             = &(sensorsInfo.motorXStatus);
+    positionXControlTaskInitArg.motorPeakCurrent        = &(sensorsInfo.motorXPeakCurrent);
+    positionXControlTaskInitArg.positionSetting         = &positionXSetting;
+    positionXControlTaskInitArg.axe                     = 'X';
+
+    positionYControlTaskInitArg.channel1                = TIM_CHANNEL_3;
+    positionYControlTaskInitArg.channel2                = TIM_CHANNEL_4;
+    positionYControlTaskInitArg.htim                    = &htim3;
+    positionYControlTaskInitArg.motorTimerConfigOC      = &motorXYTimerConfigOC;
+    positionYControlTaskInitArg.motorTimerHandle        = motorYTimerHandle;
+    positionYControlTaskInitArg.positionSettingQueue    = osMessageQueueNew (16, sizeof (PositionControlTaskData), NULL);
+    positionYControlTaskInitArg.switchLimiterCenterStat = &(sensorsInfo.limitYSwitchCenter);
+    positionYControlTaskInitArg.switchLimiterUpStat     = &(sensorsInfo.limitYSwitchUp);
+    positionYControlTaskInitArg.switchLimiterDownStat   = &(sensorsInfo.limitYSwitchDown);
+    positionYControlTaskInitArg.currentPosition         = &(sensorsInfo.currentYPosition);
+    positionYControlTaskInitArg.motorStatus             = &(sensorsInfo.motorYStatus);
+    positionYControlTaskInitArg.motorPeakCurrent        = &(sensorsInfo.motorYPeakCurrent);
+    positionXControlTaskInitArg.positionSetting         = &positionYSetting;
+    positionYControlTaskInitArg.axe                     = 'Y';
+
+    positionXControlTaskHandle = osThreadNew (PositionControlTask, &positionXControlTaskInitArg, &osThreadAttrPositionControlTask);
+    positionYControlTaskHandle = osThreadNew (PositionControlTask, &positionYControlTaskInitArg, &osThreadAttrPositionControlTask);
+}
+
+void PositionControlTask (void* argument) {
+    const int32_t PositionControlTaskTimeOut   = 100;
+    PositionControlTaskInitArg* posCtrlTaskArg = (PositionControlTaskInitArg*)argument;
+    PositionControlTaskData posCtrlData        = { 0 };
+    uint32_t motorStatus                       = 0;
+    osStatus_t queueSatus;
+    int32_t pwmValue              = MOTOR_START_STOP_PWM_VALUE;
+    int32_t sign                  = 0;
+    MovementPhases movementPhase  = idlePhase;
+    float startPosition           = 0;
+    float prevPosition            = 0;
+    int32_t timeLeftMS            = 0;
+    int32_t moveCmdTimeoutCounter = 0;
+    while (pdTRUE) {
+        queueSatus = osMessageQueueGet (posCtrlTaskArg->positionSettingQueue, &posCtrlData, 0, pdMS_TO_TICKS (PositionControlTaskTimeOut));
+        if (queueSatus == osOK) {
+            if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+                float posDiff = posCtrlData.positionSettingValue - *posCtrlTaskArg->currentPosition;
+                if (posDiff != 0) {
+                    sign                  = posDiff > 0 ? 1 : -1;
+                    startPosition         = *posCtrlTaskArg->currentPosition;
+                    movementPhase         = startPhase;
+                    moveCmdTimeoutCounter = 0;
+                    timeLeftMS            = 0;
+#ifdef DBG_POSITION
+                    printf ("Axe %c start phase\n", posCtrlTaskArg->axe);
+#endif
+                }
+                osMutexRelease (sensorsInfoMutex);
+                if (osMutexAcquire (positionSettingMutex, osWaitForever) == osOK) {
+                    *positionXControlTaskInitArg.positionSetting = posCtrlData.positionSettingValue;
+                    osMutexRelease (positionSettingMutex);
+                }
+            }
+        } else if (queueSatus == osErrorTimeout) {
+            if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
+                if ((*posCtrlTaskArg->motorStatus != 0) || (movementPhase == startPhase)) {
+                    if (((*posCtrlTaskArg->switchLimiterDownStat == 1) && (*posCtrlTaskArg->switchLimiterUpStat == 1)) ||
+                    ((*posCtrlTaskArg->switchLimiterUpStat == 1) && (*posCtrlTaskArg->switchLimiterCenterStat == 1))) {
+                        movementPhase = idlePhase;
+                        motorStatus   = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle, 0,
+                          0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
+                        *posCtrlTaskArg->motorStatus = motorStatus;
+#ifdef DBG_POSITION
+                        printf ("Axe %c limiters wrong state - idle phase\n", posCtrlTaskArg->axe);
+#endif
+                    }
+
+
+                    timeLeftMS += PositionControlTaskTimeOut;
+                    if (prevPosition == *posCtrlTaskArg->currentPosition) {
+                        moveCmdTimeoutCounter += PositionControlTaskTimeOut;
+                    } else {
+                        moveCmdTimeoutCounter = 0;
+                    }
+                    prevPosition = *posCtrlTaskArg->currentPosition;
+                    if (moveCmdTimeoutCounter > NO_MOVE_TIMEOUT_MS) {
+                        movementPhase = idlePhase;
+                        motorStatus   = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle, 0,
+                          0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
+                        *posCtrlTaskArg->motorStatus = motorStatus;
+#ifdef DBG_POSITION
+                        printf ("Axe %c no movement idle phase\n", posCtrlTaskArg->axe);
+#endif
+                    }
+                    switch (movementPhase) {
+                    case startPhase:
+                        motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
+                        sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
+                        *posCtrlTaskArg->motorStatus = motorStatus;
+                        if (motorStatus == 1) {
+                            *posCtrlTaskArg->motorPeakCurrent = 0.0;
+#ifdef DBG_POSITION
+                            printf ("Axe %c speed up phase\n", posCtrlTaskArg->axe);
+#endif
+                            movementPhase         = speedUpPhase;
+                            timeLeftMS            = 0;
+                            moveCmdTimeoutCounter = 0;
+                        } else {
+                            movementPhase = idlePhase;
+#ifdef DBG_POSITION
+                            printf ("Axe %c idle phase\n", posCtrlTaskArg->axe);
+#endif
+                        }
+
+                        break;
+                    case speedUpPhase:
+                        if ((abs (*posCtrlTaskArg->currentPosition - startPosition) >= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
+                            pwmValue    = MOTOR_HIGH_SPEED_PWM_VALUE;
+                            motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
+                            sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
+                            *posCtrlTaskArg->motorStatus = motorStatus;
+                            movementPhase                = movePhase;
+#ifdef DBG_POSITION
+                            printf ("Axe %c move phase\n", posCtrlTaskArg->axe);
+#endif
+                        }
+                        break;
+                    case movePhase:
+                        if (abs (*posCtrlTaskArg->currentPosition - *posCtrlTaskArg->positionSetting) <= ANGLE_RANGE_FOR_MOTOR_SPEED_LIMIT) {
+                            movementPhase = slowDownPhase;
+#ifdef DBG_POSITION
+                            printf ("Axe %c slow down phase\n", posCtrlTaskArg->axe);
+#endif
+                        }
+                        break;
+                    case slowDownPhase:
+                        pwmValue    = MOTOR_START_STOP_PWM_VALUE;
+                        motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
+                        sign * pwmValue, -1, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
+                        *posCtrlTaskArg->motorStatus = motorStatus;
+                        movementPhase                = stopPhase;
+                        timeLeftMS                   = 0;
+#ifdef DBG_POSITION
+                        printf ("Axe %c stop phase\n", posCtrlTaskArg->axe);
+#endif
+                        break;
+                    case stopPhase:
+                        if ((*posCtrlTaskArg->currentPosition == *posCtrlTaskArg->positionSetting) || (timeLeftMS >= TIME_MS_FOR_MOTOR_SPEED_LIMIT)) {
+                            motorStatus = MotorControl (posCtrlTaskArg->htim, posCtrlTaskArg->motorTimerConfigOC, posCtrlTaskArg->channel1, posCtrlTaskArg->channel2, posCtrlTaskArg->motorTimerHandle,
+                            0, 0, *posCtrlTaskArg->switchLimiterUpStat, *posCtrlTaskArg->switchLimiterDownStat);
+                            *posCtrlTaskArg->motorStatus = motorStatus;
+                            movementPhase                = idlePhase;
+#ifdef DBG_POSITION
+                            printf ("Axe %c idle phase\n", posCtrlTaskArg->axe);
+#endif
+                        }
+                        break;
+                    default: break;
+                    }
+                } else {
+                    if ((*posCtrlTaskArg->motorStatus == 0) && (movementPhase != idlePhase)) {
+                        movementPhase = idlePhase;
+#ifdef DBG_POSITION
+                        printf ("Axe %c idle phase\n", posCtrlTaskArg->axe);
+#endif
+                    }
+                }
+                osMutexRelease (sensorsInfoMutex);
+            }
+        }
+    }
+}

OZE_Sensor/Core/Src/uart_tasks.c

@@ -11,6 +11,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 
 #include "adc_buffers.h"
 #include "interprocess_data.h"
@@ -19,6 +20,7 @@
 #include "mock_tasks.h"
 #include "peripherial.h"
 #include "uart_tasks.h"
+#include "position_task.h"
 
 enum SerialReceiverStates { srWaitForHeader, srCheckCrc, srRecieveData, srExecuteCmd, srFail, srFinish, srLast };
 
@@ -46,21 +48,12 @@ uint8_t uart8RxBuffer[UART8_RX_BUFF_SIZE]        = { 0 };
 uint8_t uart8TxBuffer[UART8_TX_BUFF_SIZE]        = { 0 };
 uint8_t uart8TaskFrameData[INPUT_DATA_BUFF_SIZE] = { 0 };
 
-uint8_t boardToUartNumberMap[SLAVES_COUNT] = { /*1*/ 8, 3, 6, 2 };
-
 UartTaskData uart1TaskData = { 0 }; // Board 1
-UartTaskData uart3TaskData = { 0 }; // Board 2
-UartTaskData uart6TaskData = { 0 }; // Board 3
-UartTaskData uart2TaskData = { 0 }; // Board 4
 UartTaskData uart8TaskData = { 0 }; // Debug
 
-UartTaskData* uartTasks[] = { &uart8TaskData, NULL };
-
 uint8_t outputDataBuffer[OUTPUT_DATA_BUFF_SIZE];
 uint16_t outputDataBufferPos = 0;
 
-uint32_t slaveLastSeen[SLAVES_COUNT] = { 0 };
-
 extern RNG_HandleTypeDef hrng;
 
 void UartTasksInit (void) {
@@ -75,7 +68,22 @@ void UartTasksInit (void) {
     uart1TaskData.processDataCb          = Uart1ReceivedDataProcessCallback;
     uart1TaskData.processRxDataMsgBuffer = NULL;
 
+    uart8TaskData.uartRxBuffer           = uart8RxBuffer;
+    uart8TaskData.uartRxBufferLen        = UART8_RX_BUFF_SIZE;
+    uart8TaskData.uartTxBuffer           = uart8TxBuffer;
+    uart8TaskData.uartRxBufferLen        = UART8_TX_BUFF_SIZE;
+    uart8TaskData.frameData              = uart8TaskFrameData;
+    uart8TaskData.frameDataLen           = UART8_RX_BUFF_SIZE;
+    uart8TaskData.huart                  = &huart8;
+    uart8TaskData.uartNumber             = 8;
+    uart8TaskData.processDataCb          = Uart8ReceivedDataProcessCallback;
+    uart8TaskData.processRxDataMsgBuffer = NULL;
+
+#ifdef USE_UART8_INSTEAD_UART1
+    UartTaskCreate (&uart8TaskData);
+#else
     UartTaskCreate (&uart1TaskData);
+#endif
 }
 
 void UartTaskCreate (UartTaskData* uartTaskData) {
@@ -85,26 +93,6 @@ void UartTaskCreate (UartTaskData* uartTaskData) {
     uartTaskData->uartRecieveTaskHandle = osThreadNew (UartRxTask, uartTaskData, &osThreadAttrRxUart);
 }
 
-void Uart8TasksInit (void) {
-    osThreadAttr_t osThreadAttrRxUart = { 0 };
-
-    uart8TaskData.processDataCb = Uart8ReceivedDataProcessCallback;
-
-    osThreadAttrRxUart.name       = "os_thread_uart8_rx";
-    osThreadAttrRxUart.stack_size = configMINIMAL_STACK_SIZE * 2;
-    osThreadAttrRxUart.priority   = (osPriority_t)osPriorityHigh;
-
-    uart8TaskData.uartRxBuffer          = uart8RxBuffer;
-    uart8TaskData.uartRxBufferLen       = UART8_RX_BUFF_SIZE;
-    uart8TaskData.uartTxBuffer          = uart8TxBuffer;
-    uart8TaskData.uartRxBufferLen       = UART8_TX_BUFF_SIZE;
-    uart8TaskData.frameData             = uart8TaskFrameData;
-    uart8TaskData.frameDataLen          = UART8_RX_BUFF_SIZE;
-    uart8TaskData.huart                 = &huart8;
-    uart8TaskData.uartNumber            = 8;
-    uart8TaskData.uartRecieveTaskHandle = osThreadNew (UartRxTask, &uart8TaskData, &osThreadAttrRxUart);
-}
-
 void HAL_UART_RxCpltCallback (UART_HandleTypeDef* huart) {
 }
 
@@ -161,7 +149,9 @@ void UartRxTask (void* argument) {
         } else {
             if (frameTimeout == pdFALSE) {
                 proceed = pdTRUE;
+#ifdef SERIAL_PROTOCOL_DBG
                 printf ("Uart%d: RX bytes received: %ld\n", uartTaskData->uartNumber, bytesRec);
+#endif
             } else {
                 if (uartTaskData->huart->RxState == HAL_UART_STATE_READY) {
                     HAL_UARTEx_ReceiveToIdle_IT (uartTaskData->huart, uartTaskData->uartRxBuffer, uartTaskData->uartRxBufferLen);
@@ -216,7 +206,9 @@ void UartRxTask (void* argument) {
                 osMutexRelease (uartTaskData->rxDataBufferMutex);
                 crcPass = frameCrc == crc;
                 if (crcPass) {
+#ifdef SERIAL_PROTOCOL_DBG
                     printf ("Uart%d: Frame CRC PASS\n", uartTaskData->uartNumber);
+#endif
                     receverState = srExecuteCmd;
                 } else {
                     receverState = srFail;
@@ -244,17 +236,23 @@ void UartRxTask (void* argument) {
                 dataToSend = 0;
                 if ((frameTimeout == pdTRUE) && (frameBytesCount > 2)) {
                     dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spTimeout, NULL, 0);
+#ifdef SERIAL_PROTOCOL_DBG
                     printf ("Uart%d: RX data receiver timeout!\n", uartTaskData->uartNumber);
+#endif
                 } else if (!crcPass) {
                     dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spCrcFail, NULL, 0);
+#ifdef SERIAL_PROTOCOL_DBG
                     printf ("Uart%d: Frame CRC FAIL\n", uartTaskData->uartNumber);
+#endif
                 } else {
                     dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData.frameHeader.frameId, spFrameData.frameHeader.frameCommand, spInternalError, NULL, 0);
                 }
                 if (dataToSend > 0) {
                     HAL_UART_Transmit_IT (uartTaskData->huart, uartTaskData->uartTxBuffer, dataToSend);
                 }
+#ifdef SERIAL_PROTOCOL_DBG
                 printf ("Uart%d: TX bytes sent: %d\n", dataToSend, uartTaskData->uartNumber);
+#endif
                 receverState = srFinish;
                 break;
             case srFinish:
@@ -329,6 +327,10 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
             uint8_t comparatorOutput        = HAL_COMP_GetOutputLevel (&hcomp1) == COMP_OUTPUT_LEVEL_HIGH ? 1 : 0;
             sensorsInfo.powerSupplyFailMask = ~((comparatorOutput << 1) | HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_3)) & 0x01;
             WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.powerSupplyFailMask, sizeof (uint8_t));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.currentXPosition, sizeof (float));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.currentYPosition, sizeof (float));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionXWeak, sizeof (uint8_t));
+            WriteDataToBuffer (outputDataBuffer, &outputDataBufferPos, &sensorsInfo.positionYWeak, sizeof (uint8_t));
             osMutexRelease (sensorsInfoMutex);
             respStatus = spOK;
         } else {
@@ -365,7 +367,7 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorXTimerPeriod);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
             motorXStatus =
-            motorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, motorXPWMPulse, motorXTimerPeriod, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
+            MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, motorXPWMPulse, motorXTimerPeriod, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
             sensorsInfo.motorXStatus = motorXStatus;
             if (motorXStatus == 1) {
                 sensorsInfo.motorXPeakCurrent = 0.0;
@@ -384,7 +386,7 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
         ReadWordFromBufer (spFrameData->dataBuffer, &inputDataBufferPos, (uint32_t*)&motorYTimerPeriod);
         if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
             motorYStatus =
-            motorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, motorYPWMPulse, motorYTimerPeriod, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
+            MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, motorYPWMPulse, motorYTimerPeriod, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
             sensorsInfo.motorYStatus = motorYStatus;
             if (motorYStatus == 1) {
                 sensorsInfo.motorYPeakCurrent = 0.0;
@@ -509,13 +511,55 @@ void Uart1ReceivedDataProcessCallback (void* arg, SerialProtocolFrameData* spFra
     	__disable_irq();
     	NVIC_SystemReset();
     	break;
+    case spSetPositonX:
+    	PositionControlTaskData posXData = { 0 };
+    	if (positionXControlTaskInitArg.positionSettingQueue != NULL)
+    	{
+			float posXPercent = 0;
+			ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posXPercent);
+			float posXDegress = MAX_X_AXE_ANGLE * posXPercent * 0.01;
+			float angleDelta = 360 / ENCODER_X_IMP_PER_TURN;
+			float rest = fmodf(posXDegress, angleDelta);
+			if ( rest > (angleDelta/2))
+			{
+				posXData.positionSettingValue = 100 * (posXDegress - rest + angleDelta) / MAX_X_AXE_ANGLE;
+			}
+			else
+			{
+				posXData.positionSettingValue = 100 * (posXDegress - rest) / MAX_X_AXE_ANGLE;
+			}
+			osMessageQueuePut(positionXControlTaskInitArg.positionSettingQueue, &posXData, 0, 0);
+    	}
+    	break;
+    case spSetPositonY:
+    	PositionControlTaskData posYData = { 0 };
+    	if (positionYControlTaskInitArg.positionSettingQueue != NULL)
+    	{
+			float posYPercent = 0;
+			ReadFloatFromBuffer(spFrameData->dataBuffer, &inputDataBufferPos, &posYPercent);
+			float posYDegress = MAX_Y_AXE_ANGLE * posYPercent * 0.01;
+			float angleDelta = 360 / ENCODER_Y_IMP_PER_TURN;
+			float rest = fmodf(posYDegress, angleDelta);
+			if ( rest > (angleDelta/2))
+			{
+				posYData.positionSettingValue = 100 * (posYDegress - rest + angleDelta) / MAX_Y_AXE_ANGLE;
+			}
+			else
+			{
+				posYData.positionSettingValue = 100 * (posYDegress - rest) / MAX_Y_AXE_ANGLE;
+			}
+			osMessageQueuePut(positionYControlTaskInitArg.positionSettingQueue, &posYData, 0, 0);
+    	}
+    	break;
     default: respStatus = spUnknownCommand; break;
     }
     dataToSend = PrepareRespFrame (uartTaskData->uartTxBuffer, spFrameData->frameHeader.frameId, spFrameData->frameHeader.frameCommand, respStatus, outputDataBuffer, outputDataBufferPos);
     if (dataToSend > 0) {
         HAL_UART_Transmit_IT (uartTaskData->huart, uartTaskData->uartTxBuffer, dataToSend);
     }
+#ifdef SERIAL_PROTOCOL_DBG
     printf ("Uart%d: TX bytes sent: %d\n", uartTaskData->uartNumber, dataToSend);
+#endif
 }
 
 

OZE_Sensor/Debug/objects.list

@@ -4,6 +4,7 @@
 "./Core/Src/meas_tasks.o"
 "./Core/Src/mock_tasks.o"
 "./Core/Src/peripherial.o"
+"./Core/Src/position_task.o"
 "./Core/Src/serial_protocol.o"
 "./Core/Src/stm32h7xx_hal_msp.o"
 "./Core/Src/stm32h7xx_hal_timebase_tim.o"