mirror of
https://github.com/DashyFox/StackSport.git
synced 2025-05-04 07:10:17 +00:00
512 lines
12 KiB
C
512 lines
12 KiB
C
/*
|
|
* RobotFunctions.c
|
|
*
|
|
* 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"
|
|
|
|
#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;
|
|
|
|
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;
|
|
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");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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.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;
|
|
initPCA9685();
|
|
EEPROM_INIT();
|
|
UART3_START();
|
|
|
|
setPosDefault();
|
|
Shiftreg[0] = 0x00;
|
|
Shiftreg[1] = 0x44;
|
|
Shiftreg[2] = 0x00;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x00;
|
|
Shiftreg[1] = 0x66;
|
|
Shiftreg[2] = 0x00;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x00;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x00;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x01;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x01;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x03;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x03;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x07;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x07;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x0F;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x0F;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x1F;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x1F;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x3F;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x3F;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x7F;
|
|
Shiftreg[1] = 0x77;
|
|
Shiftreg[2] = 0x7F;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
Shiftreg[0] = 0x00;
|
|
Shiftreg[1] = 0x00;
|
|
Shiftreg[2] = 0x00;
|
|
SetShiftReg(Shiftreg);
|
|
HAL_Delay(10);
|
|
|
|
//testing
|
|
|
|
}
|
|
|
|
void shotApply(Shot *shot) {
|
|
setPos(shot->rotationAxial, shot->rotationHorizontal,
|
|
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 = ¤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);
|
|
}
|
|
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 = 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);
|
|
if (shot.isExist) {
|
|
currentInfo.mode = ShotMode;
|
|
currentInfo.shot.shot = shot;
|
|
currentInfo.shot.currentBallCount = 0;
|
|
return 1;
|
|
} else {
|
|
// TODO: sound_ERR(); ledFX_ERR();
|
|
print("shot not exist\n\n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
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;
|
|
if (servo == SERVO_AXIAL)
|
|
inv = !inv;
|
|
|
|
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);
|
|
}
|
|
|
|
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,
|
|
infoBlock.hwInfo.servos[SERVO_VERTICAL].def);
|
|
}
|
|
void setPosDefaultSingle(ServoMap servo) {
|
|
setPosSingle(servo, infoBlock.hwInfo.servos[servo].def);
|
|
}
|
|
|
|
// 0 .. 100
|
|
void setScrewkSpeed(uint8_t speed) {
|
|
|
|
// if(speed < 0) speed = 0;
|
|
if (speed > 100)
|
|
speed = 100;
|
|
|
|
// speed = map(speed, 0, 100, infoBlock.hwInfo.motors.speed_Screw_min, 100);
|
|
if (speed && speed < infoBlock.hwInfo.motors.speed_Screw_min)
|
|
speed = infoBlock.hwInfo.motors.speed_Screw_min;
|
|
|
|
TIM1->CCR1 = 0;
|
|
TIM1->CCR2 = (uint16_t) (40 * speed);
|
|
}
|
|
|
|
//(-v) 0 .. 100(stop) .. 200(+v)
|
|
void setRollersSpeed(uint8_t up, uint8_t down) {
|
|
|
|
if (up < 100) {
|
|
// up = map(up, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);
|
|
// if (100 - up < min_speed) {
|
|
// up = 100 - min_speed;
|
|
// }
|
|
} else {
|
|
// up = map(up, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);
|
|
// if (up - 100 < min_speed) { // Ограничиваем положительную скорость минимальной
|
|
// up = 100 + min_speed;
|
|
// }
|
|
}
|
|
|
|
if (down < 100) {
|
|
// map(down, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);
|
|
|
|
} else {
|
|
// map(down, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);
|
|
|
|
}
|
|
|
|
Vz1 = 200 - up; // invert
|
|
Vz2 = down;
|
|
}
|