111 lines
2.3 KiB
C
111 lines
2.3 KiB
C
//
|
|
// Created by nano on 5/14/25.
|
|
//
|
|
// Functies om de richting aan te passen
|
|
#include <ctype.h>
|
|
#include <stdio.h>
|
|
|
|
#include "robot.h"
|
|
#include "../lee-algorithm/lee-algorithm.h"
|
|
|
|
//Dummy moves for the simulator / when a machine with libserialport is unavailable.
|
|
|
|
extern cell neighbours[4];
|
|
//extern cell crossing_neighbours[4];
|
|
|
|
char *moves[100];
|
|
int indx;
|
|
|
|
void turnLeft(robot *r) {
|
|
r->dir = (r->dir + 3) % 4;
|
|
moves[indx] = "\33[1;102mL\33[0m";
|
|
indx++;
|
|
}
|
|
|
|
void turnRight(robot *r) {
|
|
r->dir = (r->dir + 1) % 4;
|
|
moves[indx] = "\33[1;101mR\33[0m";
|
|
indx++;
|
|
}
|
|
|
|
void turnAround(robot *r) {
|
|
r->dir = (r->dir + 2) % 4;
|
|
moves[indx] = "\33[1;103mS\33[0m";
|
|
indx++;
|
|
}
|
|
|
|
void forward(robot *r) {
|
|
cell_add(&r->pos, r->pos, neighbours[r->dir]);
|
|
moves[indx] = "\33[1;105mF\33[0m";
|
|
indx++;
|
|
}
|
|
|
|
void backward(robot *r) {
|
|
cell_add(&r->pos, r->pos, neighbours[(r->dir + 2) % 4]);
|
|
moves[indx] = "\33[1;44mB\33[0m";
|
|
indx++;
|
|
}
|
|
|
|
|
|
int followPath_with_blockages(robot *r, stack *path,int visited[MAZE_SIZE][MAZE_SIZE]) {
|
|
while (path->length > 0) {
|
|
cell current_move = stack_pop(path);
|
|
int result = move(r,current_move);
|
|
visited[r->pos.x][r->pos.y] = 5;
|
|
printMatrix_with_current_pos(visited,*r);
|
|
if (isValidCrossing(r->pos)) {
|
|
printf("ADD BLOCKAGE? (Y/N) ");
|
|
char input[10];
|
|
scanf("%s", input);
|
|
printf("\n");
|
|
if (tolower(input[0]) == 'y' ){
|
|
printf("HOW FAR? (1-4) ");
|
|
int len;
|
|
scanf("%d",&len);
|
|
printf("\n");
|
|
len=(len-1)*2+1;
|
|
register_sensor_blockages(r,len,visited);
|
|
return 0;
|
|
}
|
|
}
|
|
if (result == 0) {
|
|
move(r,current_move);
|
|
visited[r->pos.x][r->pos.y] = 5;
|
|
printMatrix_with_current_pos(visited,*r);
|
|
if (isValidCrossing(r->pos)) {
|
|
printf("ADD BLOCKAGE? (Y/N) ");
|
|
char input[10];
|
|
scanf("%s", input);
|
|
printf("\n");
|
|
if (tolower(input[0]) == 'y' ){
|
|
printf("HOW FAR? (1-4) ");
|
|
int len;
|
|
scanf("%d",&len);
|
|
printf("\n");
|
|
len=(len-1)*2+1;
|
|
register_sensor_blockages(r,len,visited);
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
void printAllMoves() {
|
|
printf("\n");
|
|
for (int i = 0; i < indx; i++) {
|
|
printf("%s",moves[i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
void parkBackwards(robot *r) {
|
|
backward(r);
|
|
}
|
|
|
|
void comm_discard_sensor_data(int timeout){}
|
|
void comm_await_crossing(){}
|
|
void comm_init_communication(){}
|
|
void comm_end_communication(){}
|