Stránka 1 z 1

Mapování bludiště robotem

Napsal: 15 lis 2019, 15:05
od Thalorn
MoveToSquare.ino
(7.39 KiB) Staženo 149 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;
}