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

window clip variant

Browse Source
This commit is contained in:
DashyFox
2024-09-17 23:01:04 +03:00
parent 2ef50d8b07
commit 953593d32a
9 changed files with 842 additions and 646 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
Core/Src/RobotFunctions.c
View File

@ -6,7 +6,10 @@
*/
#include "RobotFunctions.h"
#include "pca9685.h"
#include "UART3_Handler.h"
#include "EEPROM.h"
#include "ShiftReg.h"
#include "Print.h"
uint8_t isPause = 0;
@ -14,19 +17,84 @@ uint8_t isShooting = 0;
CurrentInfo currentInfo;
HardwareInit_t hwSettings = {
/*DelayTimes*/{
/*preRun*/0 },
/*ServoSetting*/{ { 0, 0, 90, 180 }, { 0, 0, 90, 180 }, { 0, 0, 90, 180 } },
/*Motors*/{ 0, 0 } };
extern int16_t Vz1;
extern int16_t Vz2;
unsigned char Shiftreg[3];
int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min,
int16_t out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void Robot_INIT() {
memset(&currentInfo, 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 doShot(Shot *shot) {
SetServo(0, shot->rotationHorizontal);
@ -106,21 +174,45 @@ uint8_t prepareShot(uint8_t number) {
}
void setPosSingle(ServoMap servo, uint8_t value) {
if (hwSettings.servos[servo].invert)
value = 180 - value;
SetServo(servo, value);
ServoSetting* currentServo = &infoBlock.hwInfo.servos[servo];
uint8_t inv = currentServo->invert;
if (servo == SERVO_AXIAL)
inv = !inv;
uint8_t deviationToMax = currentServo->max > currentServo->def
? currentServo->max - currentServo->def
: currentServo->def - currentServo->max;
uint8_t deviationToMin = currentServo->def > currentServo->min
? currentServo->def - currentServo->min
: currentServo->min - currentServo->def;
uint8_t maxDeviation = (deviationToMax > deviationToMin) ? deviationToMax : deviationToMin;
uint8_t minLimit = (currentServo->def >= maxDeviation) ? currentServo->def - maxDeviation : 0;
uint8_t maxLimit = (currentServo->def + maxDeviation <= 180) ? currentServo->def + maxDeviation : 180;
if (value > maxLimit) {
value = maxLimit;
} else if (value < minLimit) {
value = minLimit;
}
if (inv)
value = 180 - 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 setPosDefault() {
setPos(
hwSettings.servos[SERVO_AXIAL].def,
hwSettings.servos[SERVO_HORIZONTAL].def,
hwSettings.servos[SERVO_VERTICAL].def
);
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
@ -130,7 +222,9 @@ void setScrewkSpeed(uint8_t speed) {
if (speed > 100)
speed = 100;
speed = map(speed, 0, 100, hwSettings.motors.speed_Screw_min, 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);
@ -140,15 +234,23 @@ void setScrewkSpeed(uint8_t speed) {
void setRollersSpeed(uint8_t up, uint8_t down) {
if (up < 100) {
up = map(up, 0, 100, 0, 100 - hwSettings.motors.speed_Screw_min);
// 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 + hwSettings.motors.speed_Screw_min);
// 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 - hwSettings.motors.speed_Screw_min);
// map(down, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);
} else {
map(down, 0, 100, 0, 100 + hwSettings.motors.speed_Screw_min);
// map(down, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);
}
Vz1 = 200 - up; // invert