programm works

Core/Inc/EEPROM.h

@@ -106,8 +106,8 @@ typedef struct __attribute__((packed)) {
 } HardwareInit_t;
 
 typedef struct __attribute__((packed)) {
-	uint32_t totalShots;
+	uint32_t shotsInShot;
-	uint32_t totalPrograms;
+	uint32_t shotsInProgram;
 	uint32_t totalMacros;
 } Statistics;
 

Core/Inc/RobotFunctions.h

@@ -36,14 +36,16 @@ typedef enum State{
 
 typedef struct CurrentShot {
 	uint8_t index;
-	uint16_t currentRepeatCount;
+	uint16_t currentBallCount;
 	Shot shot;
 } CurrentShot;
 
 typedef struct CurrentProgram {
 	uint8_t index;
 	Program program;
-	uint8_t shot_index;
+	CurrentShot currentShot;
+	uint8_t programShotId;
+	uint16_t currentBallCount;
 } CurrentProgram;
 
 typedef struct CurrentMacro {
@@ -75,7 +77,7 @@ void BallEXT();
 void RobotTick();
 
 uint8_t prepareShot(uint8_t number);
-//uint8_t prepareProgramm(uint8_t number);
+uint8_t prepareProgramm(uint8_t number);
 //uint8_t prepareMacro(uint8_t number);
 
 

Core/Src/EEPROM.c

@@ -46,8 +46,8 @@ MemoryStatus EEPROM_INIT() {
 		infoBlock.hwInfo.servos[SERVO_VERTICAL].def = 90;
 		infoBlock.hwInfo.servos[SERVO_VERTICAL].max = 180;
 
-		infoBlock.statInfo.totalShots = 0;
+		infoBlock.statInfo.shotsInShot = 0;
-		infoBlock.statInfo.totalPrograms = 0;
+		infoBlock.statInfo.shotsInProgram = 0;
 		infoBlock.statInfo.totalMacros = 0;
 
 		status = saveInfoBlock(&infoBlock);
@@ -112,11 +112,11 @@ MemoryStatus EEPROM_INIT() {
 	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));
 
 	snprintf(buffer, sizeof(buffer), "Total Shots: %lu\n",
-			infoBlock.statInfo.totalShots);
+			infoBlock.statInfo.shotsInShot);
 	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));
 
 	snprintf(buffer, sizeof(buffer), "Total Programs: %lu\n",
-			infoBlock.statInfo.totalPrograms);
+			infoBlock.statInfo.shotsInProgram);
 	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));
 
 	snprintf(buffer, sizeof(buffer), "Total Macros: %lu\n\n",
@@ -263,7 +263,7 @@ MemoryStatus EEPROM_EARSE() {
 	uint16_t old_addr = 0;
 	do {
 		uint8_t Buf[255];
-		memset(Buf, 0xFF, sizeof(Buf));
+		memset(Buf, 0x00, sizeof(Buf));
 		FLASH_WriteBlock(addr, 0, Buf, sizeof(Buf), sizeof(Buf));
 		old_addr = addr;
 		addr += sizeof(Buf);

Core/Src/RobotFunctions.c

@@ -25,12 +25,22 @@ uint8_t ballDetected = 0;
 uint32_t ballReact_timer = 0;
 uint8_t isDelayTimerRun = 0;
 uint32_t startDelay_timer;
-Shot* postDelayShot = NULL;
+Shot *postDelayShot = NULL;
 uint32_t lastIndicationTime = 0;
 uint32_t noBallTimer = 0;
 uint32_t noBallTimeout = 0xFFFFFFFF;
 
 void BallEXT_Handler();
+Shot* getShotFromProgram(CurrentProgram *currentProg);
+void robotStateStart(uint32_t delayTime);
+void robotStatePause();
+void robotStateStop();
+float calculatePeriod(int pwm_value);
+uint8_t nextBallinShot(CurrentShot *shot);
+void shotApply(Shot *shot);
+uint8_t loadNextShotInProgram(CurrentProgram *currentProg);
+uint8_t loadShotFromProgram(CurrentProgram *currentProg);
+Shot* getShotFromProgram(CurrentProgram *currentProg);
 
 void robotStateStart(uint32_t delayTime) {
 	currentInfo.state = PRERUN_WAIT;
@@ -51,7 +61,6 @@ void BallEXT() {
 	ballReact_timer = millis();
 }
 
-
 // Функция для расчета периода вылета мяча на основе ШИМ
 float calculatePeriod(int pwm_value) {
 	// Коэффициенты из аппроксимации
@@ -68,7 +77,7 @@ void RobotTick() {
 	BallEXT_Handler();
 
 	// No Ball Handler
-    if(currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout){
+	if (currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout) {
 		robotStateStop();
 		setScrewkSpeed(0);
 		setRollersSpeed(100, 100);
@@ -102,6 +111,27 @@ void RobotTick() {
 	}
 }
 
+uint8_t nextBallinShot(CurrentShot *shot) {
+
+	print("shot->shot.countRepeatShot ");
+	printNumber(shot->shot.countRepeatShot);
+	print("\n");
+
+	if (shot->currentBallCount + 1 < shot->shot.countRepeatShot
+			|| !shot->shot.countRepeatShot) {
+		// nextBall
+		print("Shot ");
+		printNumber(shot->currentBallCount);
+		print("\n\n");
+
+		shot->currentBallCount++;
+		return 0;
+	} else {
+		print("Shot DONE\n");
+		return 1;
+	}
+}
+
 void BallEXT_Handler() {
 	if (ballDetected && millis() - ballReact_timer > ballReact_value) {
 		ballDetected = 0;
@@ -122,24 +152,30 @@ void BallEXT_Handler() {
 
 			break;
 		case ShotMode:
-			if (currentInfo.shot.currentRepeatCount + 1
+			if (nextBallinShot(&currentInfo.shot)) {
-					< currentInfo.shot.shot.countRepeatShot
-					|| !currentInfo.shot.shot.countRepeatShot) {
-				// nextRepeatCount
-				print("Shot ");
-				printNumber(currentInfo.shot.currentRepeatCount);
-				print("\n\n");
-				saveInfoBlock();
-				currentInfo.shot.currentRepeatCount++;
-			} else {
-				// shotDone
 				stopShooting();
-				print("Shot DONE\n");
 			}
-			infoBlock.statInfo.totalShots++;
+			infoBlock.statInfo.shotsInShot++;
+			saveInfoBlock();
 			break;
 		case ProgramMode:
+			if (nextBallinShot(&currentInfo.program.currentShot)) {
+				if (!loadNextShotInProgram(&currentInfo.program)) {
+					stopShooting();
+					print("loadNextShotInProgram ERR\n");
+				}
+				shotApply(getShotFromProgram(&currentInfo.program));
+			}
 
+			if (currentInfo.program.currentBallCount + 1
+					< currentInfo.program.program.header.countRepeat) {
+				currentInfo.program.currentBallCount++;
+			} else {
+				stopShooting();
+				print("Program DONE\n");
+			}
+			infoBlock.statInfo.shotsInProgram++;
+			saveInfoBlock();
 			break;
 		case MacroMode:
 
@@ -230,38 +266,93 @@ void shotApply(Shot *shot) {
 			shot->rotationVertical);
 	setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);
 	setScrewkSpeed(shot->speedScrew);
+	noBallTimeout = calculatePeriod(
+			shot->speedScrew) * NOBALL_TIMEOUT_MULTIPLIER;
 	noBallTimer = millis();
+	print("Fire!\n");
+	print("isExist               ");
+	printNumber(shot->isExist);
+	print("\ncountRepeatShot;    ");
+	printNumber(shot->countRepeatShot);
+	print("\nspeedRollerTop;     ");
+	printNumber(shot->speedRollerTop);
+	print("\nspeedRollerBottom;  ");
+	printNumber(shot->speedRollerBottom);
+	print("\nspeedScrew;         ");
+	printNumber(shot->speedScrew);
+	print("\nrotationAxial;      ");
+	printNumber(shot->rotationAxial);
+	print("\nrotationHorizontal; ");
+	printNumber(shot->rotationHorizontal);
+	print("\nrotationVertical;   ");
+	printNumber(shot->rotationVertical);
+	print("\nnoBallTimeout: ");
+	printNumber(noBallTimeout);
+	print("\n");
+	print("\n\n");
+}
+
+uint8_t loadShotFromProgram(CurrentProgram *currentProg) {
+	Shot shot_;
+	uint8_t currentProgramShotId = currentProg->programShotId;
+	if (currentProgramShotId >= currentProg->program.header.shotCount) {
+		print("Shot index out of range\n");
+		// TODO: sound_ERR(); ledFX_ERR();
+		return 0;
+	}
+	uint8_t shotId = currentProg->program.shots[currentProgramShotId].id;
+	currentProg->currentShot.index = shotId;
+	getShot(shotId, &shot_);
+	if (!shot_.isExist) {
+		print("Shot in program is NULL\n");
+		// TODO: sound_ERR(); ledFX_ERR();
+		return 0;
+	}
+	shot_.speedScrew =
+			currentProg->program.shots[currentProgramShotId].speedScrew;
+	uint8_t repeatCountFromShot = currentProg->program.header.options & 2U;
+	if (!repeatCountFromShot) {
+		print("Repeat Count Override\n");
+		shot_.countRepeatShot = 1;
+	}
+	currentProg->currentShot.shot = shot_;
+	print("REPEAT ");
+	printNumber(shot_.countRepeatShot);
+	print("\n");
+	currentProg->currentShot.currentBallCount = 0;
+	return 1;
+}
+
+uint8_t loadNextShotInProgram(CurrentProgram *currentProg) {
+	uint8_t random = currentProg->program.header.options & 1U;
+	uint8_t *currentProgramShotId = &currentProg->programShotId;
+
+	if (random) {
+		*currentProgramShotId = rand() % currentProg->program.header.shotCount;
+	} else {
+		if ((*currentProgramShotId) + 1
+				< currentProg->program.header.shotCount) {
+			++(*currentProgramShotId);
+		} else {
+			*currentProgramShotId = 0;
+		}
+	}
+	return loadShotFromProgram(currentProg);
+}
+
+Shot* getShotFromProgram(CurrentProgram *currentProg) {
+	return &currentProg->currentShot.shot;
 }
 
 void startShooting(uint32_t delayTime) {
+	print("StartShooting\n");
 	switch (currentInfo.mode) {
 	case ShotMode:
-		print("StartShooting\n");
 		if (currentInfo.shot.shot.isExist) {
-			print("Fire!\n");
-
-			print("isExist             ");
-			printNumber(currentInfo.shot.shot.isExist);
-			print("\ncountRepeatShot;    ");
-			printNumber(currentInfo.shot.shot.countRepeatShot);
-			print("\nspeedRollerTop;     ");
-			printNumber(currentInfo.shot.shot.speedRollerTop);
-			print("\nspeedRollerBottom;  ");
-			printNumber(currentInfo.shot.shot.speedRollerBottom);
-			print("\nspeedScrew;         ");
-			printNumber(currentInfo.shot.shot.speedScrew);
-			print("\nrotationAxial;      ");
-			printNumber(currentInfo.shot.shot.rotationAxial);
-			print("\nrotationHorizontal; ");
-			printNumber(currentInfo.shot.shot.rotationHorizontal);
-			print("\nrotationVertical;   ");
-			printNumber(currentInfo.shot.shot.rotationVertical);
-			print("\n\n");
 			postDelayShot = &currentInfo.shot.shot;
-#if PRE_RUN_DELAY_MODE == 0
+			if (PRE_RUN_DELAY_MODE) {
-#elif PRE_RUN_DELAY_MODE == 1
 				setPosFromShot(postDelayShot);
-#endif
+			}
 			robotStateStart(delayTime);
 		} else {
 			print("Current Shot is NULL\n");
@@ -269,10 +360,29 @@ void startShooting(uint32_t delayTime) {
 		}
 		break;
 	case ProgramMode:
+		if (currentInfo.program.program.header.shotCount) {
+			if (!loadShotFromProgram(&currentInfo.program)) {
+				print("loadShotFromProgram ERR\n");
+				break;
+			}
+			postDelayShot = getShotFromProgram(&currentInfo.program);
+			if (PRE_RUN_DELAY_MODE) {
+				setPosFromShot(postDelayShot);
+			}
+			robotStateStart(delayTime);
 
+		} else {
+			print("Current Program is NULL\n");
+			// TODO: sound_ERR(); ledFX_ERR();
+		}
 		break;
 	case MacroMode:
+		if (currentInfo.macro.macro.header.programmCount) {
 
+		} else {
+			print("Current Macro is NULL\n");
+			// TODO: sound_ERR(); ledFX_ERR();
+		}
 		break;
 	default:
 		break;
@@ -292,11 +402,7 @@ uint8_t prepareShot(uint8_t number) {
 	if (shot.isExist) {
 		currentInfo.mode = ShotMode;
 		currentInfo.shot.shot = shot;
-		currentInfo.shot.currentRepeatCount = 0;
+		currentInfo.shot.currentBallCount = 0;
-		noBallTimeout = calculatePeriod(shot.speedScrew)*NOBALL_TIMEOUT_MULTIPLIER;
-		print("noBallTimeout: ");
-		printNumber(noBallTimeout);
-		print("\n");
 		return 1;
 	} else {
 		// TODO: sound_ERR(); ledFX_ERR();
@@ -305,6 +411,23 @@ uint8_t prepareShot(uint8_t number) {
 	}
 }
 
+uint8_t prepareProgramm(uint8_t number) {
+	Program program;
+	getProg(number, &program);
+	if (program.header.shotCount) { // isExist
+		currentInfo.mode = ProgramMode;
+		currentInfo.program.program = program;
+		currentInfo.program.index = number;
+		currentInfo.program.programShotId = 0;
+		currentInfo.program.currentBallCount = 0;
+		return 1;
+	} else {
+		// TODO: sound_ERR(); ledFX_ERR();
+		print("program not exist\n\n");
+		return 0;
+	}
+}
+
 void setPosSingle(ServoMap servo, uint8_t value) {
 	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
 	uint8_t inv = currentServo->invert;
@@ -331,8 +454,9 @@ void setPos(uint8_t axial, uint8_t horizontal, uint8_t vertical) {
 	setPosSingle(SERVO_VERTICAL, vertical);
 }
 
-void setPosFromShot(Shot* shot){
+void setPosFromShot(Shot *shot) {
-	setPos(shot->rotationAxial, shot->rotationHorizontal, shot->rotationVertical);
+	setPos(shot->rotationAxial, shot->rotationHorizontal,
+			shot->rotationVertical);
 }
 
 void setPosDefault() {

Core/Src/UART3_CMD_Handler.c

@@ -45,11 +45,9 @@ void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) {
 	shot.speedRollerTop = dataPtr[3] + 100;
 	shot.speedRollerBottom = dataPtr[4] + 100;
 	shot.speedScrew = map(dataPtr[5], 0, 120, 0, 100);
-	shot.rotationAxial = map((int8_t)dataPtr[6], -99, 99, 0, 180);
+	shot.rotationAxial = map((int8_t) dataPtr[6], -99, 99, 0, 180);
-	shot.rotationHorizontal = map((int8_t)dataPtr[7], -99, 99, 0, 180);
+	shot.rotationHorizontal = map((int8_t) dataPtr[7], -99, 99, 0, 180);
-	shot.rotationVertical = 180 - (int8_t)dataPtr[8] - 90;
+	shot.rotationVertical = 180 - (int8_t) dataPtr[8] - 90;
-
-
 
 	saveShot(shotIndx, &shot);
 
@@ -97,10 +95,10 @@ void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) {
 			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].id = dataPtr[pos + 0];
-			macro.programs[i].speedScrew = dataPtr[pos+2];
+			macro.programs[i].speedScrew = dataPtr[pos + 2];
-			macro.programs[i].countRepeat = dataPtr[pos+3];
+			macro.programs[i].countRepeat = dataPtr[pos + 3];
-			macro.programs[i].options = dataPtr[pos+4];
+			macro.programs[i].options = dataPtr[pos + 4];
 		}
 	} else {
 		delMacro(macroIndx);
@@ -109,7 +107,6 @@ void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) {
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
-
 void UART3_StartMacro(uint8_t *dataPtr, uint8_t len) {
 	const uint8_t MIN_PARAM_LENGTH = 0;
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
@@ -123,6 +120,10 @@ void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) {
 	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
 		return;
 
+	uint8_t progIndx = dataPtr[1];
+	prepareProgramm(progIndx);
+	startShooting(infoBlock.hwInfo.timings.preRun);
+
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -196,7 +197,6 @@ void UART3_DeleteAllData(uint8_t *dataPtr, uint8_t len) {
 	EEPROM_EARSE();
 	saveInfoBlock();
 
-
 	SendResponse(dataPtr[0], 0, NULL, 0);
 }
 
@@ -452,26 +452,41 @@ void UART3_ReadStatistics(uint8_t *dataPtr, uint8_t len) {
 		uint8_t macro_number;
 		uint8_t program_number;
 		uint8_t shot_number;
-		uint8_t total_macro_done_HIGH;
 		uint8_t total_macro_done_LOW;
-		uint8_t total_program_done_HIGH;
+		uint8_t total_macro_done_HIGH;
 		uint8_t total_program_done_LOW;
-		uint8_t total_shot_done_HIGH;
+		uint8_t total_program_done_HIGH;
 		uint8_t total_shot_done_LOW;
+		uint8_t total_shot_done_HIGH;
 	} StatusStruct;
 
 	StatusStruct res;
+	uint8_t isRun = currentInfo.state == RUN;
+	res.status = isRun;
 
-	res.status = currentInfo.state == RUN;
+	res.shot_number = 0xFF;
-	res.shot_number = currentInfo.shot.index;
+	res.program_number = 0xFF;
-	res.program_number = currentInfo.program.index;
+	res.macro_number = 0xFF;
-	res.macro_number = currentInfo.macro.index;
 
-	res.total_shot_done_HIGH = HIGHBIT(infoBlock.statInfo.totalShots);
+	switch (currentInfo.mode) {
-	res.total_shot_done_LOW = LOWBIT(infoBlock.statInfo.totalShots);
+	case ShotMode:
+		res.shot_number = isRun ? currentInfo.shot.index : 0xFF;
+		break;
+	case ProgramMode:
+		res.program_number = isRun ? currentInfo.program.index : 0xFF;
+		break;
+	case MacroMode:
+		res.macro_number = isRun ? currentInfo.macro.index : 0xFF;
+		break;
+	default:
+		break;
+	}
 
-	res.total_program_done_HIGH = HIGHBIT(infoBlock.statInfo.totalPrograms);
+	res.total_shot_done_HIGH = HIGHBIT(infoBlock.statInfo.shotsInShot);
-	res.total_program_done_LOW = LOWBIT(infoBlock.statInfo.totalPrograms);
+	res.total_shot_done_LOW = LOWBIT(infoBlock.statInfo.shotsInShot);
+
+	res.total_program_done_HIGH = HIGHBIT(infoBlock.statInfo.shotsInProgram);
+	res.total_program_done_LOW = LOWBIT(infoBlock.statInfo.shotsInProgram);
 
 	res.total_macro_done_HIGH = HIGHBIT(infoBlock.statInfo.totalMacros);
 	res.total_macro_done_LOW = LOWBIT(infoBlock.statInfo.totalMacros);

TODO.md

@@ -47,5 +47,10 @@ IR:
 
 	
 Ошибки:
-	V В некоторый момент PID регулятор выдаёт 0 и двигатель не запускается не зависимо от входного значенияPWM
+	V+- В некоторый момент PID регулятор выдаёт 0 и двигатель не запускается не зависимо от входного значенияPWM
+	Оранж:
+		При сбоях UART вешает его и иногда не переинициализирует даже после рестарта контроллера
+		Программы:
+			Есл стоит "Настроить темп ударов", во все удары приходят нули.
+