Mapování bludiště robotem

Nedaří se vám s projektem a nenašli jste vhodné místo, kde se zeptat? Napište sem.
Pravidla fóra
Tohle subfórum je určeno pro konzultaci ucelených nápadů, popřípadě řešení komplexnějších projektů, které opravdu není možné rozdělit na menší části.
Většinu problémů jde rozdělit na menší a ptát se na ně v konkrétních subfórech.
Odpovědět
Thalorn
Příspěvky: 3
Registrován: 21 bře 2019, 21:39
Reputation: 0

Mapování bludiště robotem

Příspěvek od Thalorn » 15 lis 2019, 15:05

MoveToSquare.ino
(7.39 KiB) Staženo 138 x
Ahoj, potřeboval bych poradit. Pracuji na školním projektu kde robot musí projet bludiště na čtvercové síti a vykreslit jeho mapu na monitoru,
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;
}

Odpovědět

Kdo je online

Uživatelé prohlížející si toto fórum: Žádní registrovaní uživatelé a 20 hostů