Eidam/benjamin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

idk

Browse Source
This commit is contained in:
MrEidam
2026-06-06 11:58:32 +02:00
parent 580050a141
commit 03cf529fc4
1 changed files with 44 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
src/main.cpp
View File

@@ -48,6 +48,16 @@ int r, g, b;
int cubeGlobal = -1;
int mm = 0;
int redDet = 180;
int greDet = 110;
int bluDet = 100;
/*
R - 190 45 36
G - 70 117 53
B - 52 88 110
*/
int buttons[] = {33, 15};
int assOn = 4;
@@ -67,9 +77,9 @@ double fsign(double n){ return n ? n < 0 ? -1 : 1 : 0; }
inline int maxColor(){
int out;
mtx.lock();
if( r >= 120 ) out = 0;
else if( g >= 100 ) out = 1;
else if( b >= 90 ) out = 2;
if( r >= redDet ) out = 0;
else if( g >= greDet ) out = 1;
else if( b >= bluDet ) out = 2;
else out = -1;
mtx.unlock();
cubeGlobal = out;
@@ -77,20 +87,18 @@ inline int maxColor(){
};
/*
Soudruh - 186, 50, 40
Shrunk - 69, 105, 67
Jew - 73, 95, 85
Comunist - 194, 50, 46
Shrek - 75, 107, 75
Capitalistic - 65, 94, 105
return r > 170 || g > 90 || b > 80; // 150 | 90 | 95
}
Soudruh - 186, 50, 40
Shrunk - 69, 105, 67
Jew - 73, 95, 85
Comunist - 194, 50, 46
Shrek - 75, 107, 75
Capitalistic - 65, 94, 105
rot = 201, 40, 36
grune = 62, 111, 68
blau = 54, 93, 98
*/
std::string cubeColor(int num){
switch(num){
@@ -347,15 +355,16 @@ void const driveSans(int dis, int speed){
milidelay(10);
}
}
void const back(int speed, int timeout = 5){
int startL = getEncoder(MotorL);
int startR = getEncoder(MotorR);
int leftEnc = 0;
int rightEnc = 0;
int time = 0;
while(digitalRead(buttons[0]) == 1 && digitalRead(buttons[1])) {
leftEnc = getEncoder(MotorL) - startL;
rightEnc = getEncoder(MotorR) - startR;
while(digitalRead(buttons[0]) || digitalRead(buttons[1])) {
leftEnc = getEncoder(MotorL) - startL;
rightEnc = getEncoder(MotorR) - startR;
int diff = (rightEnc - leftEnc)/4;
setSpeeds(-speed + diff, -speed - diff);
time++;
@@ -369,7 +378,7 @@ void const front(int speed, int timeout = 1){
int leftEnc = 0;
int rightEnc = 0;
int time = 0;
while(digitalRead(buttons[0]) == 1 && digitalRead(buttons[1])) {
while(digitalRead(buttons[0]) || digitalRead(buttons[1])) {
leftEnc = getEncoder(MotorL) - startL;
rightEnc = getEncoder(MotorR) - startR;
int diff = (rightEnc - leftEnc)/4;
@@ -410,7 +419,7 @@ void const turn(double angle, int speed, int turnRadius = 0){
bool cubeDetect(){
maxColor();
mtx.lock();
bool result = (r >= 120 || g >= 100 || b >= 90);
bool result = (r >= redDet || g >= greDet || b >= bluDet);
mtx.unlock();
return result; // 150 | 90 | 95
}
@@ -494,10 +503,6 @@ void checkCube(){
// back(50);
}
void innitInnit(){
rb::Manager& man = rb::Manager::get();
man.install(rb::ManagerInstallFlags::MAN_DISABLE_MOTOR_FAILSAFE);
@@ -632,28 +637,33 @@ void setup(){
}
std::thread t(getRgbThread);
// pawsOpen();
//pawsOpen();
back(50, 2000);
setSpeeds(0);
while(digitalRead(assOn)){
// colorPrint(r, g, b);
// std::cout << ":D" << std::endl;
// colorPrint(r, g, b);
// std::cout << ":D" << std::endl;
//btnsPrint();
}
milidelay(500);
clockReset();
// pawsOpen();
// driveCollect(2000, 30);
back(80);
mainSS();
toysPrep();/*
back(80);
mainSS();
toysPrep();
/*
if(!stop){
back(30);
steal();
}*/
eating(60);
eating(1);
goToDen();
// while(digitalRead(buttons[0])){
@@ -731,4 +741,4 @@ void loop(){
// }
delay(500);
}
}