robo.ino

#define TRIGF 5
#define ECHOF 7
#define MOTORR 12
#define MOTORL 13
#define MINDIST 30
#define GRIDSIZE 10
#define LEFT 1
#define RIGHT 2

int grid[GRIDSIZE][GRIDSIZE];
int x = 0;
int y = 0;
int destX = 0;
int destY = 0;

bool found = false;

void setup(){
  Serial.begin(9600);
  pinMode(ECHOF, INPUT);
  pinMode(TRIGF, OUTPUT);
  pinMode(MOTORR, OUTPUT);
  pinMode(MOTORL, OUTPUT);
 
  digitalWrite(MOTORR, LOW);
  digitalWrite(MOTORL, LOW);

  // GPS
  randomSeed(analogRead(0));
  destX = random(0,GRIDSIZE-1);
  destY = random(0,GRIDSIZE-1);
  Serial.print("CX: ");
  Serial.println(x);
  Serial.print("CY: ");
  Serial.println(y);
  Serial.print("X: ");
  Serial.println(destX);
  Serial.print("Y: ");
  Serial.println(destY);

  // 2D MAP
  Serial.println("------------------------");
  for (int i = 0; i < GRIDSIZE; i++) {
    for (int j = 0; j < GRIDSIZE; j++) {
      if(j == destY && i == destX){
        grid[i][j] = 1;
      }
      Serial.print(grid[i][j]);
      Serial.print("|");
    }
    Serial.println();
  }
  Serial.println("------------------------");
}

void loop()
{
  digitalWrite(MOTORR, LOW);
  digitalWrite(MOTORL, LOW);

  if (found) {
    //led on
    Serial.print("Led on");
    delay(5000);
    //led off
    Serial.print("Led off");
  } else {
    int distanceF = calculateDistance(TRIGF, ECHOF);

    if (distanceF < MINDIST) {
      Serial.print("Found");
      found = true;
    } else {
      // random move
      int move = random(1, 10);
      Serial.print("MOVE:");
      Serial.println(move);
      switch (move) {
        case 1:
        case 2:
        case 3:
        case 4:
        case 5:
        case 6:
          forward(300);
          break;
        case 7:
        case 8:
          turn(LEFT, random(200, 1000));
          break;
        case 9:
        case 10:
          turn(RIGHT, random(200, 1000));
          break;
        default:
          Serial.print("default");
      }      
    }

    Serial.print("Distance F: ");
    Serial.print(distanceF);
    Serial.println(" cm");
    Serial.print("CX: ");
    Serial.println(x);
    Serial.print("CY: ");
    Serial.println(y);
    delay(500);
  }
}

void forward(int distance) {
  digitalWrite(MOTORR, LOW);
  digitalWrite(MOTORL, LOW);
  delay(500);
  digitalWrite(MOTORR, HIGH);
  digitalWrite(MOTORL, HIGH);
  delay(distance);
  digitalWrite(MOTORR, LOW);
  digitalWrite(MOTORL, LOW);
}

void turn(int direction, int degrees) {
  digitalWrite(MOTORR, LOW);
  digitalWrite(MOTORL, LOW);

  if (direction == LEFT) {
    digitalWrite(MOTORR, HIGH);
    delay(degrees);
    digitalWrite(MOTORR, LOW);
  } else if (direction == RIGHT) {
    digitalWrite(MOTORL, HIGH);
    delay(degrees);
    digitalWrite(MOTORL, LOW);
  }
}

int calculateDistance(int trig, int echo){
  int distance;
  long duration;

  digitalWrite(trig, LOW);
  delayMicroseconds(4);

  digitalWrite(trig, HIGH);
  delayMicroseconds(15);
  digitalWrite(trig, LOW);

  duration = pulseIn(echo, HIGH);
  distance = duration*(0.034/2);
  return distance;
}