#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#define MAX_SIZE 100
#define Row 16
#define Column 16
//forming queue data structure
struct Queue {
int items[MAX_SIZE];
int front;
int rear;
};
struct Queue* queue;
// Function to create an empty queue
struct Queue* createQueue() {
struct Queue* queue = (struct Queue*)malloc(sizeof(struct Queue));
queue->front = -1;
queue->rear = -1;
return queue;
}
// Function to check if the queue is full
int isFull(struct Queue* queue) {
return (queue->rear == MAX_SIZE - 1);
}
// Function to check if the queue is empty
int isEmpty(struct Queue* queue) {
return (queue->front == -1 && queue->rear == -1);
}
// Function to add an element to the queue
void enqueue(struct Queue* queue, int value) {
if (isFull(queue)) {
Serial.println("Queue is full, cannot enqueue!");
return;
}
if (isEmpty(queue)) {
queue->front = 0;
queue->rear = 0;
} else {
queue->rear++;
}
queue->items[queue->rear] = value;
}
// Function to remove an element from the queue
void dequeue(struct Queue* queue) {
if (isEmpty(queue)) {
Serial.println("Queue is empty, cannot dequeue!");
return;
}
if (queue->front == queue->rear) {
queue->front = -1;
queue->rear = -1;
} else {
queue->front++;
}
}
// Function to get the front element of the queue
int front(struct Queue* queue) {
if (isEmpty(queue)) {
Serial.println("Queue is empty!");
return -1;
}
return queue->items[queue->front];
}
// Function to display the queue
void display(struct Queue* queue) {
if (isEmpty(queue)) {
Serial.println("Queue is empty!");
return;
}
Serial.println("Queue elements: ");
for (int i = queue->front; i <= queue->rear; i++) {
Serial.println(queue->items[i]);
}
Serial.println("\n");
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//declaring global variables
byte grid[16][16];
byte walls[16][16][4];
byte start_x = 0;
byte start_y = 0;
byte orientation = 0;
//directions
//up = 0
//right = 1
//down = 2
//left = 3
byte min_val = 0;
byte direction = 0;
byte front_x = 0;
byte front_y = 15;
void setup() {
Serial.begin(19200);
//initial flood grid
for (int i = 0;i < Row;i++){
for (int j = 0;j < Column;j++){
if (i < 8 && j < 8){
grid[i][j] = byte(abs(i-7) + abs(j-7));
}
else if (i > 7 && j < 8){
grid[i][j] = byte(abs(i-8) + abs(j-7));
}
else if (i < 8 && j > 7){
grid[i][j] = byte(abs(i-7) + abs(j-8));
}
else {
grid[i][j] = byte(abs(i-8) + abs(j-8));
}
}
}
//printing out flood grid
for (int j = 0;j < 16;j++) {
for (int i = 0;i < 16;i++){
Serial.println(grid[i][j]);
}
}
//initial wall grid with all four values equal to 4
for (int i = 0;i < Row;i++){
for (int j = 0;j < Column;j++){
for (int k = 0;k < Column;k++){
walls[i][j][k] = 4;
}
}
}
}
//inserting wall values in array of given cell
void insert(byte start_x, byte start_y,byte value){
for (int k = 0;k < 4;k ++){
if (walls[start_x][start_y][k] > 3){
walls[start_x][start_y][k] = byte(value);
break;
}
}
}
//checking for wall in a cell
bool check_wall(byte start_x,int start_y, byte wall_direct){
for (int k = 0;k < 4;k ++){
if (walls[start_x][start_y][k] == byte(wall_direct)){
return 1;
}
}
return 0;
}
//finding reachable neighbours
void find_neighbour(byte start_x,byte start_y){
//upper wall
if((start_y - 1) > 0){
if((!check_wall(start_x,start_y,0))){
enqueue(queue,start_x);
enqueue(queue,(start_y - 1));
}
}
//down wall
if ((start_y + 1) < Column){
if ((!check_wall(start_x,start_y,2))){
enqueue(queue,start_x);
enqueue(queue,(start_y + 1));
}
}
//right wall
if((start_x + 1) < Row){
if((!check_wall(start_x,start_y,1))){
enqueue(queue,(start_x + 1));
enqueue(queue,start_y );
}
}
//left wall
if((start_x - 1) > 0){
if((!check_wall(start_x,start_y,3))){
enqueue(queue,(start_x - 1));
enqueue(queue,start_y);
}
}
}
//finding least value cell
int min_val_cell(byte start_x,byte start_y){
if ((start_y - 1) > 0){
min_val = grid[start_x][start_y - 1];
}
else if ((start_x + 1) < Row){
min_val = grid[start_x + 1][start_y];
}
else if ((start_y + 1) < Column){
min_val = grid[start_x][start_y + 1];
}
else if ((start_x - 1) > 0){
min_val = grid[start_x - 1][start_y];
}
if ((start_x + 1) < Row && min_val > grid[start_x + 1][start_y]){
min_val = grid[start_x + 1][start_y];
direction = 1;
}
if ((start_y + 1) < Column && min_val > grid[start_x][start_y + 1]){
min_val = grid[start_x][start_y + 1];
direction = 2;
}
if ((start_x - 1) > 0 && min_val > grid[start_x - 1][start_y]){
min_val = grid[start_x - 1][start_y];
direction = 3;
}
if ((start_y - 1) > 0 && min_val > grid[start_x - 1][start_y]){
min_val = grid[start_x - 1][start_y];
direction = 0;
}
return (min_val);
}
void orienting(byte direction){
//orienting up
if (direction == 0){
if (orientation == 1){
turnLeft();
}
else if (orientation == 2){
turnLeft();
turnLeft();
}
else if (orientation == 3){
turnLeft();
}
orientation = 0;
}
//orienting right
if (direction == 1){
if (orientation == 0){
turnRight();
}
else if (orientation == 2){
turnLeft();
}
else if (orientation == 3){
turnLeft();
turnLeft();
}
orientation = 1;
}
//orienting down
if (direction == 2){
if (orientation == 0){
turnLeft();
turnLeft();
}
else if (orientation == 1){
turnRight();
}
else if (orientation == 3){
turnLeft();
}
orientation = 2;
}
//orienting left
if (direction == 3){
if (orientation == 0){
turnLeft();
}
else if (orientation == 1){
turnLeft();
turnLeft();
}
else if (orientation == 2){
turnRight();
}
orientation = 3;
}
orientation = direction;
}
void loop() {
while (true){
// if (grid[start_x][start_y] == 0){
// while (1){
// }
}
min_val = 0;
direction = 0;
//finding least value cell
min_val_cell(start_x,start_y);
//orienting up
orienting(direction);
///////////////////////////////////////////////////////////
//checking right wall
if (wallRight){
if (orientation == 0){
insert(start_x,start_y,1);
if ((start_x + 1) < Row){
insert(start_x + 1,start_y,3);
}
}
if (orientation == 2){
insert(start_x,start_y,3);
if ((start_x - 1) > 0){
insert(start_x - 1,start_y,1);
}
}
if (orientation == 1){
insert(start_x,start_y,2);
if ((start_y + 1) < Column){
insert(start_x,start_y + 1,0);
}
}
if (orientation == 3){
insert(start_x,start_y,0);
if ((start_y - 1) > 0){
insert(start_x,start_y - 1,2);
}
}
}
//checking left wall
if (wallLeft){
if (orientation == 0){
insert(start_x,start_y,3);
if ((start_x - 1) > 0){
insert(start_x - 1,start_y,1);
}
}
if (orientation == 2){
insert(start_x,start_y,1);
if ((start_x + 1) < Row){
insert(start_x + 1,start_y,3);
}
}
if (orientation == 1){
insert(start_x,start_y,0);
if ((start_y - 1) > 0){
insert(start_x,start_y - 1,2);
}
}
if (orientation == 3){
insert(start_x,start_y,1);
if ((start_y + 1) < Column){
insert(start_x,start_y + 1,3);
}
}
}
if (!wallFront()){
moveForward();
if (orientation == 0){
start_y -= 1;
}
else if (orientation == 1){
start_x += 1;
}
else if (orientation == 2){
start_y += 1;
}
else if (orientation == 3){
start_x -= 1;
}
}
else{
enqueue(queue,start_x);
enqueue(queue,start_y);
//storing walls
if (orientation == 0){
insert(start_x,start_y,0);
if ((start_y - 1) > 0){
insert(start_x,start_y - 1,2);
}
}
else if (orientation == 2){
insert(start_x,start_y,2);
if ((start_y + 1) < Column){
insert(start_x,start_y + 1,0);
}
}
else if (orientation == 1){
insert(start_x,start_y,1);
if ((start_x + 1) < Row){
insert(start_x + 1,start_y,3);
}
}
else if (orientation == 3){
insert(start_x,start_y,3);
if ((start_x - 1) > 0){
insert(start_x - 1,start_y,1);
}
}
//using floodfill algorithm
while (!isEmpty(queue)){
front_x = front(queue);
dequeue(queue);
front_y = front(queue);
dequeue(queue);
find_neighbour(front_x,front_y);
if (grid[front_x][front_y] <= min_val_cell(front_x,front_y)){
grid[front_x][front_y] = min_val_cell(front_x,front_y) + 1;
}
}
}
}
}
The problem here is that I am not able to upload this code to arduino because some functions like wallFront turnRight etc are not defined by me but by the mms-arduino api.
It's github link is heremicromouse simulator with arduino.
People please come forward for help!!