Mapování bludiště robotem
Napsal: 15 lis 2019, 15:05
ale zasekl jsem se na ukládání souřadnic jednotlivých čtverců do pole a celkově na logice celého programu. Momentálně program postupuje tak, že jakmile robot přejede do nového čtverce nepřestane zapisovat souřadnice a začne se otáčet na místě. Potřeboval bych poradit, jak by se dala zavolat funkce Obstacles() pouze jednou za čtverec a jak správně upravit funkci DecideWhereNext().
NOTE: Robot nemá žádné enkodéry pouze gyroskop a ultrazvukové senzory (neptejte se proč, rozhodnutí školy). Bludiště je veliké 5x5 a jeden čtverec má 30cm. V code tagu nejsou funkce pro otáčení, ale v příloze je celý kód.
Kód: Vybrat vše
#include <AFMotor.h>
#include <elapsedMillis.h>
#include <NewPing.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#define SONAR_NUM 2
#define TRIGGER_PIN 45
#define ECHO_PIN 44
#define L_TRIG_PIN 36
#define L_ECHO_PIN 37
#define R_TRIG_PIN 30
#define R_ECHO_PIN 31
#define MAX_DISTANCE 200
#define eps 5.0
#define PING_INTERVAL 250
unsigned long pingTimer[SONAR_NUM];
unsigned int cm[SONAR_NUM];
uint8_t currentSensor = 0;
MPU6050 mpu6050(Wire);
AF_DCMotor motorL(3);
AF_DCMotor motorR(4);
NewPing sonar[SONAR_NUM] = {
NewPing(R_TRIG_PIN, R_ECHO_PIN, MAX_DISTANCE),
NewPing(L_TRIG_PIN, L_ECHO_PIN, MAX_DISTANCE)
};
NewPing front(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
long timer = 0;
bool WallRight, WallLeft, WallFront, Turned180, TurnedLeft, TurnedRight;
int Distance = 0, modulo;
int Direction = 2; // Direction 1: front, 2: right, 3: back, 4: left
int x = 0;
int y = 0;
int dx = 1;
int dy = 0;
float Angle, NewAngle;
long TimerFront = 0;
struct Array{
bool Left;
bool Right;
bool Front;
bool Back;
int StrucDirection;
};
Array Map[5][5];
void setup() {
Serial.begin(115200);
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true);
motorL.setSpeed(85);
motorR.setSpeed(80);
pingTimer[0] = millis() + 75;
for (uint8_t i = 1; i < SONAR_NUM; i++){ // Set the starting time for each sensor.
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}
Map[0][0].Left = true;
Map[0][0].Right = true;
Map[0][0].Front = false;
Map[0][0].Back = true;
Map[0][0].StrucDirection = 2;
}
void loop() {
if(millis() - TimerFront > 50){
Distance = front.ping_cm();
TimerFront = millis();
}
Forward();
if(Distance > 13) WallFront = false;
else WallFront = true;
modulo = Distance%30;
if ((modulo >= 7)&&(modulo <= 9)) {
Stop();
SetDirection();
Obstacles();
return;
}
DecideWhereNext();
}
void Obstacles(){
for (uint8_t i = 0; i < SONAR_NUM; i++) {
if (millis() >= pingTimer[i]) {
pingTimer[i] += PING_INTERVAL * SONAR_NUM;
sonar[currentSensor].timer_stop();
currentSensor = i;
cm[currentSensor] = 0;
sonar[currentSensor].ping_timer(echoCheck);
}
}
}
void echoCheck() {
if (sonar[currentSensor].check_timer())
{
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
pingResult(currentSensor);
}
}
void pingResult(uint8_t sensor) {
if(cm[0] > 13)
{
WallRight == false;
Map[x][y].Right = false;
Serial.println("No obstacle in right");
}
else if (cm[0] <= 13)
{
WallRight == true;
Map[x][y].Right = true;
Serial.println("Obstacle on right");
}
else if (cm[1] > 13)
{
WallLeft == false;
Map[x][y].Left = false;
Serial.println("No obstacle in left");
}
else if (cm[1] <= 13)
{
WallLeft == true;
Map[x][y].Left = true;
}
else if(WallFront == true)
{
Map[x][y].Front = true;
}
else if (!Map[x][y].Front)
{
Map[x][y].Back = false;
}
Map[x][y].StrucDirection = Direction;
Serial.print(" x:");
Serial.print(x);
Serial.print(" y:");
Serial.print(y);
Serial.print(" ");
Serial.print(Map[x][y].Left);
Serial.print(" ");
Serial.print(Map[x][y].Right);
Serial.print(" ");
Serial.print(Map[x][y].Front);
Serial.print(" ");
Serial.print(Map[x][y].Back);
Serial.print(" ");
Serial.print(Map[x][y].StrucDirection);
Serial.print(" ");
}
void DecideWhereNext(){
if(!Map[x][y].Front && Map[x][y].Left && Map[x][y].Right )
{
Forward();
Serial.println("Forward");
}
else if(!Map[x][y].Front && !Map[x][y].Front && WallRight)
{
Forward();
Serial.println("Forward");
}
else if(!Map[x][y].Front && !Map[x][y].Right)
{
TurnRight();
Serial.println("Right");
}
else if(Map[x][y].Front && Map[x][y].Right && !Map[x][y].Left)
{
TurnLeft();
Serial.println("Left");
}
else if (Map[x][y].Front && Map[x][y].Right && !Map[x][y].Left)
{
Turn180();
Serial.println("Turning 180");
}
}
void SetDirection(){
if(TurnedLeft){
if(dy == 1)
{
dx=-1;
dy=0;
Direction = 4;
}
else if(dy == -1)
{
dx=1;
dy=0;
Direction = 2;
}
else if(dx == 1)
{
dx=0;
dy=1;
Direction = 1;
}
else
{
dx=0;
dy=-1;
Direction = 3;
}
}
else if(TurnedRight)
{
if(dy == 1)
{
dx = 1;
dy = 0;
Direction = 2;
}
else if(dy == -1)
{
dx = -1;
dy = 0;
Direction = 4;
}
else if(dx == -1)
{
dx = 0;
dy = 1;
Direction = 1;
}
else
{
dx = 0;
dy = -1;
Direction = 3;
}
}
else if (Turned180)
{
if (dy == 1)
{
dx = 0;
dy = -1;
Direction = 3;
}
else if (dy == -1)
{
dx = 0;
dy = 1;
Direction = 1;
}
else if (dx == 1)
{
dx = -1;
dy = 0;
Direction = 4;
}
else if (dx == -1)
{
dx = 1;
dy = 0;
Direction = 2;
}
}
x = x + dx;
y = y + dy;
}