semihhhh.ino

#include <xmotion.h>

typedef char mazeBlock;

#define DIRECTION_UP 1
#define DIRECTION_DOWN 2
#define DIRECTION_RIGHT 4
#define DIRECTION_LEFT 8

#define MAZE_DISCOVERED 16
#define MAZE_WALLED 32

struct tilePosition {
  int x;
  int y;
  struct tilePosition& operator+=(struct tilePosition k) { x += k.x; y += k.y; return *this; }
};

char robotHeading = DIRECTION_UP;

struct tilePosition robotPosition;


struct tilePosition operator+( struct tilePosition lhs, struct tilePosition const& rhs ) {
  struct tilePosition t;
  t.x = lhs.x + rhs.x;
  t.y = lhs.x + rhs.y;
  return t;
}


char rotateLeft(char direction) {
  switch(direction) {
    case DIRECTION_UP:
      return DIRECTION_LEFT;
    case DIRECTION_DOWN:
      return DIRECTION_RIGHT;
    case DIRECTION_LEFT:
      return DIRECTION_DOWN;
    case DIRECTION_RIGHT:
      return DIRECTION_UP;
  }
  return 0;
}

char rotateRight(char direction) {
  switch(direction) {
    case DIRECTION_UP:
      return DIRECTION_RIGHT;
    case DIRECTION_DOWN:
      return DIRECTION_LEFT;
    case DIRECTION_LEFT:
      return DIRECTION_UP;
    case DIRECTION_RIGHT:
      return DIRECTION_DOWN;
  }
  return 0;
}

struct tilePosition dirToTile(char dir) {
  struct tilePosition t;
  switch(dir) {
    case DIRECTION_UP:
      t.x =  0;
      t.y = -1;
      break;
    case DIRECTION_DOWN:
      t.x =  0;
      t.y =  1;
      break;
    case DIRECTION_LEFT:
      t.x = -1;
      t.y =  0;
      break;
    case DIRECTION_RIGHT:
      t.x =  1;
      t.y =  0;
      break;
  }
  return t;
}

void swap(char* array, int index1, int index2) {
  char temp = array[index1];
  array[index1] = array[index2];
  array[index2] = temp;
}

void shuffle(char* array, int size) {
  for(int i = 0; i < size; i++) {
    int index1 = rand() % size;
    int index2 = rand() % size;
    swap(array, index1, index2);
  }
}

bool inBounds(int value,int upper) {
  return (value >= 0) && (value < upper);
}


bool inBounds(struct tilePosition size, struct tilePosition tile) {
  return inBounds(tile.x, size.x) && inBounds(tile.y, size.y);
}

void robotForward() {
  robotPosition += dirToTile(robotHeading);
  xmotion.Forward(20,100);
  printf("Robot Forward\n");
}

void robotLeft() {
  robotHeading = rotateLeft(robotHeading);
  xmotion.Left0(20, 100);
  printf("Robot Left\n");
}

void robotRight() {
  robotHeading = rotateRight(robotHeading);
  xmotion.Right0(20, 100);
  printf("Robot Right\n");
}

void printRobotPos() {
  printf("Pos: (%d, %d)\n", robotPosition.x, robotPosition.y);
}


void printRobotHeading(char head) {
  switch(head) {
    case DIRECTION_UP:
      printf("DIRECTION_UP, ");
      break;
    case DIRECTION_DOWN:
      printf("DIRECTION_DOWN, ");
      break;
    case DIRECTION_LEFT:
      printf("DIRECTION_LEFT, ");
      break;
    case DIRECTION_RIGHT:
      printf("DIRECTION_RIGHT, ");
      break;
  }
}

void setup() {
  
  while(!Serial);
  
  Serial.begin(115200);
  
  robotPosition.x = 0;
  robotPosition.y = 0;

  
  /*struct tilePosition mazeSize;
  mazeSize.x = 32; // X
  mazeSize.y = mazeSize.x; //  Y
  
  
  
  volatile mazeBlock deneme[mazeSize.x][mazeSize.y]; // [x][y]
  
  for(int x = 0; x < mazeSize.x; x++) {
    for (int y = 0; y < mazeSize.y; y++) {
      deneme[x][y] = DIRECTION_UP | DIRECTION_DOWN | DIRECTION_LEFT | DIRECTION_RIGHT;
      if(x == 0) {
        deneme[x][y] &= ~(DIRECTION_LEFT);
      }
      
      if(y == 0) {
        deneme[x][y] &= ~(DIRECTION_UP);
      }
      
      if(x == mazeSize.x - 1) {
        deneme[x][y] &= ~(DIRECTION_RIGHT);
      }
      
      if(y == mazeSize.y - 1) {
        deneme[x][y] &= ~(DIRECTION_DOWN);
      }
    }
  }*/
  
  char orders[] = {4, 4, 2, 2, 2, 8, 1, 8, 2, 2, 4, 2, 4, 4, 1, 4, 1, 4, 1, 1, 1, 4, 2, 4, 2, 8, 2, 4, 2, 2, 2, 2, };
  int numOrders = 32;
  
  for(int i = 0; i < numOrders; i++) {
    int order = orders[i];
    
    printf("Order: ");
    printRobotHeading(order);
    printf("\n");
    if(order & robotHeading) {
      robotForward();
    } else if(order & rotateLeft(robotHeading)) {
      robotLeft();
      robotForward();
    } else if(order & rotateRight(robotHeading)) {
      robotRight();
      robotForward();
    } else {
      printf("Bakcwards\n");
    }
    
    printRobotHeading(robotHeading);
    printRobotPos();
    printf("------\n");
  }
  
}

void loop() {
  
}