mirror of
https://github.com/DashyFox/StackSport.git
synced 2025-06-28 05:09:32 +00:00
half work
This commit is contained in:
@ -25,22 +25,23 @@ uint8_t ballDetected = 0;
|
||||
uint32_t ballReact_timer = 0;
|
||||
uint8_t isDelayTimerRun = 0;
|
||||
uint32_t startDelay_timer;
|
||||
Shot *postDelayShot = NULL;
|
||||
CurrentShot *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 nextBallInShot(CurrentShot *shot);
|
||||
uint8_t nextProgramInMacro(CurrentMacro *macro);
|
||||
uint8_t nextShotInProgram(CurrentProgram *prog);
|
||||
uint8_t loadNextShotInProgram(CurrentProgram *currentProg);
|
||||
uint8_t loadShotFromProgram(CurrentProgram *currentProg);
|
||||
Shot* getShotFromProgram(CurrentProgram *currentProg);
|
||||
uint8_t loadNextProgramInMacro(CurrentMacro *currentMacro);
|
||||
uint8_t loadProgramFromMacro(CurrentMacro *currentMacro);
|
||||
|
||||
void robotStateStart(uint32_t delayTime) {
|
||||
currentInfo.state = PRERUN_WAIT;
|
||||
@ -56,13 +57,10 @@ void robotStateStop() {
|
||||
currentInfo.state = STOP;
|
||||
}
|
||||
|
||||
void BallEXT() {
|
||||
ballDetected = 1;
|
||||
ballReact_timer = millis();
|
||||
long map(long x, long in_min, long in_max, long out_min, long out_max) {
|
||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
}
|
||||
|
||||
// Функция для расчета периода вылета мяча на основе ШИМ
|
||||
float calculatePeriod(int pwm_value) {
|
||||
float calculatePeriod(int pwm_value) {// Функция для расчета периода вылета мяча на основе ШИМ
|
||||
// Коэффициенты из аппроксимации
|
||||
float a = 100382.255;
|
||||
float b = 0.21895;
|
||||
@ -73,130 +71,12 @@ float calculatePeriod(int pwm_value) {
|
||||
return period;
|
||||
}
|
||||
|
||||
void RobotTick() {
|
||||
BallEXT_Handler();
|
||||
|
||||
// No Ball Handler
|
||||
if (currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout) {
|
||||
robotStateStop();
|
||||
setScrewkSpeed(0);
|
||||
setRollersSpeed(100, 100);
|
||||
setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def,
|
||||
infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def,
|
||||
infoBlock.hwInfo.servos[SERVO_VERTICAL].min);
|
||||
print("NO BALL!!!");
|
||||
}
|
||||
|
||||
// PreRun delay
|
||||
if (isDelayTimerRun) {
|
||||
uint32_t elapsedTime = millis() - startDelay_timer;
|
||||
|
||||
if (elapsedTime > currentInfo.startDelay) {
|
||||
isDelayTimerRun = 0;
|
||||
if (currentInfo.state == PRERUN_WAIT) {
|
||||
currentInfo.state = RUN;
|
||||
shotApply(postDelayShot);
|
||||
}
|
||||
} else {
|
||||
uint32_t intervalStep = currentInfo.startDelay / NUMLEDS;
|
||||
if (elapsedTime - lastIndicationTime >= intervalStep) {
|
||||
lastIndicationTime = elapsedTime;
|
||||
uint8_t progress = (elapsedTime * 100) / currentInfo.startDelay;
|
||||
// indicate(progress);
|
||||
print("delay: ");
|
||||
printNumber(progress);
|
||||
print("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
if (currentInfo.state != RUN) {
|
||||
print("BallDetected on idle\n");
|
||||
return;
|
||||
}
|
||||
|
||||
print("BallDetected ");
|
||||
uint16_t period = ballReact_timer - noBallTimer;
|
||||
printNumber(period);
|
||||
print("ms\n");
|
||||
noBallTimer = ballReact_timer;
|
||||
|
||||
switch (currentInfo.mode) {
|
||||
case NoneMode:
|
||||
|
||||
break;
|
||||
case ShotMode:
|
||||
if (nextBallinShot(¤tInfo.shot)) {
|
||||
stopShooting();
|
||||
}
|
||||
infoBlock.statInfo.shotsInShot++;
|
||||
saveInfoBlock();
|
||||
break;
|
||||
case ProgramMode:
|
||||
if (nextBallinShot(¤tInfo.program.currentShot)) {
|
||||
if (!loadNextShotInProgram(¤tInfo.program)) {
|
||||
stopShooting();
|
||||
print("loadNextShotInProgram ERR\n");
|
||||
}
|
||||
shotApply(getShotFromProgram(¤tInfo.program));
|
||||
}
|
||||
|
||||
if (currentInfo.program.currentBallCount + 1
|
||||
< currentInfo.program.program.header.countRepeat
|
||||
|| currentInfo.program.program.header.countRepeat == 0) {
|
||||
currentInfo.program.currentBallCount++;
|
||||
} else {
|
||||
stopShooting();
|
||||
print("Program DONE\n");
|
||||
}
|
||||
infoBlock.statInfo.shotsInProgram++;
|
||||
saveInfoBlock();
|
||||
break;
|
||||
case MacroMode:
|
||||
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
long map(long x, long in_min, long in_max, long out_min, long out_max) {
|
||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
}
|
||||
void Robot_INIT() {
|
||||
memset(¤tInfo, 0x00, sizeof(currentInfo));
|
||||
// NULL
|
||||
currentInfo.shot.index = 0xFF;
|
||||
currentInfo.program.index = 0xFF;
|
||||
currentInfo.macro.index = 0xFF;
|
||||
currentInfo.shot.indexGlobal = 0xFF;
|
||||
currentInfo.program.indexGlobal = 0xFF;
|
||||
currentInfo.macro.indexGlobal = 0xFF;
|
||||
initPCA9685();
|
||||
EEPROM_INIT();
|
||||
UART3_START();
|
||||
@ -262,147 +142,50 @@ void Robot_INIT() {
|
||||
|
||||
}
|
||||
|
||||
void shotApply(Shot *shot) {
|
||||
setPos(shot->rotationAxial, shot->rotationHorizontal,
|
||||
shot->rotationVertical);
|
||||
setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);
|
||||
setScrewkSpeed(shot->speedScrew);
|
||||
noBallTimeout = MIN(
|
||||
calculatePeriod( shot->speedScrew) * NOBALL_TIMEOUT_MULTIPLIER,
|
||||
30000);
|
||||
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");
|
||||
void BallEXT() {
|
||||
ballDetected = 1;
|
||||
ballReact_timer = millis();
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
void RobotTick() {
|
||||
BallEXT_Handler();
|
||||
|
||||
// No Ball Handler
|
||||
if (currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout) {
|
||||
robotStateStop();
|
||||
setScrewkSpeed(0);
|
||||
setRollersSpeed(100, 100);
|
||||
setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def,
|
||||
infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def,
|
||||
infoBlock.hwInfo.servos[SERVO_VERTICAL].min);
|
||||
print("NO BALL!!!");
|
||||
}
|
||||
|
||||
uint8_t overridenSpeedScrew =
|
||||
currentProg->program.shots[currentProgramShotId].speedScrew;
|
||||
if (overridenSpeedScrew != 0) {
|
||||
shot_.speedScrew = overridenSpeedScrew;
|
||||
}
|
||||
// PreRun delay
|
||||
if (isDelayTimerRun) {
|
||||
uint32_t elapsedTime = millis() - startDelay_timer;
|
||||
|
||||
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 = ¤tProg->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 ¤tProg->currentShot.shot;
|
||||
}
|
||||
|
||||
void startShooting(uint32_t delayTime) {
|
||||
print("StartShooting\n");
|
||||
switch (currentInfo.mode) {
|
||||
case ShotMode:
|
||||
if (currentInfo.shot.shot.isExist) {
|
||||
postDelayShot = ¤tInfo.shot.shot;
|
||||
if (PRE_RUN_DELAY_MODE) {
|
||||
setPosFromShot(postDelayShot);
|
||||
if (elapsedTime > currentInfo.startDelay) {
|
||||
isDelayTimerRun = 0;
|
||||
if (currentInfo.state == PRERUN_WAIT) {
|
||||
currentInfo.state = RUN;
|
||||
shotApply(postDelayShot);
|
||||
}
|
||||
robotStateStart(delayTime);
|
||||
} else {
|
||||
print("Current Shot is NULL\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
}
|
||||
break;
|
||||
case ProgramMode:
|
||||
if (currentInfo.program.program.header.shotCount) {
|
||||
if (!loadShotFromProgram(¤tInfo.program)) {
|
||||
print("loadShotFromProgram ERR\n");
|
||||
break;
|
||||
uint32_t intervalStep = currentInfo.startDelay / NUMLEDS;
|
||||
if (elapsedTime - lastIndicationTime >= intervalStep) {
|
||||
lastIndicationTime = elapsedTime;
|
||||
uint8_t progress = (elapsedTime * 100) / currentInfo.startDelay;
|
||||
// indicate(progress);
|
||||
print("delay: ");
|
||||
printNumber(progress);
|
||||
print("\n");
|
||||
}
|
||||
postDelayShot = getShotFromProgram(¤tInfo.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;
|
||||
}
|
||||
}
|
||||
|
||||
void stopShooting() {
|
||||
robotStateStop();
|
||||
setScrewkSpeed(0);
|
||||
setRollersSpeed(100, 100);
|
||||
setPosDefault();
|
||||
}
|
||||
|
||||
uint8_t prepareShot(uint8_t number) {
|
||||
Shot shot;
|
||||
getShot(number, &shot);
|
||||
@ -423,8 +206,8 @@ uint8_t prepareProgramm(uint8_t number) {
|
||||
if (program.header.shotCount) { // isExist
|
||||
currentInfo.mode = ProgramMode;
|
||||
currentInfo.program.program = program;
|
||||
currentInfo.program.index = number;
|
||||
currentInfo.program.programShotId = 0;
|
||||
currentInfo.program.indexGlobal = number;
|
||||
currentInfo.program.currentShotIndexLocal = 0;
|
||||
currentInfo.program.currentBallCount = 0;
|
||||
return 1;
|
||||
} else {
|
||||
@ -434,6 +217,343 @@ uint8_t prepareProgramm(uint8_t number) {
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t prepareMacro(uint8_t number) {
|
||||
Macro macro;
|
||||
getMacro(number, ¯o);
|
||||
if (macro.header.programmCount) {
|
||||
currentInfo.mode = MacroMode;
|
||||
currentInfo.macro.macro = macro;
|
||||
currentInfo.macro.indexGlobal = number;
|
||||
currentInfo.macro.currentProgramIndexLocal = 0;
|
||||
currentInfo.macro.currentBallCount = 0;
|
||||
|
||||
return 1;
|
||||
} else {
|
||||
print("Macro not exist ");
|
||||
printNumber(number);
|
||||
print("\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void BallEXT_Handler() {
|
||||
if (ballDetected && millis() - ballReact_timer > ballReact_value) {
|
||||
ballDetected = 0;
|
||||
|
||||
if (currentInfo.state != RUN) {
|
||||
print("BallDetected on idle\n");
|
||||
return;
|
||||
}
|
||||
|
||||
print("BallDetected ");
|
||||
uint16_t period = ballReact_timer - noBallTimer;
|
||||
printNumber(period);
|
||||
print("ms\n");
|
||||
noBallTimer = ballReact_timer;
|
||||
|
||||
switch (currentInfo.mode) {
|
||||
case NoneMode:
|
||||
|
||||
break;
|
||||
case ShotMode:
|
||||
print("ShotMode\n");
|
||||
if (nextBallInShot(¤tInfo.shot)) {
|
||||
stopShooting();
|
||||
}
|
||||
infoBlock.statInfo.shotsInShot++;
|
||||
saveInfoBlock();
|
||||
break;
|
||||
case ProgramMode:
|
||||
print("ProgramMode\n");
|
||||
if (nextShotInProgram(¤tInfo.program)) {
|
||||
stopShooting();
|
||||
}
|
||||
infoBlock.statInfo.shotsInProgram++;
|
||||
saveInfoBlock();
|
||||
break;
|
||||
case MacroMode:
|
||||
print("MacroMode\n");
|
||||
if (nextProgramInMacro(¤tInfo.macro)) {
|
||||
stopShooting();
|
||||
}
|
||||
infoBlock.statInfo.shotInMacro++;
|
||||
saveInfoBlock();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t loadShotFromProgram(CurrentProgram *currentProg) {
|
||||
Shot shot_;
|
||||
uint8_t currentShotIndexLocal = currentProg->currentShotIndexLocal;
|
||||
if (currentShotIndexLocal >= currentProg->program.header.shotCount) {
|
||||
print("Shot indexGlobal out of range\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
return 0;
|
||||
}
|
||||
uint8_t shotId = currentProg->program.shots[currentShotIndexLocal].id;
|
||||
currentProg->currentShot.indexGlobal = shotId;
|
||||
getShot(shotId, &shot_);
|
||||
if (!shot_.isExist) {
|
||||
print("Shot in program is NULL\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t overridenSpeedScrew =
|
||||
currentProg->program.shots[currentShotIndexLocal].speedScrew;
|
||||
if (overridenSpeedScrew != 0) {
|
||||
shot_.speedScrew = overridenSpeedScrew;
|
||||
}
|
||||
|
||||
uint8_t repeatCountFromShot = currentProg->program.header.options & 2U;
|
||||
if (!repeatCountFromShot) {
|
||||
print("Repeat Count Override\n");
|
||||
shot_.countRepeatShot = 1;
|
||||
}else if(shot_.countRepeatShot == 0 && currentInfo.mode != ShotMode){
|
||||
shot_.countRepeatShot = 1;
|
||||
}
|
||||
currentProg->currentShot.shot = shot_;
|
||||
currentProg->currentShot.currentBallCount = 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint8_t loadNextShotInProgram(CurrentProgram *currentProg) {
|
||||
uint8_t random = currentProg->program.header.options & 1U;
|
||||
uint8_t *currentProgramShotId = ¤tProg->currentShotIndexLocal;
|
||||
|
||||
if (random) {
|
||||
*currentProgramShotId = rand() % currentProg->program.header.shotCount;
|
||||
} else {
|
||||
if ((*currentProgramShotId) + 1
|
||||
< currentProg->program.header.shotCount) {
|
||||
++(*currentProgramShotId);
|
||||
} else {
|
||||
*currentProgramShotId = 0;
|
||||
}
|
||||
}
|
||||
return loadShotFromProgram(currentProg);
|
||||
}
|
||||
|
||||
uint8_t loadProgramFromMacro(CurrentMacro *currentMacro) {
|
||||
Program program_;
|
||||
uint8_t currentProgIndexLocal = currentMacro->currentProgramIndexLocal;
|
||||
if (currentMacro->currentProgramIndexLocal
|
||||
>= currentMacro->macro.header.programmCount) {
|
||||
print("Program indexGlobal out of range in currentMacro\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
uint8_t programId = currentMacro->macro.programs[currentProgIndexLocal].id;
|
||||
currentMacro->currentProgram.indexGlobal = programId;
|
||||
getProg(programId, &program_);
|
||||
|
||||
if (program_.header.shotCount == 0) {
|
||||
print("Program in macro is NULL\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t needOverride =
|
||||
currentMacro->macro.programs[currentProgIndexLocal].countRepeat != 0
|
||||
&& currentMacro->macro.programs[currentProgIndexLocal].speedScrew
|
||||
!= 0;
|
||||
|
||||
if (needOverride)
|
||||
{
|
||||
print("Override programs params\n");
|
||||
program_.header.countRepeat = currentMacro->macro.programs[currentProgIndexLocal].countRepeat;
|
||||
program_.header.options = currentMacro->macro.programs[currentProgIndexLocal].options;
|
||||
for (uint8_t i = 0; i < program_.header.shotCount; i++) {
|
||||
program_.shots[i].speedScrew =
|
||||
currentMacro->macro.programs[currentProgIndexLocal].speedScrew;
|
||||
}
|
||||
}
|
||||
currentMacro->currentProgram.program = program_;
|
||||
currentMacro->currentProgram.currentShotIndexLocal = 0;
|
||||
currentMacro->currentProgram.currentBallCount = 0;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint8_t loadNextProgramInMacro(CurrentMacro *currentMacro) {
|
||||
if (currentMacro->currentProgramIndexLocal + 1
|
||||
< currentMacro->macro.header.programmCount) {
|
||||
currentMacro->currentProgramIndexLocal++;
|
||||
} else {
|
||||
currentMacro->currentProgramIndexLocal = 0;
|
||||
}
|
||||
return loadProgramFromMacro(currentMacro);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t nextShotInProgram(CurrentProgram *prog) {
|
||||
if (nextBallInShot(&prog->currentShot)) {
|
||||
print("\nSwitch shot\n");
|
||||
if (!loadNextShotInProgram(prog)) {
|
||||
print("loadNextShotInProgram ERR\n");
|
||||
return 0;
|
||||
}
|
||||
shotApply(&prog->currentShot);
|
||||
}
|
||||
|
||||
// Проверка на завершение программы
|
||||
if (prog->currentBallCount + 1 < prog->program.header.countRepeat
|
||||
|| prog->program.header.countRepeat == 0) {
|
||||
prog->currentBallCount++;
|
||||
} else {
|
||||
print("Program DONE\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t nextProgramInMacro(CurrentMacro *currentMacro) {
|
||||
if (nextShotInProgram(¤tMacro->currentProgram)) {
|
||||
print("\nSwitch program\n");
|
||||
if (!loadNextProgramInMacro(currentMacro)) {
|
||||
print("loadNextProgramInMacro ERR\n");
|
||||
return 0;
|
||||
}
|
||||
// shotApply(¤tMacro->currentProgram.currentShot);
|
||||
}
|
||||
|
||||
// Проверка на завершение макро
|
||||
// if (currentMacro->currentProgramIndexLocal < currentMacro->macro.header.programmCount) {
|
||||
// } else {
|
||||
// print("Macro DONE\n");
|
||||
// return 1;
|
||||
// }
|
||||
return 0;
|
||||
}
|
||||
|
||||
void startShooting(uint32_t delayTime) {
|
||||
print("StartShooting\n");
|
||||
switch (currentInfo.mode) {
|
||||
case ShotMode:
|
||||
if (currentInfo.shot.shot.isExist) {
|
||||
postDelayShot = ¤tInfo.shot;
|
||||
if (PRE_RUN_DELAY_MODE) {
|
||||
setPosFromShot(&postDelayShot->shot);
|
||||
}
|
||||
robotStateStart(delayTime);
|
||||
} else {
|
||||
print("Current Shot is NULL\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
}
|
||||
break;
|
||||
case ProgramMode:
|
||||
if (currentInfo.program.program.header.shotCount) {
|
||||
if (!loadShotFromProgram(¤tInfo.program)) {
|
||||
print("loadShotFromProgram ERR\n");
|
||||
break;
|
||||
}
|
||||
postDelayShot = ¤tInfo.program.currentShot;
|
||||
if (PRE_RUN_DELAY_MODE) {
|
||||
setPosFromShot(&postDelayShot->shot);
|
||||
}
|
||||
robotStateStart(delayTime);
|
||||
|
||||
} else {
|
||||
print("Current Program is NULL\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
}
|
||||
break;
|
||||
case MacroMode:
|
||||
if (currentInfo.macro.macro.header.programmCount) {
|
||||
if (!loadProgramFromMacro(¤tInfo.macro)) {
|
||||
print("loadProgramFromMacro ERR\n");
|
||||
break;
|
||||
}
|
||||
if (!loadShotFromProgram(¤tInfo.macro.currentProgram)) {
|
||||
print("loadShotFromProgram in macro ERR\n");
|
||||
break;
|
||||
}
|
||||
|
||||
postDelayShot = ¤tInfo.macro.currentProgram.currentShot;
|
||||
if (PRE_RUN_DELAY_MODE) {
|
||||
setPosFromShot(&postDelayShot->shot);
|
||||
}
|
||||
robotStateStart(delayTime);
|
||||
|
||||
} else {
|
||||
print("Current Macro is NULL\n");
|
||||
// TODO: sound_ERR(); ledFX_ERR();
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void stopShooting() {
|
||||
robotStateStop();
|
||||
setScrewkSpeed(0);
|
||||
setRollersSpeed(100, 100);
|
||||
setPosDefault();
|
||||
}
|
||||
|
||||
void shotApply(CurrentShot *currentShot) {
|
||||
Shot* shot = ¤tShot->shot;
|
||||
setPos(shot->rotationAxial, shot->rotationHorizontal,
|
||||
shot->rotationVertical);
|
||||
setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);
|
||||
setScrewkSpeed(shot->speedScrew);
|
||||
noBallTimeout = MIN(
|
||||
calculatePeriod( shot->speedScrew) * NOBALL_TIMEOUT_MULTIPLIER,
|
||||
30000);
|
||||
noBallTimer = millis();
|
||||
print("Global indx ");
|
||||
printNumber(currentShot->indexGlobal);
|
||||
print("\nisExist ");
|
||||
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");
|
||||
}
|
||||
|
||||
void setPosSingle(ServoMap servo, uint8_t value) {
|
||||
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
|
||||
uint8_t inv = currentServo->invert;
|
||||
|
||||
Reference in New Issue
Block a user