Pavouček na videu pracuje na upraveném firemním programu. Zachoval jsem okamžitou možnost ručního ovládání.
V programu je vložený kontrolní podprográmek který ukazuje na matricovém displeji vzdálenost v cm změřenou ultrazvukem.
Teď mě čeká instalace kamery - ESP32 CAM ale zatím nevím kde vzít napětí 5V s odběrem min. 300 mA. Budu tam muset vložit mini step-down měnič. ) Připojit to přímo na Sensor Board se mi zdá velmi odvážné. Kdoví jak to mají ti kluci z Číny popropojované. Nechci nic odpálit. Serva by měly jet na 11,1V takže vůbec netuším kde je jaké napětí
Přikládám upravený program.
Kód: Vybrat vše
/******************************************************
* servo ultrazvuk - D12
* svetlo - A3
* zvuk - A2 rozsah 0-1023 min cca 100
* PIR - D6
* infra levý - D 2 ?
* infra pravý - D 3 ?
*
* RX - D9
* TX - D8
* DISPLAY - A0 , A1
*****************************************************/
#include <Wire.h>
#include "Servo.h"
#include "WMMatrixLed.h"
#include "SoftwareSerial.h"
#include "LobotServoController.h"
#include <MPU6050.h>
#include <I2Cdev.h>
#define rxPin 9 //Serial communication interface between arduino and servo controller
#define txPin 8
#define DIN A0 //Dot matrix interface
#define CLK A1
//--------------------------------------------------------------
//#define GO_FORWARD 26 // pomaleji vpřed, střední postoj (původní 26)
#define RYCHLE 30 // rychle vpřed, střední postoj
#define GO_BACK 27 // pomalu kráčet vzad, střední postoj
#define DOLEVA 32 // rychle doleva, střední postoj
#define DOPRAVA 33 // rychle doprava, střední postoj
#define LEHNOUT 13 // čapnutí a čekat na PIR
//#define NAHORU 34
#define POMALU 26 // nejpomaleji vpřed
#define PRESUN_DOLEVA 7
#define PRESUN_DOPRAVA 8
//---------------------------------------------------------------
#define MIN_DISTANCE_TURN 350 /*Distance avoiding obstacle.The robot avoid obstacle when the obstacle avoidance is less than the set distance*/
#define DALEKO // vzdálenost větší než 800-1000
#define BIAS 0 /*Servo deviation.Adjust the data according to the actual situation to make the ultrasound face straight forward*/
#define I2C_ADDR 0x77
//Rigister
#define DISDENCE_L 0 //Low 8 bytes of distance, unit mm
#define DISDENCE_H 1
#define RGB_BRIGHTNESS 50 //0-255
#define RGB_WORK_MODE 2 //RGB mode, 0: user-defined mode 1: breathing light mode default 0
#define RGB1_R 3//R value of No.1 light，0~255，Default is 0
#define RGB1_G 4//Default is 0
#define RGB1_B 5//Default is 255
#define RGB2_R 6//R value of No.2 light，0~255，Default is 0
#define RGB2_G 7//Default is 0
#define RGB2_B 8//Default is 255
#define RGB1_R_BREATHING_CYCLE 9 //In the breathing light mode, the breathing cycle of the R of the No. 1light, the unit is 100ms and the default is 0.
//If the period is set to 3000ms, the value is 30
#define RGB1_G_BREATHING_CYCLE 10
#define RGB1_B_BREATHING_CYCLE 11
#define RGB2_R_BREATHING_CYCLE 12//RGB 2
#define RGB2_G_BREATHING_CYCLE 13
#define RGB2_B_BREATHING_CYCLE 14
#define RGB_WORK_SIMPLE_MODE 0
#define RGB_WORK_BREATHING_MODE 1
//*************************************Gyroskop ********************************************
const float pi = 3.141592;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float x, y, z;
byte angle_x, angle_y, angle_z, _angle_x, _angle_y, _angle_z;
long ax_p, ay_p, az_p;
//*******************************************************************************************
int svetlo;
int pocitadlo = 0;
Servo sonarServo; //Examples of ultrasonic servo control
SoftwareSerial MySerial(rxPin, txPin); //Software serial port instantiation
WMMatrixLed matrixLed(CLK, DIN); //Instantiated dot matrix
LobotServoController Controller(MySerial); //Instantiated secondary development
MPU6050 accelgyro;
bool WireWriteByte(uint8_t val)
{
Wire.beginTransmission(I2C_ADDR);
Wire.write(val);
if( Wire.endTransmission() != 0 ) {
return false;
}
return true;
}
bool WireWriteDataArray( uint8_t reg,uint8_t *val,unsigned int len)
{
unsigned int i;
Wire.beginTransmission(I2C_ADDR);
Wire.write(reg);
for(i = 0; i < len; i++) {
Wire.write(val[i]);
}
if( Wire.endTransmission() != 0 ) {
return false;
}
return true;
}
bool WireReadDataByte(uint8_t reg, uint8_t &val)
{
if (!WireWriteByte(reg)) {
return false;
}
Wire.requestFrom(I2C_ADDR, 1);
while (Wire.available()) {
val = Wire.read();
}
return true;
}
int WireReadDataArray( uint8_t reg, uint8_t *val, unsigned int len)
{
unsigned char i = 0;
/* Indicate which register we want to read from */
if (!WireWriteByte(reg)) {
return -1;
}
/* Read block data */
Wire.requestFrom(I2C_ADDR, len);
while (Wire.available()) {
if (i >= len) {
return -1;
}
val[i] = Wire.read();
i++;
}
return i;
}
int getDistance() { //čtení vzdálenosti ultrazvuku
u16 distance1, distance2, distance3;
int distance = 0;
WireReadDataArray(0,(uint8_t *)&distance1,2);
WireReadDataArray(0,(uint8_t *)&distance2,2);
WireReadDataArray(0,(uint8_t *)&distance3,2);
distance = (distance1 + distance2 + distance3)/3;
return distance; //Back to the measured distance
}
int gDistance; //Global variable, used to store the distance measured by ultrasonic at the intermediate position
int gLDistance; //Used to store the distance measured on the left side of the robot
int gRDistance; //Used to store the distance measured on the right side of the robot
//**************************************************************************************************************
void getAllDistance() // změřit vzdálenost ve všech třech směrech
{
uint8_t RGB[6];
uint8_t breathing[6];
uint8_t value;
value = RGB_WORK_SIMPLE_MODE;
WireWriteDataArray(RGB_WORK_MODE,&value,1);
RGB[0] = 0;RGB[1] = 255;RGB[2] = 255;//RGB1
RGB[3] = 0;RGB[4] = 255;RGB[5] = 255;//RGB2
WireWriteDataArray(RGB1_R,RGB,6);
sonarServo.write(90); //The ultrasonic servo rotates to 90 degrees, which is the middle position
delay(500);
delay(100);
gDistance = getDistance(); //Measure the distance and save it to the global variable face gDistance
sonarServo.write(180); //turn to 180 degrees
delay(500); //delay for waiting servo to rotate
delay(100);
gLDistance = getDistance(); //Measure the distance and save it to gLDistance
sonarServo.write(0); //Turn to 0 degree
delay(500); //delay for waiting servo to rotate
delay(100);
gRDistance = getDistance(); //Measure the distance and save it to gRDistance
sonarServo.write(90); //The ultrasonic servo rotates to 90 degrees, which is the middle position
delay(600);
}
//***************************************************************************************************************************************
//****************************************************************************************************************************************
void sonar() //logic of avoiding obstacle
{
static uint32_t timer = 0; //počáteční hodnota pro časovač
static uint8_t step_ = 0; //počáteční hodnota pro kroky step_
if (timer > millis()) //Vraťte se,pokud je nastavený čas větší než aktuální počet milisekund,pokud ne, pokračujte v následujících operacích.
return;
switch (step_) //skákání na funkce "case" podle step_
{
//-----------------------------------------------------------------------------------------------------------------
case 0: //step 0
gDistance = getDistance(); //změřit vzdálenost vpředu a uložit do gDistance
naklon (); // kontrola náklonu (pokud je pavouček nakloněný tak zastavit
if (pocitadlo >= 5) // počet kroků 5 kontrola počtu kroků (po překročení lehnout a čekat na PIR)
{
pocitadlo = 0; // vynulování počítadla kroků
PIR (); // kontrola PIR
}
if (!Controller.isRunning()) { //Počkejte, dokud nebude ukončena akční skupina
if (gDistance > 1000 ){
Controller.runActionGroup(RYCHLE, 0); //kráčet rychle vpřed 0- znamená neustále až do přerušení
pocitadlo = pocitadlo + 1;
step_ = 1; //skočit na step 1
}
else if (gDistance > 500) {
Controller.runActionGroup(POMALU, 0); //kráčet pomalu vpřed 0- znamená neustále až do přerušení
// pocitadlo = pocitadlo + 1;
step_ = 1; //skočit na step 0
}
else if (gDistance > 350 || gDistance == 0) { //Pokud je naměřená vzdálenost větší než specifikovaná vzdálenost pro vyhnutí
//se překážce (350 mm), jdi vpřed
Controller.runActionGroup(POMALU, 0); //kráčet nejpomaleji vpřed 0- znamená neustále až do přerušení
pocitadlo = pocitadlo + 1;
step_ = 1; //skočit na step 1
}
else { // Pokud je naměřená vzdálenost menší než zadaná vzdálenost pro vyhnutí se překážce
Controller.stopActionGroup(); // zastavit kráčení
step_ = 2; // skočit na step 2
timer = millis() + 500; // zjišťuje stav každých 500 ms (původní hodnota 500)
}
}
break; // ukončení podmínky pro přepínání
//---------------------------------------------------------------------------------------------------------
case 1: // step 1
naklon (); // kontrola náklonu
gDistance = getDistance(); // změřit vzdálenost vpředu a uložit do gDistance
if (gDistance < MIN_DISTANCE_TURN && gDistance > 0) {
// Pokud je naměřená vzdálenost menší než specifikovaná vzdálenost pro vyhnutí se překážce (350 mm)
// zastavte všechny akční skupiny a přejděte ke kroku 2.
Controller.stopActionGroup(); // zastavit
step_ = 2; // skočit na step 2
timer = millis() + 500; // původní hodnota 500
}
break; // ukončení kroku 1
//-----------------------------------------------------------------------------------------------------------
case 2:
if (!Controller.isRunning()) { // Počkejte, dokud nebude ukončena aktivní skupina
getAllDistance(); // změřit vzdálenost ve všech třech směrech
step_ = 3; // skočit na case 3
}
else {
timer = millis() + 300; // původní hodnota 500
}
break; // ukončení kroku 2
//----------------------------------------------------------------------------------------------------------------
case 3:
static bool lastActionIsGoBack = false; //Statická proměnná - zaznamenat, zda je poslední akce zpět
if (((gDistance > MIN_DISTANCE_TURN) || (gDistance == 0)) && lastActionIsGoBack == false) {
//Pokud je mezilehlá vzdálenost větší než zadaná vzdálenost pro vyhnutí se překážce (350 mm) a poslední akcí není ustoupit,
//vraťte se ke kroku 0. Posuzuje se, zda se poslední akce vrací zpět nebo ne, aby se zabránilo pádu programu do
//smyčky zpět-"dopředu-"dozadu-"dopředu..."
// Když je poslední krok zpět, neprovede se vpřed
step_ = 0; // návrat na krok 0
timer = millis() + 200; // původní hodnota 200
lastActionIsGoBack = false;
break;
}
if ((((gLDistance >= gRDistance) && (gLDistance > MIN_DISTANCE_TURN)) || gLDistance == 0) && gDistance > 200) {
//Když je minimální vzdálenost na levé straně měřená ultrazvukem větší než minimální vzdálenost na pravé straně
//větší než specifikovaná vzdálenost pro vyhnutí se překážce a vzdálenost naměřená uprostřed je větší než 200 mm
//Účelem detekce vzdálenosti uprostřed je vyhnout se předmětům mezi dvěma předními nohami robota, které způsobí
// že se robot nemůže otočit.
if (!Controller.isRunning()) { //Počkejte, dokud nebude ukončena aktivní skupina
naklon();
if (gLDistance > 1000){ // když je vzdálenost vlevo větší než 1000 tak boční přesun
Controller.runActionGroup(PRESUN_DOLEVA, 4);
goto presunL; // přesunout se bokem doleva a pokračovat bez otáčení
}
Controller.runActionGroup(DOLEVA, 4); // 3x zatočit doleva nízký postoj
presunL:
pocitadlo = pocitadlo + 1;
naklon (); // kontrola náklonu
lastActionIsGoBack = false; //Identifies that the last action is not back
step_ = 0;
timer = millis() + 300; // původně 300
}
}
else if ((((gRDistance > gLDistance) && (gRDistance > MIN_DISTANCE_TURN)) || gRDistance == 0) && gDistance > 200) {
// Když je minimální vzdálenost na levé straně měřená ultrazvukem větší než minimální vzdálenost na pravé straně
// větší než specifikovaná vzdálenost pro vyhnutí se překážce a vzdálenost měřená uprostřed (mezi nohama) je větší než 200 mm
if (!Controller.isRunning()) { //Počkat, dokud nebude ukončena akční skupina
if (gRDistance > 1000){ // když je vzdálenost vpravo větší než 1000 tak boční přesun
Controller.runActionGroup(PRESUN_DOPRAVA, 4);
goto presunR; // přesunout se bokem doprava a pokračovat bez otáčení
}
naklon (); // kontrola náklonu
Controller.runActionGroup(DOPRAVA, 4); // 3x zatočit doprava
presunR:
pocitadlo = pocitadlo + 1;
lastActionIsGoBack = false; //Identifies that the last action is not back
step_ = 0;
timer = millis() + 300; // původně 300
}
}
else {
Controller.runActionGroup(GO_BACK, 3); // 3x kráčet zpět
naklon (); // kontrola náklonu
pocitadlo = pocitadlo + 1;
lastActionIsGoBack = true; //poslední akce kráčení je zpět
step_ = 2; //turn to step 2
timer = millis() + 300; // původně 300
}
break;
}
}
//************************************************************************************************************************
void setup()
{
Wire.begin();
Serial.begin(9600); //Initialize serial port 0, baud rate 9600
MySerial.begin(9600); //Initialize serial port , baud rate 9600
sonarServo.attach(12); //Set the servo control io port
sonarServo.write(90);
accelgyro.initialize();
matrixLed.setColorIndex(1);
matrixLed.setBrightness(3);
matrixLed.clearScreen();
pinMode(A3, INPUT); // fotočidlo
pinMode(A2, INPUT); // zvukové čidlo
//Controller.runActionGroup(0, 1); // střední počáteční pozice 25 0 - nízký postoj , 34 - vysoký postoj
Controller.runActionGroup(LEHNOUT, 1); // původní OBRANA
Controller.stopActionGroup(); // zastavit
delay(3000); // mírné čekání
PIR(); // kontrola PIR a světla při startu
uint8_t drawBuffer[16] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
for (int i = 0; i < 16; i++) {
drawBuffer[i] = 0xff;
matrixLed.drawBitmap(0,0,16,drawBuffer);
delay(20);
}
matrixLed.clearScreen();
}
//*********************************************************************************************************************************************
void loop()
{
Controller.receiveHandle(); //Receive and process function, fetch data from the serial port receive buffer
sonar(); // hlavní vyhodnocování pohybu
int distance = getDistance();
distance = int(distance/10); //cm
matrixLed.showNum(1, float(distance)); // zobrazení hodnoty vzdálenosti na display
}
//***********************************************************************************************************************************************
void gyroskop() {
accelgyro.getAcceleration(&ax, &ay, &az); // zjistí všechny hodnoty z akcelerometru
ax_p = ax_p + ax; // sčítání potřebný počet hodnot
ay_p = ay_p + ay;
az_p = az_p + az;
x = ax_p;
y = ay_p;
z = az_p;
// vypočteme sklon a náklon [°]
angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);
angle_z = atan2(z, sqrt(square(x) + square(y)))/(pi/180);
ax_p = 0;
ay_p = 0;
az_p = 0;
}
//************************************ KONTROLA náklonu *********************************************************************
void naklon(){
uhel: // návrat z kontroly velkého náklonu
MPU6050 accelgyro;
gyroskop (); // --- základní pozice - x-y-z 2 - 0 - 85
if (angle_x > 20 && angle_x < 235 ){
goto uhel;
}
if (angle_y > 20 && angle_y < 235 ){
goto uhel;
}
}
//****************************************************************************************************************************************
void PIR () {
pocitadlo = 0;
sonarServo.write(90); //The ultrasonic servo rotates to 90 degrees
delay(600);
digitalWrite(6,HIGH);
Controller.runActionGroup(LEHNOUT, 1); // útok - 5 obrana - 13
Controller.stopActionGroup(); // zastavit
delay(2000);
while ( digitalRead(6) == LOW && analogRead(A2) >= 50){ // ----přečte PIR (zvuk)--- (---&& analogRead(A2)>200 fotocidlo)
// svetlo = analogRead (A1); // precte fotosenzor - A
//
// if (svetlo >600){
// digitalWrite (5,HIGH); } // zapnutí blesku při nedostatku světla - pin D
// else {
// digitalWrite (5,LOW); } // když je dostatek světla tak vypnout blesk
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void kroky (){
if (pocitadlo >= 5) // počet kroků 5
{
pocitadlo = 0; // vynulování počítadla kroků
delay(1000);
PIR ();
}
}
. Serva mají tah 20 kg cm a když ho držíte v ruce když zrovna "běží" tak sebou mrská jak ulovený kapr 
