Benjo
This commit is contained in:
MrEidam
734
src/main.cpp
Normal file
734
src/main.cpp
Normal file
@@ -0,0 +1,734 @@
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "RBControl.hpp"
|
||||
|
||||
#include <Arduino.h>
|
||||
//#include "./robot.hpp"
|
||||
|
||||
#include <Wire.h> // 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 <thread>
|
||||
#include <mutex>
|
||||
|
||||
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;"<<r<<';'<<g<<';'<<b<<"m \033[0m"
|
||||
<<"\033[38;2;"<<r<<';'<<g<<';'<<b<<"m "
|
||||
<</*hex(r,g,b)<<*/" "<<r<<", "<<g<<", "<<b
|
||||
<<cubeColor(maxColor())<<"\033[0m"<<std::endl;
|
||||
}
|
||||
void getRgbThread(){
|
||||
// Wire1.begin(col.SDA, col.SCL, 100000);
|
||||
// digitalWrite(col.LED, 1);
|
||||
while(true){
|
||||
if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)) {
|
||||
std::cout << "Can not connect to the RGB sensor" << std::endl;
|
||||
r = g = b = -1;
|
||||
return;
|
||||
}
|
||||
// read + send RGB
|
||||
float rr = 0, gg = 0, bb = 0;
|
||||
adatcs.getRGB(&rr, &gg, &bb);
|
||||
mtx.lock();
|
||||
|
||||
r = rr;
|
||||
g = gg;
|
||||
b = bb;
|
||||
|
||||
mtx.unlock();
|
||||
milidelay(200);
|
||||
colorPrint(r, g, b);
|
||||
}
|
||||
}
|
||||
void colorInit(int& SDA, int& SCL, int& LED){
|
||||
pinMode(SDA, PULLUP);
|
||||
pinMode(SCL, PULLUP);
|
||||
pinMode(LED, GPIO_MODE_OUTPUT);
|
||||
Wire1.begin(SDA, SCL, 100000);
|
||||
if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)){
|
||||
std::cout<<"Sensor failed to initialize\n";
|
||||
}else{
|
||||
std::cout<<"Sensor OK\n";
|
||||
}
|
||||
digitalWrite(LED, 1);
|
||||
}
|
||||
void const getRGB(int& r, int& g, int& b){
|
||||
// Wire1.begin(col.SDA, col.SCL, 100000);
|
||||
// digitalWrite(col.LED, 1);
|
||||
if(!adatcs.begin(TCS34725_ADDRESS, &Wire1)) {
|
||||
std::cout << "Can not connect to the RGB sensor" << std::endl;
|
||||
r = g = b = -1;
|
||||
return;
|
||||
}
|
||||
// read + send RGB
|
||||
float rr = 0, gg = 0, bb = 0;
|
||||
adatcs.getRGB(&rr, &gg, &bb);
|
||||
|
||||
r = rr;
|
||||
g = gg;
|
||||
b = bb;
|
||||
}
|
||||
std::string hex(int& r, int& g, int& b){
|
||||
std::string hex="0123456789ABCDEF", out="#000000";
|
||||
out[1]=hex[r>>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);
|
||||
}
|
||||
Reference in New Issue
Block a user