add SF_MoveToFlap

ADIS_tinyK22_SplitFlap/source/splitflap.c

@@ -88,12 +88,13 @@ bool SF_MoveMotorToZeroPosition(SF_Handle_t instance){
 		McuWait_Waitms(20);
 		numStepsMoved++;
 	}
-
+	McuULN2003_SetPos(((SF_t*)instance)->motor, 0);
+	McuULN2003_PowerOff(((SF_t*)instance)->motor);
 	// success if less than one rotation
 	return numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND;
 }
 
-void SF_MoveSteps(SF_Handle_t instance, uint8_t steps){
+void SF_MoveSteps(SF_Handle_t instance, int32_t steps){
 
 	// run move with acceleration & deceleration
 	McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
@@ -115,6 +116,23 @@ bool SF_GetMagSensorAtZeroPosition(SF_Handle_t instance){
 	return McuGPIO_GetValue(((SF_t*)instance)->magSensor);
 }
 
+void SF_MoveToFlap(SF_Handle_t instance, char* flap){
+	// get flap pos from dictonary
+	int32_t flapPos = (int32_t)getItem(*splitFlapDict, flap);
+	// get current motor pos
+	int32_t currentPos = McuULN2003_GetPos(((SF_t*)instance)->motor);
+	// calc steps to move
+	int32_t stepsToReachFlap = 0;
+	if(flapPos == currentPos){
+		if(flapPos < currentPos){
+				stepsToReachFlap = SPLITFLAP_STEPS_ONE_ROUND-currentPos+flapPos;
+		}else if(flapPos > currentPos){
+				stepsToReachFlap = flapPos - currentPos;
+		}
+		SF_MoveSteps(((SF_t*)instance)->motor, stepsToReachFlap);
+	}
+
+}
 
 void SF_Deinit(SF_Handle_t instance){
 	McuULN2003_DeinitMotor(((SF_t*)instance)->motor);

ADIS_tinyK22_SplitFlap/source/splitflap.h

@@ -52,11 +52,14 @@ SF_Handle_t SF_Init(SF_Config_t* instance, int id);
 bool SF_MoveMotorToZeroPosition(SF_Handle_t instance);
 
 /* split flap moves number of steps maybe uint16_t? */
-void SF_MoveSteps(SF_Handle_t instance, uint8_t steps);
+void SF_MoveSteps(SF_Handle_t instance, int32_t steps);
 
 /* get mag sensor state. returns true if sensor is at zero position, otherwise false */
 bool SF_GetMagSensorAtZeroPosition(SF_Handle_t instance);
 
+/* move to specific flap */
+void SF_MoveToFlap(SF_Handle_t instance, char* flap);
+
 /* split flap deinitialization */
 void SF_Deinit(SF_Handle_t instance);