From 2821ca66707f38dee5f9caa5a4db9cb2dd513d48 Mon Sep 17 00:00:00 2001
From: Jonas Arnold <jonas.arnold@live.de>
Date: Fri, 30 Sep 2022 17:34:52 +0200
Subject: [PATCH] added testpattern for moving directly to a letter added
 SF_GetMotorPosition

---
 ADIS_tinyK22_SplitFlap/source/application.c | 12 ++++++++++++
 ADIS_tinyK22_SplitFlap/source/splitflap.c   |  9 +++++++--
 ADIS_tinyK22_SplitFlap/source/splitflap.h   |  4 ++++
 3 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/ADIS_tinyK22_SplitFlap/source/application.c b/ADIS_tinyK22_SplitFlap/source/application.c
index 9444728..1183c76 100644
--- a/ADIS_tinyK22_SplitFlap/source/application.c
+++ b/ADIS_tinyK22_SplitFlap/source/application.c
@@ -5,6 +5,7 @@
  *      Author: jonas
  */
 
+#include "fsl_debug_console.h"
 #include "application.h"
 #include "McuWait.h"
 #include "McuLED.h"
@@ -71,8 +72,19 @@ void App_Init(void){
 /* Application run */
 void App_Run(void){
 
+	PRINTF("Initializing split flap motors.");
 	SF_MoveMotorToZeroPosition(splitflap0);
+	PRINTF("Init of motors done.");
 
+	// go through the whole dictionary
+	char* letters[] = {"A", "B", "C", "A"};
+
+	for(int i = 0; i < 4; i++){
+		McuWait_Waitms(1000);
+		PRINTF("Moving to letter '%s', position is %i", letters[i], (int)SF_GetMotorPosition(splitflap0));
+		SF_MoveToFlap(splitflap0, letters[i]);
+		PRINTF("Position is %i", (int)SF_GetMotorPosition(splitflap0));
+	}
 
 
 	/* TEST PATTERN */
diff --git a/ADIS_tinyK22_SplitFlap/source/splitflap.c b/ADIS_tinyK22_SplitFlap/source/splitflap.c
index 9892548..b2f0a3d 100644
--- a/ADIS_tinyK22_SplitFlap/source/splitflap.c
+++ b/ADIS_tinyK22_SplitFlap/source/splitflap.c
@@ -13,7 +13,7 @@
 #include "lib/dict.h"
 #include "splitflap_flaps.h"
 
-dict_t **splitFlapDict;
+static dict_t **splitFlapDict;
 
 void SF_InitConfig(void){
 	splitFlapDict = dictAlloc();
@@ -144,7 +144,8 @@ void SF_MoveToFlap(SF_Handle_t instance, char* flap){
 	int32_t currentPos = McuULN2003_GetPos(((SF_t*)instance)->motor);
 	// calc steps to move
 	int32_t stepsToReachFlap = 0;
-	if(flapPos == currentPos){
+	// not already there
+	if(flapPos != currentPos){
 		if(flapPos < currentPos){
 				stepsToReachFlap = SPLITFLAP_STEPS_ONE_ROUND-currentPos+flapPos;
 		}else if(flapPos > currentPos){
@@ -155,6 +156,10 @@ void SF_MoveToFlap(SF_Handle_t instance, char* flap){
 
 }
 
+int32_t SF_GetMotorPosition(SF_Handle_t instance){
+	return McuULN2003_GetPos(((SF_t*)instance)->motor);
+}
+
 void SF_Deinit(SF_Handle_t instance){
 	McuULN2003_DeinitMotor(((SF_t*)instance)->motor);
 	McuGPIO_DeinitGPIO(((SF_t*)instance)->magSensor);
diff --git a/ADIS_tinyK22_SplitFlap/source/splitflap.h b/ADIS_tinyK22_SplitFlap/source/splitflap.h
index 05b4144..9ee2f52 100644
--- a/ADIS_tinyK22_SplitFlap/source/splitflap.h
+++ b/ADIS_tinyK22_SplitFlap/source/splitflap.h
@@ -11,6 +11,7 @@
 #include <stdbool.h>
 #include "McuULN2003.h"
 #include "McuGPIO.h"
+#include "lib/dict.h"
 
 
 /****** SETTINGS ******/
@@ -61,6 +62,9 @@ bool SF_GetMagSensorAtZeroPosition(SF_Handle_t instance);
 /* move to specific flap */
 void SF_MoveToFlap(SF_Handle_t instance, char* flap);
 
+/* get current position of motor */
+int32_t SF_GetMotorPosition(SF_Handle_t instance);
+
 /* split flap deinitialization */
 void SF_Deinit(SF_Handle_t instance);