DashyFox/PingPong
1
0
Fork 0
mirror of https://github.com/DashyFox/StackSport.git synced 2025-06-28 05:09:32 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

programm works

Browse Source
This commit is contained in:
DashyFox
2024-09-22 21:38:17 +03:00
parent b98846eac1
commit 5b4db505c2
6 changed files with 263 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

290
Core/Src/RobotFunctions.c
View File

@ -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,55 +61,75 @@ void BallEXT() {
ballReact_timer = millis();
}
// Функция для расчета периода вылета мяча на основе ШИМ
float calculatePeriod(int pwm_value) {
// Коэффициенты из аппроксимации
float a = 100382.255;
float b = 0.21895;
float c = 883.456;
// Коэффициенты из аппроксимации
float a = 100382.255;
float b = 0.21895;
float c = 883.456;
// Расчет периода на основе экспоненциальной формулы
float period = a * expf(-b * pwm_value) + c;
return period;
// Расчет периода на основе экспоненциальной формулы
float period = a * expf(-b * pwm_value) + c;
return period;
}
void RobotTick() {
BallEXT_Handler();
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,
// 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!!!");
}
print("NO BALL!!!");
}
// PreRun delay
if (isDelayTimerRun) {
uint32_t elapsedTime = millis() - startDelay_timer;
// 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;
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");
}
}
}
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() {
@ -122,24 +152,30 @@ void BallEXT_Handler() {
break;
case ShotMode:
if (currentInfo.shot.currentRepeatCount + 1
< currentInfo.shot.shot.countRepeatShot
|| !currentInfo.shot.shot.countRepeatShot) {
// nextRepeatCount
print("Shot ");
printNumber(currentInfo.shot.currentRepeatCount);
print("\n\n");
saveInfoBlock();
currentInfo.shot.currentRepeatCount++;
} else {
// shotDone
if (nextBallinShot(&currentInfo.shot)) {
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
#elif PRE_RUN_DELAY_MODE == 1
setPosFromShot(postDelayShot);
#endif
if (PRE_RUN_DELAY_MODE) {
setPosFromShot(postDelayShot);
}
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;
noBallTimeout = calculatePeriod(shot.speedScrew)*NOBALL_TIMEOUT_MULTIPLIER;
print("noBallTimeout: ");
printNumber(noBallTimeout);
print("\n");
currentInfo.shot.currentBallCount = 0;
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){
setPos(shot->rotationAxial, shot->rotationHorizontal, shot->rotationVertical);
void setPosFromShot(Shot *shot) {
setPos(shot->rotationAxial, shot->rotationHorizontal,
shot->rotationVertical);
}
void setPosDefault() {