idk

src/main.cpp

@@ -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(){
 };
 
 /*
-
-
-	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,13 +355,14 @@ 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])) {
+	while(digitalRead(buttons[0]) || digitalRead(buttons[1])) {
 	        leftEnc = getEncoder(MotorL) - startL;
                 rightEnc = getEncoder(MotorR) - startR;
 		int diff = (rightEnc - leftEnc)/4;
@@ -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,11 +637,15 @@ 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;
+	    //btnsPrint();
 	}
 	
 	milidelay(500);
@@ -648,12 +657,13 @@ void setup(){
 
         back(80);
         mainSS();
-	toysPrep();/*
+        toysPrep();
+        /*
 	if(!stop){
 		back(30);
 		steal();
 	}*/
-	eating(60);
+	eating(1);
 	goToDen();
 
 	// while(digitalRead(buttons[0])){