-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathvirt_instr.ino
152 lines (141 loc) · 4.87 KB
/
virt_instr.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
/**Function to update the room and update the room data
* INPUTS: MazeValues structure
* OUTPUTS: MazeValues structure
*/
MazeValues enterRoom(MazeValues data) {
data = stepInMaze(data);
data = roomInMaze(data, theMaze);
return data;
}
/**Determine which way to go!
* Will give you the next path depending on what the current room is
* INPUTS: MazeValues strucutre
* OUTPUTS: Char determining which direction to go
*/
char whichWay(MazeValues data) {
const char room1Decider[6] = {'S', 'L', 'L', 'S', 'L','S'};
const char room4Decider[4] = {'R', 'R', 'S'};
uint8_t roomType = 0;
static uint8_t n = 0;
static uint8_t k = 0;
roomType |= (leftWall(data) << 2);
roomType |= (hitWall(data) << 1);
roomType |= rightWall(data);
switch(roomType) {
case 0: return 'S';
break;
case 1: return room1Decider[n++];
break;
case 2: return 'L';
break;
case 3: return 'L';
break;
case 4: return room4Decider[k++];
break;
case 5: return 'S';
break;
case 6: return 'R';
break;
case 7: return 'T';
break;
}
}
/*Turn in maze
* Returns the MazeValues data structure with updated direction and turn
* none right left turn around
* 0 0b01 0b10 0b11
*/
MazeValues turnInMaze(MazeValues test) {
uint8_t index = (test.turn << 2) + test.dir; //Multiply the turn value by 4 and add the dir
test.turn = 0x00; //Reset the turn
test.dir = turn_table[index]; //Grab the corresponding turn from a lookup table
return test; //Return current direction
}
/* Step in Maze
* Returns the MazeValues data structure with update coordinates
*/
MazeValues stepInMaze(MazeValues test) {
const int8_t rowMap[] = {+1, 0, 0, -1};
const int8_t colMap[] = {0, +1, -1, 0};
test.coord.row = (uint8_t) ((int8_t) test.coord.row + rowMap[test.dir]);
test.coord.col = (uint8_t) ((int8_t) test.coord.col + colMap[test.dir]);
return test;
}
/* Room in Maze
* Returns the MazeValues data structure with update room and bees
*/
MazeValues roomInMaze(MazeValues test, const uint8_t maze[]) {
uint16_t index = (test.coord.row * 21) + test.coord.col; //Find the index that our current row and column gives us
uint8_t maze_data = pgm_read_word_near(maze+index); //Saving the maze data into a temp variable
test.bees += (maze_data & 0xF0); //Clearing the lower nibble to grab the bees
test.room = (maze_data & 0x0F); //Clearing the upper nibble to grab the room
return test;
}
/* Did you hit a wall?
* Called from WhichWay subroutine
* Inputs: Structure MazeValues
* Output: Boolean
*/
// Compass S E W N
// dir 00 01 10 11
boolean hitWall(MazeValues mazeData) {
const uint8_t hitTable[] = {0x08, 0x02, 0x04, 0x01}; //room bits
uint8_t wall = hitTable[mazeData.dir]; //grab the specific room that will cause a hit
return (mazeData.room & wall); //Bitwise AND to check if there is a collision
}
/*Is there a right wall?
* Called from main loop
* Turns right then calls hit wall routine then restores original direction
* Inputs: Structure MazeValues
* Output: Boolean
* none right left turn around
* 0 0b01 0b10 0b11
*/
boolean rightWall(MazeValues mazeData) {
uint8_t tempDir = mazeData.dir;
mazeData.turn = 0b01; //Set a right turn
mazeData = turnInMaze(mazeData);
boolean rightWall = hitWall(mazeData); //Call hitwall function
mazeData.dir = tempDir; //Restore original direction
return rightWall;
}
/*Is there a left wall?
* Called from main loop
* Turns left and calls hit wall routine then restores original direction
* Inputs: Structure MazeValues
* Output: Boolean
* none right left turn around
* 0 0b01 0b10 0b11
*/
boolean leftWall(MazeValues mazeData) {
uint8_t tempDir = mazeData.dir;
mazeData.turn = 0b10; //Set a left turn and turn
mazeData = turnInMaze(mazeData);
boolean leftWall = hitWall(mazeData); //Call hitwall and check if the wall is there
mazeData.dir = tempDir; //restore original direction and exit
return leftWall;
}
/*Are we in the forest?
* Called from main loop will put the mcu to sleep if it is
* Inputs: Structure MazeValues
* Output: Boolean
*/
void inForest(MazeValues mazeData) {
if(mazeData.coord.row == 0 && mazeData.coord.col == 0x14) {
victorySpin(mazeData);
set_sleep_mode(SLEEP_MODE_PWR_DOWN);
sleep_enable();
sleep_mode();
}
}
/*We are in the forest so spin!
* Called from main loop will spin the bot a few times
* Inputs: Structure MazeValues
* Output: Boolean
*/
void victorySpin(MazeValues mazeData) {
for(uint8_t i = 0; i < mazeData.bees; i++){
tankTurnRight(maxSpeed, maxSpeed);
delay(2000);
}
}