Benjo
This commit is contained in:
MrEidam
5
.gitignore
vendored
Normal file
5
.gitignore
vendored
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
.pio
|
||||||
|
.vscode/.browse.c_cpp.db*
|
||||||
|
.vscode/c_cpp_properties.json
|
||||||
|
.vscode/launch.json
|
||||||
|
.vscode/ipch
|
||||||
39
include/README
Normal file
39
include/README
Normal file
@@ -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
|
||||||
28
platformio.ini
Normal file
28
platformio.ini
Normal file
@@ -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
|
||||||
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