commit 580050a141f72ee4c740ac0a769b9745ee3228f2 Author: MrEidam Date: Tue Jun 2 13:29:27 2026 +0200 Benjo diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/include/README b/include/README new file mode 100644 index 0000000..45496b1 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..cb015e9 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,28 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + + + +[env:esp32dev] +platform = espressif32@~1.12.4 +board = esp32dev +framework = arduino +monitor_speed = 115200 +upload_speed = 921600 +build_flags = -std=c++14 +build_unflags = -std=gnu++11 +monitor_filters = esp32_exception_decoder +# Nastav mne! +upload_port = /dev/ttyUSB0 +#upload_port = COM4 + +lib_deps = + RB3201-RBControl @ 7.1.2 + adafruit/Adafruit TCS34725@^1.4.4 diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..823b0b2 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,734 @@ +#include +#include +#include +#include + +#include "RBControl.hpp" + +#include +//#include "./robot.hpp" + +#include // must be manually included before #include "Adafruit_TCS34725.h" + +#include "Adafruit_TCS34725.h" +Adafruit_TCS34725 adatcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_1X); + +#include +#include + +std::mutex mtx; + +struct GayTector{ + int SDA; + int SCL; + int LED; +}; +struct Sanic{ + int TRIG; + int ECHO; +}; +/*Robot& get_robot(){ + static Robot robot(168, 34); + return robot; +}*/ + +struct Sack{ + int RED; + int GREEN; + int BLUE; +}; + +#define armDelay 1000 +#define baseWidth 168 +#define wheelRadius 34. +#define motorEncSpin 450. +#define mmToEnc (motorEncSpin / (2. * 3.1415926535 * wheelRadius)) + +int r, g, b; +int cubeGlobal = -1; +int mm = 0; + +int buttons[] = {33, 15}; + +int assOn = 4; + +GayTector col = {23, 22, 5}; +Sanic son = {16, 14}; +Sack backpack = {0, 0, 0}; + +constexpr rb::MotorId MotorR = rb::MotorId::M6; +constexpr rb::MotorId MotorL = rb::MotorId::M1; + +bool cubeDetect(); +inline int clampSpeed(int speed){return speed < -100 ? -100 : speed > 100 ? 100 : speed;} +void milidelay(int ms){std::this_thread::sleep_for(std::chrono::milliseconds(ms));} +inline float lerp(float a, float b, float t){return a + t * (b - a);} +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; + else out = -1; + mtx.unlock(); + cubeGlobal = out; + return out; +}; + +/* + + + 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 +*/ +std::string cubeColor(int num){ + switch(num){ + case 0: + return " RED"; + case 1: + return " GREEN"; + case 2: + return " BLUE"; + default: + return " \e[31mERROR\e[0m"; + } +} + +int timeStart = 0; +//# CLOCK (the L is silent) +void clockReset(){ + timeStart = millis(); +} +int getTime(){ + return millis() - timeStart; +} + +//# Gaydar +void colorPrint(int& r, int& g, int& b){ + if(r!=-1 && g!=-1 && b!=-1) + std::cout<<"\033[48;2;"<>4]; out[2]=hex[r&15]; + out[3]=hex[g>>4]; out[4]=hex[g&15]; + out[5]=hex[b>>4]; out[6]=hex[b&15]; + return out; +} + +//# Sanic +constexpr double SOUND_SPEED = 0.34; // mm / uS +double const getDistance(int trig, int echo) { + digitalWrite(trig, LOW); // reset + delayMicroseconds(2); + digitalWrite(trig, HIGH); // send pulse + delayMicroseconds(10); + digitalWrite(trig, LOW); + int duration = pulseIn(echo, HIGH); // get duration + double distance = duration * SOUND_SPEED / 2; + return distance; +} + +//# Butt +void btnsInit(){ + for(auto& b : buttons) + pinMode(b, INPUT_PULLUP); +} +void btnsPrint(){ + if(!digitalRead(buttons[0])){ + std::cout << "Right Button" << std::endl; + } + if(!digitalRead(buttons[1])){ + std::cout << "Left Button" << std::endl; + } +} +bool btns(){ + return (!digitalRead(buttons[0]) && !digitalRead(buttons[1])); +} + +//# Paws +/* + # Beans + *Štartýren - 140 + *Opium - 115 + *Copium - 150 +*/ +void pawArmInit(){ + rb::Manager& man = rb::Manager::get(); + man.initSmartServoBus(2, GPIO_NUM_32); + rb::Angle a0 = rb::Angle::deg(40); + man.servoBus().set(0, a0); + rb::Angle a1 = rb::Angle::deg(140); + man.servoBus().set(1, a1); + delay(armDelay); +} +void armDown(){ + rb::Manager& man = rb::Manager::get(); + rb::Angle ang = rb::Angle::deg(40); + man.servoBus().set(0, ang); + delay(armDelay); +} +void armUp(){ + rb::Manager& man = rb::Manager::get(); + rb::Angle ang = rb::Angle::deg(69); + man.servoBus().set(0, ang); + delay(armDelay); +} +void pawsOpen(){ + rb::Manager& man = rb::Manager::get(); + rb::Angle ang = rb::Angle::deg(115); + man.servoBus().set(1, ang); + delay(armDelay); +} +void pawsClose(){ + rb::Manager& man = rb::Manager::get(); + rb::Angle ang = rb::Angle::deg(150); + man.servoBus().set(1, ang); + delay(armDelay); +} +void pawArmAutoStop(){ + rb::Manager& man = rb::Manager::get(); + man.servoBus().setAutoStop(0); + man.servoBus().setAutoStop(1); +} + +//# Špinny Špin +int getEncoder(rb::MotorId id){ + rb::Manager& man = rb::Manager::get(); + switch(id){ + case MotorL: return (man.motor(MotorL).encoder()->value()); + case MotorR: return (-man.motor(MotorR).encoder()->value()); + default: + std::cout << "motor not found" << std::endl; + return 0; + } +} +void const setSpeeds(int speed){ + rb::Manager& man = rb::Manager::get(); + man.motor(MotorL).power(speed); + man.motor(MotorR).power(-speed); +} +void const setSpeeds(int speedL, int speedR){ + rb::Manager& man = rb::Manager::get(); + man.motor(MotorL).power(clampSpeed(speedL)); + man.motor(MotorR).power(clampSpeed(-speedR)); +} +void const drive(int dis, int speed){ + int startL = getEncoder(MotorL); + int startR = getEncoder(MotorR); + int leftEnc = 0; + int rightEnc = 0; + while(abs(dis * mmToEnc) > abs(leftEnc)) { + leftEnc = getEncoder(MotorL) - startL; + rightEnc = getEncoder(MotorR) - startR; + int diff = (rightEnc - leftEnc);// /4; + setSpeeds(speed + diff, speed - diff); + + milidelay(10); + } + setSpeeds(0); +} +void const driveCollect(int dis, int speed){ + int startL = getEncoder(MotorL); + int startR = getEncoder(MotorR); + int leftEnc = 0; + int rightEnc = 0; + mm = 0; + while(mm < dis && !cubeDetect()) { + leftEnc = getEncoder(MotorL) - startL; + rightEnc = getEncoder(MotorR) - startR; + int diff = (rightEnc - leftEnc);// /4; + setSpeeds(speed + diff, speed - diff); + mm = abs(leftEnc) / mmToEnc; + milidelay(10); + } + setSpeeds(0); + drive(20, speed); + milidelay(300); + pawsClose(); + milidelay(333); + armUp(); + milidelay(333); + drive(mm, -speed); + milidelay(333); +} +void const driveFancy(int dis, int startSpeed, int endSpeed){ + int startL = getEncoder(MotorL); + int startR = getEncoder(MotorR); + int leftEnc = 0; + int rightEnc = 0; + int speed = startSpeed; + while(abs(dis * mmToEnc) > abs(leftEnc)) { + leftEnc = getEncoder(MotorL) - startL; + rightEnc = getEncoder(MotorR) - startR; + int diff = (rightEnc - leftEnc)/4; + speed = lerp(startSpeed, endSpeed, abs(leftEnc) / (dis * mmToEnc)); + setSpeeds(speed + diff, speed - diff); + + milidelay(10); + } + setSpeeds(0); +} +void const driveSans(int dis, int speed){ + int startL = getEncoder(MotorL); + int startR = getEncoder(MotorR); + int leftEnc = 0; + int rightEnc = 0; + while(getDistance(son.TRIG, son.ECHO)>dis){ + leftEnc = getEncoder(MotorL) - startL; + rightEnc = getEncoder(MotorR) - startR; + int diff = (rightEnc - leftEnc)/4; + setSpeeds(speed + diff, +speed - diff); + 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; + int diff = (rightEnc - leftEnc)/4; + setSpeeds(-speed + diff, -speed - diff); + time++; + if(time > timeout*100) break; + milidelay(10); + } +} +void const front(int speed, int timeout = 1){ + 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; + int diff = (rightEnc - leftEnc)/4; + setSpeeds(speed + diff, speed - diff); + time++; + if(time > timeout*100) break; + milidelay(10); + } +} +void const turn(double angle, int speed, int turnRadius = 0){ + angle = angle * PI / 180; + int startL = getEncoder(MotorL); + int startR = getEncoder(MotorR); + double disl = (turnRadius + fsign(angle) * baseWidth / 2.) * fabs(angle); + double disr = (turnRadius - fsign(angle) * baseWidth / 2.) * fabs(angle); + + //std::cout << "dis " << disl * mmToEnc << " " << disr * mmToEnc << std::endl; + //std::cout << "angle " << angle << " " << fsign(angle) << std::endl; + + int leftEnc = 0; + int rightEnc = 0; + while(true){ + leftEnc = getEncoder(MotorL) - startL; + rightEnc = getEncoder(MotorR) - startR; + // std::cout << "dis " << disl * mmToEnc << " " << disr * mmToEnc << std::endl; + // std::cout << "enc " << leftEnc << " " << rightEnc << std::endl; + double percl = fabs((double)leftEnc / (disl * mmToEnc)); + double percr = fabs((double)rightEnc / (disr * mmToEnc)); + double diff = (percl - percr) * 20; + setSpeeds(fsign(disl) * speed * (1-diff), fsign(disr) * speed * (1+diff)); + if(abs(disl * mmToEnc) <= abs(leftEnc)) break; + milidelay(10); + } + setSpeeds(0); +} + +//# CUBE +bool cubeDetect(){ + maxColor(); + mtx.lock(); + bool result = (r >= 120 || g >= 100 || b >= 90); + mtx.unlock(); + return result; // 150 | 90 | 95 +} +void sortCubeDeposit(){ + turn(-90, 50); + front(50, 2); + pawsOpen(); + milidelay(200); + drive(200, -60); + pawsClose(); + milidelay(200); + armDown(); +} +void sortCubeRed(){ + driveFancy(425, 40, 90); + driveFancy(425, 90, 40); + drive(45*(backpack.RED%3), 40); + sortCubeDeposit(); + turn(-90, 50); + milidelay(100); + back(50); + milidelay(300); + drive(200, 30); + milidelay(300); + turn(180, 30); + milidelay(300); + back(50, 7); + milidelay(333); + backpack.RED++; +} +void sortCubeGreen(){ + driveFancy(200, 40, 90); + driveFancy(200, 90, 40); + drive(45*(backpack.GREEN%3), 40); + sortCubeDeposit(); + turn(90, 50); + milidelay(300); + back(50); + milidelay(300); + backpack.GREEN++; +} +void sortCubeBlue(){ + drive(45*(backpack.BLUE%3), 40); + sortCubeDeposit(); + turn(90, 50); + back(50); + milidelay(300); + backpack.BLUE++; +} +void sortCube(int cube){ + armUp(); + switch(cube){ + case 0: + sortCubeRed(); + break; + case 1: + sortCubeGreen(); + break; + case 2: + sortCubeBlue(); + break; + default: + armDown(); + break; + } +} +void checkCube(){ + armDown(); + milidelay(200); + pawsOpen(); + milidelay(200); + drive(100, 30); + milidelay(200); + pawsClose(); + + // cubeGlobal = maxColor(); + // if(cubeGlobal != -1) + sortCube(cubeGlobal); + cubeGlobal = -1; + // else + // back(50); +} + + + + + +void innitInnit(){ + rb::Manager& man = rb::Manager::get(); + man.install(rb::ManagerInstallFlags::MAN_DISABLE_MOTOR_FAILSAFE); + + pinMode(assOn, INPUT); + + // Sanic + pinMode(son.TRIG, OUTPUT); + pinMode(son.ECHO, INPUT); + + // Gaydar + colorInit(col.SDA, col.SCL, col.LED); + + // Butt + btnsInit(); + + // Paw + Arm + pawArmInit(); +} + +void mainSS(){ + driveFancy(650, 40, 90); + turn(-180, 90, 250); + turn(180, 90, 250); + driveFancy(500, 90, 60); + driveFancy(200, 60, 40); + milidelay(350); + turn(45, 30); + milidelay(300); + turn(-90, 30); + milidelay(300); + turn(45, 30); + milidelay(300); +} +void toysPrep(){ + pawsOpen(); + milidelay(300); + driveFancy(550, 50, 90); + if(cubeDetect()) + pawsClose(); + + turn(45, 30); + milidelay(325); + turn(-135, 30); + milidelay(325); + turn(90, 30); + milidelay(325); + + driveFancy(250, 50, 90); + milidelay(325); + + if(cubeDetect()) + pawsClose(); + + armUp(); + front(80, 2); + milidelay(300); + drive(200, -70); + milidelay(300); + turn(90, 30); + milidelay(300); + back(70); + if(cubeDetect()) checkCube(); + armDown(); +} +void eating(int cube){ + for(int i = 0; i < cube; i++){ + int dis = 350 + 88 * (i%8); + driveFancy(dis, 40, 90); + turn(90, 30); + milidelay(200); + + pawsOpen(); + armDown(); + milidelay(200); + driveCollect(750, 40); + + turn(-90, 30); + milidelay(200); + back(50); + milidelay(200); + if(getTime() > 4.2 * 60 * 1000){ // min * sec * mil (in mills) + i = cube; + continue; + } + checkCube(); + if(getTime() > 4.2 * 60 * 1000){ // min * sec * mil (in mills) + i = cube; + continue; + } + } +} +void goToDen(){ + drive(200, 40); + turn(90, 50); + milidelay(500); + driveFancy(450, 50, 90); + // drive(1600, 90); + drive(950, 90); + turn(-90, 30); + milidelay(300); + back(50); + + drive(200, 40); + turn(90, 50); + milidelay(500); + drive(650, 90); + milidelay(450); + turn(-90, 30); + milidelay(450); + back(80); + milidelay(500); + drive(500, 80); + turn(-90, 80, 230); + drive(100, 100); + turn(180, 90, 250); + drive(10000, 100); +} + + + + +//Serial.begin (115200); +void setup(){ + // rb::Manager& man = rb::Manager::get(); + // man.initSmartServoBus(2, GPIO_NUM_32); + innitInnit(); + if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)){ + std::cout<<"Sensor failed to initialize\n"; + }else{ + std::cout<<"Sensor OK\n"; + } + std::thread t(getRgbThread); + + // pawsOpen(); + + while(digitalRead(assOn)){ + // colorPrint(r, g, b); + // std::cout << ":D" << std::endl; + } + + milidelay(500); + + clockReset(); + + // pawsOpen(); + // driveCollect(2000, 30); + + back(80); + mainSS(); + toysPrep();/* + if(!stop){ + back(30); + steal(); + }*/ + eating(60); + goToDen(); + + // while(digitalRead(buttons[0])){ + + // colorPrint(r, g, b); + // delay(500); + // } + + + //milidelay(200); + // turn(45, 50); + // milidelay(200); + // turn(-90, 50); + // milidelay(200); + // turn(45, 50); + // milidelay(200); + + //armUp(); + //pawsOpen(); + //setSpeeds(50); + //delay(2000); + //setSpeeds(0); + //driveFancy(300, 20, 100); + //back(50, 99999); + //turn(90, 50, 0); + + t.detach(); +} + +void loop(){ + // std::thread t(getRgbThread); + + //rb::Manager& man = rb::Manager::get(); + // man.install(rb::ManagerInstallFlags::MAN_DISABLE_MOTOR_FAILSAFE); +/* + armDown(); + pawsClose(); + armUp(); + pawsOpen();*/ + + // mtx.lock(); + // colorPrint(r, g, b); + // mtx.unlock(); + if(!digitalRead(buttons[0])){ + armUp(); + }else{ + armDown(); + } + if(!digitalRead(buttons[1])){ + pawsOpen(); + }else{ + pawsClose(); + } + + // delay(1); + // btnsPrint(); + // std::cout << "Angle: " << man.servoBus().pos(1).deg() << std::endl; + + // std::cout << man.servoBus().pos(0).deg() << std::endl; + // std::cout << man.servoBus().pos(1).deg() << std::endl; + // std::cout << getDistance(son.TRIG, son.ECHO) << " mm" << std::endl; + // getRGB(r, g, b); + // colorPrint(r, g, b); + /*printf("GetId: %d %f\n", 0, servoBus.posOffline(0).deg()); + printf("GetId: %d %f\n", 1, servoBus.posOffline(1).deg()); +*/ + // for(int i = 0; i < 2; ++i){ + // try{ + // float deg = man.servoBus().pos(i).deg(); + // std::cout << i << ": " << deg << "\t"; + // }catch(...){ + // std::cout << "Servo " << i << " not responding" << std::endl; + // } + // if(i == 1) std::cout << std::endl; + // } + + delay(500); +} \ No newline at end of file