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