fix and update save funcs

Core/Inc/EEPROM.h

@@ -49,7 +49,7 @@ typedef struct __attribute__((packed)) {
 } Shot;
 
 typedef struct __attribute__((packed)) {
-	unsigned char isExist;
+	unsigned char shotCount; //isExist
 	unsigned char countRepeat;
 	unsigned char options;
 } ProgramHeader;
@@ -65,7 +65,7 @@ typedef struct __attribute__((packed)) {
 } Program;
 
 typedef struct __attribute__((packed)) {
-	unsigned char isExist;
+	unsigned char programmCount; //isExist
 } MacroHeader;
 
 typedef struct __attribute__((packed)) {
@@ -126,18 +126,23 @@ MemoryStatus EEPROM_INIT();
 MemoryStatus EEPROM_EARSE();
 
 MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number,
-		uint8_t *writeData, uint16_t dataSize);
+		uint8_t *writeData, uint16_t dataSize, uint16_t blockSize);
 MemoryStatus FLASH_ReadBlock(uint16_t startAddr, uint8_t number,
 		uint8_t *readData, uint16_t dataSize);
+MemoryStatus FLASH_DelBlock(uint16_t startAddr, uint8_t number,
+		uint16_t dataSize);
 
 MemoryStatus saveShot(unsigned char number, Shot *shot);
 MemoryStatus getShot(unsigned char number, Shot *shot);
+MemoryStatus delShot(unsigned char number);
 
 MemoryStatus saveProg(unsigned char number, Program *prog);
 MemoryStatus getProg(unsigned char number, Program *prog);
+MemoryStatus delProg(unsigned char number);
 
 MemoryStatus saveMacro(unsigned char number, Macro *macro);
 MemoryStatus getMacro(unsigned char number, Macro *macro);
+MemoryStatus delMacro(unsigned char number);
 
 MemoryStatus saveInfoBlock();
 MemoryStatus getInfoBlock();

Core/Inc/RobotFunctions.h

@@ -57,6 +57,8 @@ void doShot(Shot*);
 void doShotForever(uint8_t number);
 
 uint8_t prepareShot(uint8_t number);
+//uint8_t prepareProgramm(uint8_t number);
+//uint8_t prepareMacro(uint8_t number);
 
 void startShooting();
 void stopShooting();
@@ -76,13 +78,4 @@ void setScrewkSpeed(uint8_t speed);
 //(-v) 0 .. 100(stop) .. 200(+v)
 void setRollersSpeed(uint8_t up, uint8_t down); //(-v) 0 . 100(stop) . 200(+v)
 
-void startProgram(); // shot sequence
-void startMacro(); // shot sequence
-
-
-
-
-
-
-
 #endif /* INC_ROBOTFUNCTIONS_H_ */

Core/Src/EEPROM.c

@@ -127,10 +127,9 @@ MemoryStatus EEPROM_INIT() {
 }
 //************************************************************//
 
-
 MemoryStatus saveInfoBlock() {
 	return FLASH_WriteBlock(START_ADR_STAT, 0, (uint8_t*) &infoBlock,
-			sizeof(InfoBlock));
+			sizeof(InfoBlock), sizeof(InfoBlock));
 }
 
 MemoryStatus getInfoBlock() {
@@ -146,7 +145,7 @@ MemoryStatus getInfoBlock() {
 
 MemoryStatus saveShot(unsigned char number, Shot *shot) {
 	if (FLASH_WriteBlock(START_ADR_SHOT, number, (uint8_t*) shot,
-	SHOT_BLOCKSIZE) == EEPROM_OK) {
+	SHOT_BLOCKSIZE, SHOT_BLOCKSIZE) == EEPROM_OK) {
 		return EEPROM_OK;
 	}
 	return EEPROM_FAIL;
@@ -163,9 +162,16 @@ MemoryStatus getShot(unsigned char number, Shot *shot) {
 	return EEPROM_OK;
 }
 
+MemoryStatus delShot(unsigned char number) {
+	if (FLASH_DelBlock(START_ADR_SHOT, number, SHOT_BLOCKSIZE) == EEPROM_OK) {
+		return EEPROM_OK;
+	}
+	return EEPROM_FAIL;
+}
+
 MemoryStatus saveProg(unsigned char number, Program *prog) {
 	MemoryStatus result = EEPROM_OK;
-	for (uint16_t i = 0; i < MAX_SHOT_COUNT_IN_PROGRAMS; ++i) {
+	for (uint16_t i = 0; i < prog->header.shotCount; ++i) {
 		Shot shot;
 		MemoryStatus stat = getShot(prog->shots[i].id, &shot);
 		if (!(stat == EEPROM_OK || stat == EEPROM_MISSING_ELEMENT)) {
@@ -177,7 +183,10 @@ MemoryStatus saveProg(unsigned char number, Program *prog) {
 		}
 	}
 
-	if (FLASH_WriteBlock(START_ADR_PROGRAM, number, (uint8_t*) prog,
+	uint16_t totalSize = sizeof(ProgramHeader)
+			+ sizeof(ProgramShot) * prog->header.shotCount;
+
+	if (FLASH_WriteBlock(START_ADR_PROGRAM, number, (uint8_t*) prog, totalSize,
 	PROGRAM_BLOCKSIZE) != EEPROM_OK) {
 		return EEPROM_FAIL;
 	}
@@ -190,28 +199,39 @@ MemoryStatus getProg(unsigned char number, Program *prog) {
 		return EEPROM_FAIL;
 	}
 
-	if (!prog->header.isExist) {
+	if (!prog->header.shotCount) {
 		return EEPROM_MISSING_ELEMENT;
 	}
 
 	return EEPROM_OK;
 }
 
+MemoryStatus delProg(unsigned char number) {
+	if (FLASH_DelBlock(START_ADR_PROGRAM, number, PROGRAM_BLOCKSIZE)
+			== EEPROM_OK) {
+		return EEPROM_OK;
+	}
+	return EEPROM_FAIL;
+}
+
 MemoryStatus saveMacro(unsigned char number, Macro *macro) {
 	MemoryStatus result = EEPROM_OK;
-	for (uint16_t i = 0; i < MAX_PROGRAM_COUNT_IN_MACRO; ++i) {
+	for (uint16_t i = 0; i < macro->header.programmCount; ++i) {
 		Program prog;
 		MemoryStatus stat = getProg(macro->programs[i].id, &prog);
 		if (!(stat == EEPROM_OK || stat == EEPROM_MISSING_ELEMENT)) {
 			return EEPROM_FAIL;
 		}
-		if (!prog.header.isExist) {
+		if (!prog.header.shotCount) {
 			result = EEPROM_MISSING_ELEMENT;
 			// todo: добавить в запросы для загрузки программ
 		}
 	}
 
-	if (FLASH_WriteBlock(START_ADR_MACRO, number, (uint8_t*) macro,
+	uint16_t totalSize = sizeof(MacroHeader)
+			+ sizeof(MacroProgram) * macro->header.programmCount;
+
+	if (FLASH_WriteBlock(START_ADR_MACRO, number, (uint8_t*) macro, totalSize,
 	MACRO_BLOCKSIZE) != EEPROM_OK) {
 		return EEPROM_FAIL;
 	}
@@ -225,19 +245,26 @@ MemoryStatus getMacro(unsigned char number, Macro *macro) {
 		return EEPROM_FAIL;
 	}
 
-	if (!macro->header.isExist) {
+	if (!macro->header.programmCount) {
 		return EEPROM_MISSING_ELEMENT;
 	}
 	return EEPROM_OK;
 }
 
+MemoryStatus delMacro(unsigned char number) {
+	if (FLASH_DelBlock(START_ADR_MACRO, number, MACRO_BLOCKSIZE) == EEPROM_OK) {
+		return EEPROM_OK;
+	}
+	return EEPROM_FAIL;
+}
+
 MemoryStatus EEPROM_EARSE() {
 	uint16_t addr = 0;
 	uint16_t old_addr = 0;
 	do {
 		uint8_t Buf[255];
 		memset(Buf, 0xFF, sizeof(Buf));
-		FLASH_WriteBlock(addr, 0, Buf, (uint8_t) sizeof(Buf));
+		FLASH_WriteBlock(addr, 0, Buf, sizeof(Buf), sizeof(Buf));
 		old_addr = addr;
 		addr += sizeof(Buf);
 	} while (old_addr <= addr);
@@ -246,7 +273,42 @@ MemoryStatus EEPROM_EARSE() {
 }
 
 MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number,
-		uint8_t *writeData, uint16_t dataSize) {
+		uint8_t *writeData, uint16_t dataSize, uint16_t blockSize) {
+	HAL_StatusTypeDef result;
+
+	// Проверка на корректность входных данных
+	if ((startAddr == START_ADR_SHOT && number >= MAX_SHOT_COUNT)
+			|| (startAddr == START_ADR_PROGRAM && number >= MAX_PROGRAM_COUNT)
+			|| (startAddr == START_ADR_MACRO && number >= MAX_MACRO_COUNT)) {
+		return EEPROM_OUT_OF_RANGE;
+	}
+
+	uint16_t blockAddr16 = (uint16_t) (startAddr
+			+ (uint16_t) (number * blockSize));
+	uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) };
+
+	unsigned char Buf[dataSize + 2];
+	memset(Buf, 0x00, sizeof(Buf));
+	Buf[0] = blockAddr[0];
+	Buf[1] = blockAddr[1];
+
+	for (unsigned char i = 0; i < dataSize; i++)
+		Buf[i + 2] = writeData[i];
+
+	result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), Buf,
+			(dataSize + 2), 10);
+	print("Written ");
+	printNumber(dataSize);
+	print("bytes\n");
+	HAL_Delay(1);
+	if (result != HAL_OK) {
+		return EEPROM_FAIL;
+	}
+	return EEPROM_OK;
+}
+
+MemoryStatus FLASH_DelBlock(uint16_t startAddr, uint8_t number,
+		uint16_t dataSize) {
 	HAL_StatusTypeDef result;
 
 	// Проверка на корректность входных данных
@@ -260,16 +322,14 @@ MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number,
 			+ (uint16_t) (number * dataSize));
 	uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) };
 
-	unsigned char Buf[dataSize + 2];
+	unsigned char Buf[2 + 1];
 	memset(Buf, 0x00, sizeof(Buf));
 	Buf[0] = blockAddr[0];
 	Buf[1] = blockAddr[1];
-
+	Buf[2] = 0;
-	for (unsigned char i = 0; i < dataSize; i++)
-		Buf[i + 2] = writeData[i];
 
 	result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), Buf,
-			(dataSize + 2), 10);
+			sizeof(Buf), 10);
 	HAL_Delay(1);
 	if (result != HAL_OK) {
 		return EEPROM_FAIL;

Core/Src/RobotFunctions.c

@@ -169,6 +169,7 @@ uint8_t prepareShot(uint8_t number) {
 		return 1;
 	} else {
 		// TODO: sound_ERR(); ledFX_ERR();
+		print("shot not exist\n\n");
 		return 0;
 	}
 }
@@ -179,24 +180,18 @@ void setPosSingle(ServoMap servo, uint8_t value) {
     if (servo == SERVO_AXIAL)
         inv = !inv;
 
-    uint8_t deviationToMax = currentServo->max > currentServo->def
-                             ? currentServo->max - currentServo->def
-                             : currentServo->def - currentServo->max;
-
-    uint8_t deviationToMin = currentServo->def > currentServo->min
-                             ? currentServo->def - currentServo->min
-                             : currentServo->min - currentServo->def;
-
-    uint8_t maxDeviation = (deviationToMax > deviationToMin) ? deviationToMax : deviationToMin;
-    uint8_t minLimit = (currentServo->def >= maxDeviation) ? currentServo->def - maxDeviation : 0;
-    uint8_t maxLimit = (currentServo->def + maxDeviation <= 180) ? currentServo->def + maxDeviation : 180;
-    if (value > maxLimit) {
-        value = maxLimit;
-    } else if (value < minLimit) {
-        value = minLimit;
-    }
     if (inv)
         value = 180 - value;
+
+    if(value > 90){
+    	value = map(value, 91, 180, currentServo->def, currentServo->max);
+    }else if(value < 90) {
+    	value = map(value, 89, 0, currentServo->def, currentServo->min);
+    } else {
+    	value = currentServo->def;
+    }
+
+
     SetServo(servo, value);
 }
 
@@ -256,11 +251,3 @@ void setRollersSpeed(uint8_t up, uint8_t down) {
 	Vz1 = 200 - up; // invert
 	Vz2 = down;
 }
-// shot sequence
-void startProgram() {
-
-}
-// shot sequence
-void startMacro() {
-
-}

Core/Src/UART3_CMD_Handler.c

@@ -39,29 +39,70 @@ void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) {
 	uint8_t shotIndx = dataPtr[1];
 
 	Shot shot;
+	shot.isExist = 1;
 	shot.countRepeatShot = dataPtr[2];
-	shot.speedRollerTop = dataPtr[3]+100;
+	shot.speedRollerTop = dataPtr[3] + 100;
-	shot.speedRollerBottom = dataPtr[4]+100;
+	shot.speedRollerBottom = dataPtr[4] + 100;
-	shot.speedScrew = map(dataPtr[6], 0, 120, 0, 100);
+	shot.speedScrew = map(dataPtr[5], 0, 120, 0, 100);
-	shot.rotationAxial = map(dataPtr[5], -99, 99, 0, 180);
+	shot.rotationAxial = map(dataPtr[6], -99, 99, 0, 180);
-	shot.rotationHorizontal = map(dataPtr[6], -99, 99, 90-45, 90+45);
+	shot.rotationHorizontal = map(dataPtr[7], -99, 99, 90 - 45, 90 + 45);
+	shot.rotationVertical = 180 - dataPtr[8];
+
+	saveShot(shotIndx, &shot);
 
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
 void UART3_SaveProgram(uint8_t *dataPtr, uint8_t len) {
-	const uint8_t MIN_PARAM_LENGTH = 0;
+	const uint8_t MIN_PARAM_LENGTH = 5;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	Program prog;
+	uint8_t progIndx = dataPtr[1];
+	prog.header.shotCount = (len - 3) / sizeof(ProgramShot);
+	prog.header.countRepeat = dataPtr[2];
+	prog.header.options = dataPtr[3];
+
+	if (dataPtr[4] != 0xFF && dataPtr[5]) {
+		for (uint8_t i = 0; i < prog.header.shotCount; i++) {
+			uint8_t pos = 4 + i * sizeof(ProgramShot);
+			prog.shots[i].id = dataPtr[pos + 0];
+			prog.shots[i].speedScrew = dataPtr[pos + 1];
+		}
+	} else {
+		delProg(progIndx);
+	}
+
+	saveProg(progIndx, &prog);
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
 void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) {
-	const uint8_t MIN_PARAM_LENGTH = 0;
+	const uint8_t MIN_PARAM_LENGTH = 5;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	Macro macro;
+	uint8_t macroIndx = dataPtr[1];
+	macro.header.programmCount = (len - 1) / sizeof(MacroProgram);
+
+	if (/**/dataPtr[2] != 0xFF && //
+			dataPtr[3] != 0xFF && //
+			dataPtr[4] != 0xFF && //
+			dataPtr[5] != 0xFF) {
+		for (uint8_t i = 0; i < macro.header.programmCount; i++) {
+			uint8_t pos = 2 + i * sizeof(MacroProgram);
+			macro.programs[i].id = dataPtr[pos+0];
+			macro.programs[i].speedScrew = dataPtr[pos+2];
+			macro.programs[i].countRepeat = dataPtr[pos+3];
+			macro.programs[i].options = dataPtr[pos+4];
+		}
+	} else {
+		delMacro(macroIndx);
+	}
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -82,10 +123,14 @@ void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) {
 }
 
 void UART3_StartShot(uint8_t *dataPtr, uint8_t len) {
-	const uint8_t MIN_PARAM_LENGTH = 0;
+	const uint8_t MIN_PARAM_LENGTH = 1;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	uint8_t shotIndx = dataPtr[1];
+	prepareShot(shotIndx);
+	startShooting();
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -98,10 +143,13 @@ void UART3_Stop(uint8_t *dataPtr, uint8_t len) {
 }
 
 void UART3_DeleteShot(uint8_t *dataPtr, uint8_t len) {
-	const uint8_t MIN_PARAM_LENGTH = 0;
+	const uint8_t MIN_PARAM_LENGTH = 1;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	uint8_t shotIndex = dataPtr[1];
+	delShot(shotIndex);
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -110,6 +158,9 @@ void UART3_DeleteProgram(uint8_t *dataPtr, uint8_t len) {
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	uint8_t progIndex = dataPtr[1];
+	delProg(progIndex);
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -118,6 +169,9 @@ void UART3_DeleteMacro(uint8_t *dataPtr, uint8_t len) {
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	uint8_t macroIndex = dataPtr[1];
+	delMacro(macroIndex);
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -127,7 +181,10 @@ void UART3_DeleteAllData(uint8_t *dataPtr, uint8_t len) {
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	getInfoBlock();
 	EEPROM_EARSE();
+	saveInfoBlock();
+
 
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
@@ -152,22 +209,15 @@ void UART3_SetServoOffset(uint8_t *dataPtr, uint8_t len) {
 	ServoMap servo = dataPtr[1];
 	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
 	int16_t newDef = (dataPtr[2] << 8) | dataPtr[3];
-    newDef+=90; // from center
 
-    if (newDef < 0) newDef = 0;
+	newDef += 90; // from center
-    if (newDef > 180) newDef = 180;
+	if (newDef < 0)
-
+		newDef = 0;
-    int16_t maxDeviation = (currentServo->max > currentServo->def)
+	if (newDef > 180)
-                           ? currentServo->max - currentServo->def
+		newDef = 180;
-                           : currentServo->def - currentServo->min;
 
 	currentServo->def = newDef;
-    int16_t newMax = currentServo->def + maxDeviation;
+
-    int16_t newMin = currentServo->def - maxDeviation;
-    if (newMax > 180) newMax = 180;
-    if (newMin < 0) newMin = 0;
-    currentServo->max = newMax;
-    currentServo->min = newMin;
 	saveInfoBlock();
 
 	SendResponse(dataPtr[0], 0, NULL, 0);
@@ -190,7 +240,6 @@ void UART3_GetServoOffset(uint8_t *dataPtr, uint8_t len) {
 	SendResponse(dataPtr[0], 0, res, sizeof(res));
 }
 
-
 //201
 void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
 	const uint8_t MIN_PARAM_LENGTH = 3;
@@ -200,18 +249,17 @@ void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
 	ServoMap servo = dataPtr[1];
 	uint16_t maxAngl = (dataPtr[2] << 8) | dataPtr[3];
 	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
-    int16_t newMax = currentServo->def + maxAngl;
+
-    int16_t newMin = currentServo->def - maxAngl;
+	if (maxAngl > 180)
-    if (newMax > 180) newMax = 180;
+		maxAngl = 180;
-    if (newMin < 0) newMin = 0;
+	if (maxAngl < 0)
-    currentServo->max = newMax;
+		maxAngl = 0;
-    currentServo->min = newMin;
+	currentServo->max = maxAngl;
+
 	saveInfoBlock();
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
-
-//202
 void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
 	const uint8_t MIN_PARAM_LENGTH = 1;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
@@ -220,15 +268,7 @@ void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
 	ServoMap servo = dataPtr[1];
 	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
 
-    uint8_t deviationToMax = currentServo->max > currentServo->def
+	uint8_t maxAngl = currentServo->max;
-                             ? currentServo->max - currentServo->def
-                             : currentServo->def - currentServo->max;
-
-    uint8_t deviationToMin = currentServo->def > currentServo->min
-                             ? currentServo->def - currentServo->min
-                             : currentServo->min - currentServo->def;
-
-    uint8_t maxAngl = (deviationToMax > deviationToMin) ? deviationToMax : deviationToMin;
 
 	uint8_t res[2];
 	res[0] = HIGHBIT(maxAngl);
@@ -236,6 +276,41 @@ void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
 	SendResponse(dataPtr[0], 0, res, sizeof(res));
 }
 
+//202
+void UART3_SetServoMinAngle(uint8_t *dataPtr, uint8_t len) {
+	const uint8_t MIN_PARAM_LENGTH = 3;
+	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
+		return;
+
+	ServoMap servo = dataPtr[1];
+	uint16_t minAngl = (dataPtr[2] << 8) | dataPtr[3];
+	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
+
+	if (minAngl > 180)
+		minAngl = 180;
+	if (minAngl < 0)
+		minAngl = 0;
+	currentServo->min = minAngl;
+
+	saveInfoBlock();
+	SendResponse(dataPtr[0], 0, NULL, 0);
+}
+
+void UART3_GetServoMinAngle(uint8_t *dataPtr, uint8_t len) {
+	const uint8_t MIN_PARAM_LENGTH = 1;
+	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
+		return;
+
+	ServoMap servo = dataPtr[1];
+	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
+
+	uint8_t minAngl = currentServo->min;
+
+	uint8_t res[2];
+	res[0] = HIGHBIT(minAngl);
+	res[1] = LOWBIT(minAngl);
+	SendResponse(dataPtr[0], 0, res, sizeof(res));
+}
 
 //203
 void UART3_MoveServoToInitialPosition(uint8_t *dataPtr, uint8_t len) {

Core/Src/UART3_Handler.c

@@ -250,7 +250,7 @@ void UART3_CMD_Handler(uint8_t *dataPtr, uint8_t len) {
 		UART3_SetServoMaxAngle(dataPtr, len);
 		break;
 	case 202:
-		UART3_GetServoMaxAngle(dataPtr, len);
+		UART3_SetServoMinAngle(dataPtr, len);
 		break;
 	case 203:
 		UART3_MoveServoToInitialPosition(dataPtr, len);