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

shooting process

Browse Source
This commit is contained in:
DashyFox
2024-09-22 00:19:50 +03:00
parent 6746f19cf0
commit b98846eac1
12 changed files with 393 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

224
Core/Src/RobotFunctions.c
View File

@ -4,25 +4,154 @@
* Created on: Aug 28, 2024
* Author: DashyFox
*/
#include <math.h>
#include "RobotFunctions.h"
#include "pca9685.h"
#include "UART3_Handler.h"
#include "EEPROM.h"
#include "ShiftReg.h"
#include "Print.h"
#include "SimpleTimer.h"
uint8_t isPause = 0;
uint8_t isShooting = 0;
#define ballReact_value 10
CurrentInfo currentInfo;
extern int16_t Vz1;
extern int16_t Vz2;
unsigned char Shiftreg[3];
extern InfoBlock infoBlock;
uint8_t ballDetected = 0;
uint32_t ballReact_timer = 0;
uint8_t isDelayTimerRun = 0;
uint32_t startDelay_timer;
Shot* postDelayShot = NULL;
uint32_t lastIndicationTime = 0;
uint32_t noBallTimer = 0;
uint32_t noBallTimeout = 0xFFFFFFFF;
int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min,
int16_t out_max) {
void BallEXT_Handler();
void robotStateStart(uint32_t delayTime) {
currentInfo.state = PRERUN_WAIT;
currentInfo.startDelay = delayTime;
startDelay_timer = millis();
isDelayTimerRun = 1;
}
void robotStatePause() {
currentInfo.state = PAUSE;
}
void robotStateStop() {
currentInfo.state = STOP;
}
void BallEXT() {
ballDetected = 1;
ballReact_timer = millis();
}
// Функция для расчета периода вылета мяча на основе ШИМ
float calculatePeriod(int pwm_value) {
// Коэффициенты из аппроксимации
float a = 100382.255;
float b = 0.21895;
float c = 883.456;
// Расчет периода на основе экспоненциальной формулы
float period = a * expf(-b * pwm_value) + c;
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");
}
}
}
}
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 (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
stopShooting();
print("Shot DONE\n");
}
infoBlock.statInfo.totalShots++;
break;
case ProgramMode:
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() {
@ -96,16 +225,15 @@ void Robot_INIT() {
}
void doShot(Shot *shot) {
SetServo(0, shot->rotationHorizontal);
SetServo(1, shot->rotationVertical);
SetServo(2, shot->rotationAxial);
Vz1 = shot->speedRollerTop;
Vz2 = shot->speedRollerBottom;
void shotApply(Shot *shot) {
setPos(shot->rotationAxial, shot->rotationHorizontal,
shot->rotationVertical);
setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);
setScrewkSpeed(shot->speedScrew);
noBallTimer = millis();
}
void startShooting() {
void startShooting(uint32_t delayTime) {
switch (currentInfo.mode) {
case ShotMode:
print("StartShooting\n");
@ -114,23 +242,27 @@ void startShooting() {
print("isExist ");
printNumber(currentInfo.shot.shot.isExist);
print("countRepeatShot; ");
print("\ncountRepeatShot; ");
printNumber(currentInfo.shot.shot.countRepeatShot);
print("speedRollerTop; ");
print("\nspeedRollerTop; ");
printNumber(currentInfo.shot.shot.speedRollerTop);
print("speedRollerBottom; ");
print("\nspeedRollerBottom; ");
printNumber(currentInfo.shot.shot.speedRollerBottom);
print("speedScrew; ");
print("\nspeedScrew; ");
printNumber(currentInfo.shot.shot.speedScrew);
print("rotationAxial; ");
print("\nrotationAxial; ");
printNumber(currentInfo.shot.shot.rotationAxial);
print("rotationHorizontal; ");
print("\nrotationHorizontal; ");
printNumber(currentInfo.shot.shot.rotationHorizontal);
print("rotationVertical; ");
print("\nrotationVertical; ");
printNumber(currentInfo.shot.shot.rotationVertical);
currentInfo.state.isShooting = 1;
doShot(&currentInfo.shot.shot);
print("\n\n");
postDelayShot = &currentInfo.shot.shot;
#if PRE_RUN_DELAY_MODE == 0
#elif PRE_RUN_DELAY_MODE == 1
setPosFromShot(postDelayShot);
#endif
robotStateStart(delayTime);
} else {
print("Current Shot is NULL\n");
// TODO: sound_ERR(); ledFX_ERR();
@ -148,24 +280,23 @@ void startShooting() {
}
void stopShooting() {
currentInfo.state.isPause = 0;
currentInfo.state.isShooting = 0;
robotStateStop();
setScrewkSpeed(0);
setRollersSpeed(100, 100);
setPosDefault();
}
void doShotForever(uint8_t number) {
}
uint8_t prepareShot(uint8_t number) {
Shot shot;
getShot(number, &shot);
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");
return 1;
} else {
// TODO: sound_ERR(); ledFX_ERR();
@ -175,32 +306,35 @@ uint8_t prepareShot(uint8_t number) {
}
void setPosSingle(ServoMap servo, uint8_t value) {
ServoSetting* currentServo = &infoBlock.hwInfo.servos[servo];
uint8_t inv = currentServo->invert;
if (servo == SERVO_AXIAL)
inv = !inv;
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
uint8_t inv = currentServo->invert;
if (servo == SERVO_AXIAL)
inv = !inv;
if (inv)
value = 180 - value;
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;
}
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);
SetServo(servo, value);
}
void setPos(uint8_t axial, uint8_t horizontal, uint8_t vertical) {
setPosSingle(SERVO_AXIAL, axial);
setPosSingle(SERVO_HORIZONTAL, horizontal);
setPosSingle(SERVO_VERTICAL, vertical);
}
void setPosFromShot(Shot* shot){
setPos(shot->rotationAxial, shot->rotationHorizontal, shot->rotationVertical);
}
void setPosDefault() {
setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def,
infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def,