global refactoring

Core/Src/lock.c

@@ -0,0 +1,88 @@
+/*
+ * lock.c
+ *
+ *  Created on: Jul 31, 2024
+ *      Author: colorbass
+ */
+#include "lock.h"
+
+uint8_t LOCK_POLARITY = 1;
+uint8_t LOCK_MOTOR_POLARITY = 1;
+uint8_t LOCK_DELAY = 50;
+
+GBT_LockState_t GBT_LockState;
+
+
+void GBT_ForceLock(uint8_t state){
+	if(LOCK_MOTOR_POLARITY){
+		if(state){//LOCK
+			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 1);
+			  HAL_Delay(LOCK_DELAY);
+			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 0);
+		}else{ //UNLOCK
+			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 1);
+			  HAL_Delay(LOCK_DELAY);
+			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 0);
+		}
+	}else{
+		if(state){//LOCK
+			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 1);
+			  HAL_Delay(LOCK_DELAY);
+			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 0);
+		}else{ //UNLOCK
+			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 1);
+			  HAL_Delay(LOCK_DELAY);
+			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 0);
+		}
+	}
+}
+
+uint8_t GBT_LockGetState(){
+	//1 = locked
+	//0 = unlocked
+	if(LOCK_POLARITY){
+		return HAL_GPIO_ReadPin(LOCK_FB_GPIO_Port, LOCK_FB_Pin);
+	}else{
+		return !HAL_GPIO_ReadPin(LOCK_FB_GPIO_Port, LOCK_FB_Pin);
+
+	}
+}
+
+void GBT_Lock(uint8_t state){
+	GBT_LockState.demand = state;
+}
+
+void GBT_ManageLock(){
+	uint8_t MAX_RETRIES = 5;
+    if (GBT_LockState.error) {
+        return;
+    }
+
+	bool lock_is_open = GBT_LockGetState() == 0;
+	bool lock_should_be_open = GBT_LockState.demand == 0;
+	uint8_t retry_count = 0;
+
+	if (lock_is_open != lock_should_be_open) {
+		while (retry_count < MAX_RETRIES) {
+			if (lock_should_be_open) {
+				GBT_ForceLock(0);
+			} else {
+				GBT_ForceLock(1);
+			}
+
+			lock_is_open = GBT_LockGetState() == 0;
+
+			if (lock_is_open == lock_should_be_open) {
+				break;
+			}
+
+			retry_count++;
+		}
+
+		if (retry_count >= MAX_RETRIES) {
+			GBT_LockState.error = 1;
+			GBT_ForceLock(0);
+			printf ("Lock error\n");
+		}
+	}
+}