// Maze map array
const PROGMEM int maze_map[144][4] = {{2,0,0,0},
{1,14,0,0},
{4,15,0,0},
{3,16,0,0},
{6,17,0,0},
{5,18,0,0},
{8,0,0,0},
{7,9,20,0},
{8,21,0,0},
{11,22,0,0},
{10,12,0,0},
{11,24,0,0},
{25,0,0,0},
{2,15,0,0},
{3,14,0,0},
{4,17,0,0},
{5,16,0,0},
{6,30,0,0},
{20,31,0,0},
{8,19,0,0},
{9,33,0,0},
{10,23,0,0},
{22,0,0,0},
{12,36,0,0},
{13,26,37,0},
{25,38,0,0},
{28,39,0,0},
{27,29,0,0},
{28,0,0,0},
{18,42,0,0},
{19,32,43,0},
{31,44,0,0},
{21,45,0,0},
{35,46,0,0},
{34,47,0,0},
{24,48,0,0},
{25,49,0,0},
{26,39,0,0},
{27,38,0,0},
{41,52,0,0},
{40,42,0,0},
{30,41,0,0},
{31,0,0,0},
{32,56,0,0},
{33,46,0,0},
{34,45,0,0},
{35,59,0,0},
{36,60,0,0},
{37,50,61,0},
{49,51,0,0},
{50,52,0,0},
{40,51,0,0},
{54,65,0,0},
{53,55,0,0},
{54,56,0,0},
{44,55,0,0},
{58,69,0,0},
{57,70,0,0},
{47,71,0,0},
{48,72,0,0},
{49,62,0,0},
{61,74,0,0},
{64,75,0,0},
{63,65,0,0},
{53,64,0,0},
{67,0,0,0},
{66,68,79,0},
{67,69,80,0},
{57,68,0,0},
{58,0,0,0},
{59,72,0,0},
{60,71,84,0},
{74,85,0,0},
{62,73,0,0},
{63,76,0,0},
{75,77,88,0},
{76,78,0,0},
{77,90,0,0},
{67,0,0,0},
{68,81,0,0},
{80,82,0,0},
{81,83,0,0},
{82,84,95,0},
{72,83,0,0},
{73,86,0,0},
{85,87,0,0},
{86,99,0,0},
{76,89,0,0},
{88,101,0,0},
{78,91,0,0},
{90,92,0,0},
{91,104,0,0},
{94,105,0,0},
{93,106,0,0},
{83,107,0,0},
{108,0,0,0},
{98,109,0,0},
{97,110,0,0},
{87,100,0,0},
{99,112,0,0},
{89,113,0,0},
{103,0,0,0},
{102,115,0,0},
{92,116,0,0},
{93,117,0,0},
{94,118,0,0},
{95,108,0,0},
{96,107,0,0},
{97,121,0,0},
{98,122,0,0},
{112,123,0,0},
{100,111,0,0},
{101,114,0,0},
{113,115,0,0},
{103,114,127,0},
{104,117,0,0},
{105,116,0,0},
{106,119,0,0},
{118,120,0,0},
{119,132,0,0},
{109,133,0,0},
{110,0,0,0},
{111,124,0,0},
{123,136,0,0},
{126,0,0,0},
{125,127,0,0},
{115,126,128,0},
{127,0,0,0},
{130,141,0,0},
{129,131,0,0},
{130,132,0,0},
{120,131,144,0},
{121,134,0,0},
{133,135,0,0},
{134,136,0,0},
{124,135,137,0},
{136,138,0,0},
{137,139,0,0},
{138,140,0,0},
{139,141,0,0},
{129,140,0,0},
{143,0,0,0},
{142,144,0,0},
{132,143,0,0}};
const PROGMEM int maze_map2[144][4] = {{2,0,0,0},
{1,3,0,0},
{2,15,0,0},
{5,16,0,0},
{4,6,0,0},
{5,18,0,0},
{8,0,0,0},
{7,9,0,0},
{8,10,0,0},
{9,11,0,0},
{10,12,0,0},
{11,24,0,0},
{25,0,0,0},
{15,26,0,0},
{3,14,0,0},
{4,28,0,0},
{18,0,0,0},
{6,17,19,0},
{18,20,0,0},
{19,21,0,0},
{20,22,0,0},
{21,23,0,0},
{22,35,0,0},
{12,36,0,0},
{13,37,0,0},
{14,27,0,0},
{26,39,0,0},
{16,29,0,0},
{28,41,0,0},
{31,42,0,0},
{30,32,0,0},
{31,33,0,0},
{32,34,0,0},
{33,46,0,0},
{23,47,0,0},
{24,48,0,0},
{25,38,49,0},
{37,0,0,0},
{27,40,0,0},
{39,52,0,0},
{29,53,0,0},
{30,54,0,0},
{44,55,0,0},
{43,56,0,0},
{46,57,0,0},
{34,45,0,0},
{35,59,0,0},
{36,60,0,0},
{37,50,0,0},
{49,51,62,0},
{50,0,0,0},
{40,53,0,0},
{41,52,0,0},
{42,66,0,0},
{43,67,0,0},
{44,68,0,0},
{45,69,0,0},
{59,70,0,0},
{47,58,0,0},
{48,72,0,0},
{62,73,0,0},
{50,61,74,0},
{64,75,0,0},
{63,65,76,0},
{64,77,0,0},
{54,78,0,0},
{55,0,0,0},
{56,80,0,0},
{57,70,0,0},
{58,69,0,0},
{72,0,0,0},
{60,71,84,0},
{61,85,0,0},
{62,0,0,0},
{63,87,0,0},
{64,88,0,0},
{65,0,0,0},
{66,79,0,0},
{78,91,0,0},
{68,81,92,0},
{80,82,0,0},
{81,83,0,0},
{82,84,0,0},
{72,83,96,0},
{73,86,97,0},
{85,87,0,0},
{75,86,0,0},
{76,89,0,0},
{88,90,0,0},
{89,102,0,0},
{79,103,0,0},
{80,93,0,0},
{92,94,0,0},
{93,106,0,0},
{96,0,0,0},
{84,95,108,0},
{85,109,0,0},
{99,0,0,0},
{98,100,0,0},
{99,101,112,0},
{100,0,0,0},
{90,103,0,0},
{91,102,0,0},
{105,116,0,0},
{104,117,0,0},
{94,107,0,0},
{106,119,0,0},
{96,0,0,0},
{97,110,0,0},
{109,111,0,0},
{110,123,0,0},
{100,124,0,0},
{114,125,0,0},
{113,115,126,0},
{114,116,0,0},
{104,115,0,0},
{105,118,0,0},
{117,130,0,0},
{107,120,0,0},
{119,132,0,0},
{133,0,0,0},
{123,134,0,0},
{111,122,0,0},
{112,125,0,0},
{113,124,0,0},
{114,138,0,0},
{128,139,0,0},
{127,0,0,0},
{130,141,0,0},
{118,129,0,0},
{132,143,0,0},
{120,131,144,0},
{121,134,0,0},
{122,133,135,0},
{134,136,0,0},
{135,137,0,0},
{136,138,0,0},
{126,137,0,0},
{127,140,0,0},
{139,141,0,0},
{129,140,142,0},
{141,143,0,0},
{131,142,0,0},
{132,0,0,0}};
const PROGMEM int maze_map3[144][4] = {{13,0,0,0},
{3,14,0,0},
{2,4,0,0},
{3,5,0,0},
{4,6,0,0},
{5,7,18,0},
{6,8,0,0},
{7,9,0,0},
{8,10,0,0},
{9,11,0,0},
{10,12,0,0},
{11,24,0,0},
{1,25,0,0},
{2,15,0,0},
{14,0,0,0},
{17,28,0,0},
{16,29,0,0},
{6,19,0,0},
{18,31,0,0},
{21,0,0,0},
{20,22,0,0},
{21,23,0,0},
{22,24,35,0},
{12,23,0,0},
{13,26,0,0},
{25,38,0,0},
{28,39,0,0},
{16,27,0,0},
{17,30,0,0},
{29,42,0,0},
{19,32,0,0},
{31,33,0,0},
{32,34,0,0},
{33,46,0,0},
{23,36,0,0},
{35,0,0,0},
{49,0,0,0},
{26,50,0,0},
{27,51,0,0},
{41,52,0,0},
{40,53,0,0},
{30,43,0,0},
{42,44,0,0},
{43,45,0,0},
{44,57,0,0},
{34,47,0,0},
{46,48,0,0},
{47,60,0,0},
{37,50,61,0},
{38,49,0,0},
{39,63,0,0},
{40,64,0,0},
{41,54,0,0},
{53,66,0,0},
{56,67,0,0},
{55,0,0,0},
{45,69,0,0},
{59,70,0,0},
{58,60,0,0},
{48,59,72,0},
{49,73,0,0},
{63,0,0,0},
{51,62,75,0},
{52,76,0,0},
{77,0,0,0},
{54,78,0,0},
{55,68,79,0},
{67,80,0,0},
{57,81,0,0},
{58,82,0,0},
{72,0,0,0},
{60,71,0,0},
{61,74,0,0},
{73,86,0,0},
{63,76,0,0},
{64,75,77,0},
{65,76,0,0},
{66,90,0,0},
{67,0,0,0},
{68,81,92,0},
{69,80,0,0},
{70,83,94,0},
{82,84,0,0},
{83,96,0,0},
{97,0,0,0},
{74,98,0,0},
{88,99,0,0},
{87,89,0,0},
{88,101,0,0},
{78,91,102,0},
{90,103,0,0},
{80,93,0,0},
{92,105,0,0},
{82,95,0,0},
{94,0,0,0},
{84,108,0,0},
{85,109,0,0},
{86,99,0,0},
{87,98,0,0},
{101,112,0,0},
{89,100,0,0},
{90,0,0,0},
{91,115,0,0},
{105,116,0,0},
{93,104,0,0},
{107,118,0,0},
{106,119,0,0},
{96,120,0,0},
{97,110,121,0},
{109,111,0,0},
{110,123,0,0},
{100,113,0,0},
{112,114,0,0},
{113,115,0,0},
{103,114,0,0},
{104,128,0,0},
{118,129,0,0},
{106,117,0,0},
{107,120,0,0},
{108,119,132,0},
{109,133,0,0},
{123,134,0,0},
{111,122,0,0},
{125,136,0,0},
{124,137,0,0},
{127,138,0,0},
{126,139,0,0},
{116,129,0,0},
{117,128,0,0},
{131,142,0,0},
{130,143,0,0},
{120,144,0,0},
{121,0,0,0},
{122,135,0,0},
{134,136,0,0},
{124,135,0,0},
{125,138,0,0},
{126,137,0,0},
{127,140,0,0},
{139,141,0,0},
{140,142,0,0},
{130,141,0,0},
{131,144,0,0},
{132,143,0,0}};
const PROGMEM int maze_map4[144][4] = {{13,0,0,0},
{3,14,0,0},
{2,4,0,0},
{3,16,0,0},
{6,17,0,0},
{5,7,0,0},
{6,8,0,0},
{7,9,0,0},
{8,10,0,0},
{9,11,0,0},
{10,23,0,0},
{24,0,0,0},
{1,25,0,0},
{2,15,0,0},
{14,27,0,0},
{4,17,0,0},
{5,16,0,0},
{19,0,0,0},
{18,20,0,0},
{19,32,0,0},
{22,33,0,0},
{21,34,0,0},
{11,35,0,0},
{12,36,0,0},
{13,26,0,0},
{25,27,0,0},
{15,26,0,0},
{29,40,0,0},
{28,30,0,0},
{29,31,0,0},
{30,32,0,0},
{20,31,44,0},
{21,45,0,0},
{22,35,0,0},
{23,34,0,0},
{24,48,0,0},
{38,49,0,0},
{37,39,0,0},
{38,51,0,0},
{28,41,0,0},
{40,42,0,0},
{41,54,0,0},
{44,0,0,0},
{32,43,0,0},
{33,46,0,0},
{45,47,0,0},
{46,59,0,0},
{36,60,0,0},
{37,50,61,0},
{49,62,0,0},
{39,52,0,0},
{51,64,0,0},
{54,65,0,0},
{42,53,66,0},
{56,67,0,0},
{55,68,0,0},
{58,69,0,0},
{57,59,0,0},
{47,58,0,0},
{48,72,0,0},
{49,0,0,0},
{50,74,0,0},
{75,0,0,0},
{52,76,0,0},
{53,77,0,0},
{54,0,0,0},
{55,79,0,0},
{56,69,0,0},
{57,68,0,0},
{71,82,0,0},
{70,83,0,0},
{60,84,0,0},
{74,85,0,0},
{62,73,0,0},
{63,76,87,0},
{64,75,0,0},
{65,78,0,0},
{77,90,0,0},
{67,80,0,0},
{79,81,0,0},
{80,82,0,0},
{70,81,0,0},
{71,84,0,0},
{72,83,96,0},
{73,86,97,0},
{85,98,0,0},
{75,88,0,0},
{87,89,0,0},
{88,90,0,0},
{78,89,91,0},
{90,103,0,0},
{93,104,0,0},
{92,94,105,0},
{93,95,0,0},
{94,0,0,0},
{84,108,0,0},
{85,109,0,0},
{86,99,0,0},
{98,100,0,0},
{99,101,0,0},
{100,102,0,0},
{101,114,0,0},
{91,0,0,0},
{92,116,0,0},
{93,106,0,0},
{105,118,0,0},
{108,119,0,0},
{96,107,0,0},
{97,121,0,0},
{111,0,0,0},
{110,112,123,0},
{111,113,0,0},
{112,125,0,0},
{102,115,0,0},
{114,116,0,0},
{104,115,0,0},
{129,0,0,0},
{106,119,0,0},
{107,118,0,0},
{132,0,0,0},
{109,122,0,0},
{121,134,0,0},
{111,124,0,0},
{123,136,0,0},
{113,126,0,0},
{125,127,0,0},
{126,139,0,0},
{129,140,0,0},
{117,128,130,0},
{129,131,0,0},
{130,132,0,0},
{120,131,144,0},
{134,0,0,0},
{122,133,135,0},
{134,136,0,0},
{124,135,0,0},
{138,0,0,0},
{137,139,0,0},
{127,138,140,0},
{128,139,0,0},
{142,0,0,0},
{141,143,0,0},
{142,144,0,0},
{132,143,0,0}};
const PROGMEM int maze_map5[144][4] = {{2,0,0,0},
{1,3,0,0},
{2,4,0,0},
{3,16,0,0},
{6,17,0,0},
{5,7,18,0},
{6,19,0,0},
{9,0,0,0},
{8,10,0,0},
{9,11,22,0},
{10,23,0,0},
{24,0,0,0},
{14,25,0,0},
{13,15,0,0},
{14,27,0,0},
{4,28,0,0},
{5,0,0,0},
{6,30,0,0},
{7,20,0,0},
{19,32,0,0},
{22,33,0,0},
{10,21,0,0},
{11,35,0,0},
{12,36,0,0},
{13,37,0,0},
{27,38,0,0},
{15,26,39,0},
{16,29,0,0},
{28,41,0,0},
{18,42,0,0},
{32,0,0,0},
{20,31,33,0},
{21,32,45,0},
{35,0,0,0},
{23,34,36,0},
{24,35,0,0},
{25,0,0,0},
{26,50,0,0},
{27,51,0,0},
{41,52,0,0},
{29,40,0,0},
{30,43,54,0},
{42,55,0,0},
{45,0,0,0},
{33,44,0,0},
{47,58,0,0},
{46,48,0,0},
{47,60,0,0},
{50,61,0,0},
{38,49,0,0},
{39,0,0,0},
{40,64,0,0},
{54,0,0,0},
{42,53,66,0},
{43,56,0,0},
{55,57,0,0},
{56,58,0,0},
{46,57,59,0},
{58,0,0,0},
{48,72,0,0},
{49,62,0,0},
{61,63,0,0},
{62,75,0,0},
{52,65,0,0},
{64,77,0,0},
{54,67,0,0},
{66,68,0,0},
{67,80,0,0},
{70,0,0,0},
{69,82,0,0},
{72,83,0,0},
{60,71,84,0},
{74,85,0,0},
{73,75,0,0},
{63,74,76,0},
{75,0,0,0},
{65,78,0,0},
{77,90,0,0},
{80,91,0,0},
{68,79,81,0},
{80,82,0,0},
{70,81,0,0},
{71,95,0,0},
{72,0,0,0},
{73,86,97,0},
{85,87,0,0},
{86,88,0,0},
{87,89,100,0},
{88,101,0,0},
{78,102,0,0},
{79,0,0,0},
{93,104,0,0},
{92,94,0,0},
{93,106,0,0},
{83,96,0,0},
{95,108,0,0},
{85,109,0,0},
{110,0,0,0},
{100,111,0,0},
{88,99,0,0},
{89,113,0,0},
{90,103,0,0},
{102,104,0,0},
{92,103,0,0},
{117,0,0,0},
{94,107,0,0},
{106,119,0,0},
{96,120,0,0},
{97,121,0,0},
{98,122,0,0},
{99,112,0,0},
{111,124,0,0},
{101,125,0,0},
{115,126,0,0},
{114,116,0,0},
{115,117,0,0},
{105,116,118,129},
{117,0,0,0},
{107,120,0,0},
{108,119,132,0},
{109,133,0,0},
{110,134,0,0},
{124,135,0,0},
{112,123,0,0},
{113,0,0,0},
{114,138,0,0},
{139,0,0,0},
{129,140,0,0},
{117,128,0,0},
{131,142,0,0},
{130,132,0,0},
{120,131,0,0},
{121,134,0,0},
{122,133,0,0},
{123,136,0,0},
{135,137,0,0},
{136,138,0,0},
{126,137,139,0},
{127,138,0,0},
{128,141,0,0},
{140,142,0,0},
{130,141,143,0},
{142,144,0,0},
{143,0,0,0}};
const PROGMEM int maze_map6[144][4] = {{2,0,0,0},
{1,3,0,0},
{2,15,0,0},
{5,16,0,0},
{4,17,0,0},
{7,18,0,0},
{6,8,0,0},
{7,9,0,0},
{8,10,0,0},
{9,11,0,0},
{10,12,23,0},
{11,24,0,0},
{14,25,0,0},
{13,0,0,0},
{3,27,0,0},
{4,28,0,0},
{5,29,0,0},
{6,19,0,0},
{18,31,0,0},
{21,32,0,0},
{20,22,33,0},
{21,0,0,0},
{11,35,0,0},
{12,36,0,0},
{13,26,37,0},
{25,27,0,0},
{15,26,0,0},
{16,40,0,0},
{17,30,0,0},
{29,42,0,0},
{19,43,0,0},
{20,44,0,0},
{21,34,0,0},
{33,46,0,0},
{23,47,0,0},
{24,0,0,0},
{25,49,0,0},
{39,50,0,0},
{38,51,0,0},
{28,52,0,0},
{42,0,0,0},
{30,41,54,0},
{31,55,0,0},
{32,45,0,0},
{44,0,0,0},
{34,58,0,0},
{35,48,0,0},
{47,60,0,0},
{37,50,0,0},
{38,49,0,0},
{39,63,0,0},
{40,53,0,0},
{52,65,0,0},
{42,0,0,0},
{43,67,0,0},
{57,68,0,0},
{56,69,0,0},
{46,59,70,0},
{58,0,0,0},
{48,72,0,0},
{62,73,0,0},
{61,63,0,0},
{51,62,0,0},
{65,76,0,0},
{53,64,77,0},
{67,78,0,0},
{55,66,68,0},
{56,67,0,0},
{57,70,0,0},
{58,69,0,0},
{72,83,0,0},
{60,71,84,0},
{61,74,0,0},
{73,86,0,0},
{76,0,0,0},
{64,75,88,0},
{65,0,0,0},
{66,79,0,0},
{78,91,0,0},
{81,92,0,0},
{80,93,0,0},
{83,94,0,0},
{71,82,0,0},
{72,0,0,0},
{97,0,0,0},
{74,87,0,0},
{86,99,0,0},
{76,89,100,0},
{88,90,0,0},
{89,91,0,0},
{79,90,0,0},
{80,104,0,0},
{81,105,0,0},
{82,95,0,0},
{94,96,0,0},
{95,108,0,0},
{85,98,109,0},
{97,99,0,0},
{87,98,0,0},
{88,101,0,0},
{100,102,0,0},
{101,103,0,0},
{102,0,0,0},
{92,116,0,0},
{93,106,0,0},
{105,107,0,0},
{106,108,0,0},
{96,107,120,0},
{97,121,0,0},
{111,0,0,0},
{110,112,123,0},
{111,113,124,0},
{112,125,0,0},
{115,126,0,0},
{114,127,0,0},
{104,128,0,0},
{118,0,0,0},
{117,119,130,0},
{118,131,0,0},
{108,132,0,0},
{109,122,0,0},
{121,134,0,0},
{111,0,0,0},
{112,136,0,0},
{113,137,0,0},
{114,138,0,0},
{115,139,0,0},
{116,129,0,0},
{128,141,0,0},
{118,142,0,0},
{119,143,0,0},
{120,144,0,0},
{134,0,0,0},
{122,133,135,0},
{134,136,0,0},
{124,135,0,0},
{125,138,0,0},
{126,137,0,0},
{127,140,0,0},
{139,141,0,0},
{129,140,0,0},
{130,0,0,0},
{131,144,0,0},
{132,143,0,0}};
// Define scientific target locations
int targets[] = {28,85,129,143,118,101,65,56,95,-1};
int targets2[] = {21,46,86,62,25,115,80,68,11,-1};
int targets3[] = {102,52,16,43,70,46,33,142,121,133};
int targets4[] = {27,5,8,34,48,24,104,102,49,137};
int targets5[] = {95,30,20,67,80,91,39,37,122,-1};
int targets6[] = {27,73,113,137,126,68,58,33,54,30};
int target_index = 0;
// Variables to track movement and states
int prev_cell = 0;
int curr_cell = 1; // Starting from cell 1
int next_step;
int prev_cell2 = 0;
int curr_cell2;
bool flag_move2;
int current_cell = 1;
int current_cell2 = 0;
// LED pins
const int redLED = 3;
const int greenLED = 6;
const int aled = 14; // Pin Display
const int bled = 15; // Pin Display
const int cled = 16; // Pin Display
const int dled = 17; // Pin Display
const int eled = 7; // Pin Display
const int fled = 8; // Pin Display
const int gled = 5; // Pin Display
void maze_sensor(int cmap[144][4] , int prev_cell, int curr_cell, char next_step, int &prev_cell2,
int &curr_cell2, bool &flag_move2)
{
//delay(1000);
bool flag_move = false;
int dirvect = curr_cell - prev_cell;
int front = curr_cell + dirvect;
int back = prev_cell;
int nn = 12;
int twm1 = 1;
int twm2 = 1;
if (floor((float)curr_cell / (float)nn) == ceil((float)curr_cell / (float)nn) && curr_cell != 0)
twm1 = nn;
else
twm1 = curr_cell % nn;
if (floor((float)prev_cell / (float)nn) == ceil((float)prev_cell / (float)nn) && prev_cell != 0)
twm2 = nn;
else
twm2 = prev_cell % nn;
int tmp1 = twm1 - twm2;
int sng = 1;
if (dirvect < 0)
sng = -1;
if (dirvect == 0)
sng = 0;
int vectorsx = tmp1;
int vectorsy = sng * (1 - abs(tmp1));
int left = curr_cell + (-vectorsy + (nn)*vectorsx);
int right = curr_cell + (vectorsy - (nn)*vectorsx);
char *mazx[] = {'c', 'c', 'c', 'c'};
int tmp2 = 1;
int tmp3 = 0;
for (int icnt = 0; icnt <= 3; icnt++)
{
tmp3 = pgm_read_word_near(&cmap[curr_cell - 1][icnt]);
if (tmp3 == left)
mazx[icnt] = 'l';
else if (tmp3 == right)
mazx[icnt] = 'r';
else if (tmp3 == front)
mazx[icnt] = 'f';
else if (tmp3 == back)
mazx[icnt] = 'b';
else
mazx[icnt] = 'c';
if (tmp3 > 0 || tmp3 <= nn * nn)
{
if (mazx[icnt] == next_step)
{
flag_move = true;
prev_cell = curr_cell;
tmp2 = tmp3;
if (tmp2 == 0)
{
curr_cell = 1;
prev_cell = 0;
}
else
curr_cell = tmp2;
}
}
}
curr_cell2 = curr_cell;
prev_cell2 = prev_cell;
flag_move2 = flag_move;
}
void setup(){
Serial.begin(9600); //sets baud rate to match arduino usedd for serial communcation
pinMode(redLED, OUTPUT); //initialises red led as an output signal
pinMode(greenLED, OUTPUT); //initialises green led as an output signal
pinMode(aled, OUTPUT); //initialises segment 1 of digital display as an output signal
pinMode(bled, OUTPUT); //initialises segment 2 of digital display as an output signal
pinMode(cled, OUTPUT); //initialises segment 3 of digital display as an output signal
pinMode(dled, OUTPUT); //initialises segment 4 of digital display as an output signal
pinMode(eled, OUTPUT); //initialises segment 5 of digital display as an output signal
pinMode(fled, OUTPUT); //initialises segment 6 of digital display as an output signal
pinMode(gled, OUTPUT); //initialises segment 7 of digital display as an output signal
// Initial rover state
digitalWrite(greenLED, HIGH); // sets green led to on to signal that the Rover can move
digitalWrite(redLED, LOW); // sets red led to off to signal that the Rover can move
digitalWrite(aled, LOW); //sets segment 1 of digital display off
digitalWrite(bled, LOW); //sets segment 2 of digital display off
digitalWrite(cled, LOW); //sets segment 3 of digital display off
digitalWrite(dled, LOW); //sets segment 4 of digital display off
digitalWrite(eled, LOW); //sets segment 5 of digital display off
digitalWrite(fled, LOW); //sets segment 6 of digital display off
digitalWrite(gled, HIGH); //sets segment 7 of digital display on to signal movememnt
Serial.println("Rover initialized."); //output message in serial to show rover initialization is complete
}
void performDrilling() {
//subroutine that is called when a spot can be drilled
digitalWrite(greenLED, LOW); // sets green led to off to signal that the Rover cant move
digitalWrite(redLED, HIGH); // sets red led to on to signal that the Rover cant move
digitalWrite(aled, LOW); //sets segment 1 of digital display off
digitalWrite(bled, HIGH); //sets segment 2 of digital display on to signal drilling action
digitalWrite(cled, HIGH); //sets segment 3 of digital display on to signal drilling action
digitalWrite(dled, HIGH); //sets segment 4 of digital display on to signal drilling action
digitalWrite(eled, HIGH); //sets segment 5 of digital display on to signal drilling action
digitalWrite(fled, LOW); //sets segment 6 of digital display off
digitalWrite(gled, HIGH); //sets segment 7 of digital display on to signal drilling action
Serial.println("DRILLING ON TARGET CELL:"); //Output message to say what cell is currently being drilled
Serial.println(current_cell);
delay(5000); //5 second delay to simulate drilling time
digitalWrite(redLED, LOW); // sets red led to off to signal that the Rover can move
digitalWrite(greenLED, HIGH); // sets green led to on to signal that the Rover can move
digitalWrite(aled, LOW); //sets segment 1 of digital display off
digitalWrite(bled, LOW); //sets segment 2 of digital display off
digitalWrite(cled, LOW); //sets segment 3 of digital display off
digitalWrite(dled, LOW); //sets segment 4 of digital display off
digitalWrite(eled, LOW); //sets segment 5 of digital display off
digitalWrite(fled, LOW); //sets segment 6 of digital display off
digitalWrite(gled, HIGH); //sets segment 7 of digital display on to signal movememnt
}
void loop()
{
bool flag_move2 = false; // set movment flag to false so rover cannot move to next cell intially
bool running = true; // sets running to true for the while loop
while (running) //loop for map 1
{
maze_sensor(maze_map,prev_cell, current_cell, 'r', prev_cell2, current_cell2, flag_move2); // calls maze_sensor function to see if rover can turn right
if (flag_move2 == true && current_cell2 != 1 && current_cell != 12) // checks movement flag and prevents bugs that arise from calculations in maze_sensor function
{
Serial.println("ROTATING RIGHT"); // outputs movement direction to serial monitor
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 35.5°, N2 = 47.2°, N3 = 0°, N4 = -47.2°, N5 = -35.5°, N6 = 0°"); // outputs steering angles to serial monitor
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT"); // outputs confirmation of movement to serial monitor
delay(2000); // 2 second delay to simulate right turn and movement to next cell
prev_cell = prev_cell2; // updates the previous cell of the rover to the cell set by maze_sensor function
current_cell = current_cell2; // updates the current cell of the rover to the cell set by the maze_sensor function
for (int i = 0; i < sizeof(targets) / sizeof(targets[0]); i++) // for loop to iterate through each target in the targets array for map 1
{
if (targets[i] == current_cell) // checks to see if the cell the rover is on is a target cell to drill
{
performDrilling(); // drills the current cell if it is in the targets array for map 1
targets[i] = -1; // sets that target in the array to null (-1 in the case of this program)
}
}
}
else // if rover cannot turn right then tries a different direction
{
maze_sensor(maze_map,prev_cell, current_cell, 'f', prev_cell2, current_cell2, flag_move2); // calls maze_sensor function to see if rover can move forward
if (flag_move2 == true && current_cell2 != 1 && current_cell != 12) // checks movement flag and prevents bugs that arise from calculations in maze_sensor function
{
Serial.println("MOVING FORWARDS"); // outputs movement direction to serial monitor
delay(1000); // 1 second delay to simulate movement to next cell
prev_cell = prev_cell2; // updates the previous cell of the rover to the cell set by maze_sensor function
current_cell = current_cell2; // updates the current cell of the rover to the cell set by the maze_sensor function
for (int i = 0; i < sizeof(targets) / sizeof(targets[0]); i++) // for loop to iterate through each target in the targets array for map 1
{
if (targets[i] == current_cell) // checks to see if the cell the rover is on is a target cell to drill
{
performDrilling(); // drills the current cell if it is in the targets array for map 1
targets[i] = -1; // sets that target in the array to null (-1 in the case of this program)
}
}
}
else // if rover cannot move forward then tries a different direction
{
maze_sensor(maze_map,prev_cell, current_cell, 'l', prev_cell2, current_cell2, flag_move2); // calls maze_sensor function to see if rover can turn left
if (flag_move2 == true && current_cell2 != 1 && current_cell != 12) // checks movement flag and prevents bugs that arise from calculations in maze_sensor function
{
Serial.println("ROTATING LEFT"); // outputs movement direction to serial monitor
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°"); // outputs steering angles to serial monitor
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT"); // outputs confirmation of movement to serial monitor
delay(2000); // 2 second delay to simulate left turn and movement to next cell
prev_cell = prev_cell2; // updates the previous cell of the rover to the cell set by maze_sensor function
current_cell = current_cell2; // updates the current cell of the rover to the cell set by the maze_sensor function
for (int i = 0; i < sizeof(targets) / sizeof(targets[0]); i++) // for loop to iterate through each target in the targets array for map 1
{
if (targets[i] == current_cell) // checks to see if the cell the rover is on is a target cell to drill
{
performDrilling(); // drills the current cell if it is in the targets array for map 1
targets[i] = -1; // sets that target in the array to null (-1 in the case of this program)
}
}
}
else // if rover cannot move left then tries a different direction
{
maze_sensor(maze_map,prev_cell, current_cell, 'b', prev_cell2, current_cell2, flag_move2); // calls maze_sensor function to see if rover can move backwards
if (flag_move2 == true)// checks movement flag
{
Serial.println("MOVING BACK BY ROTATING LEFT 180°"); // outputs movement direction to serial monitor
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°"); // outputs steering angles to serial monitor
Serial.println("ROTATION COMPLETE, EXECUTING BACKWARDS MOVEMENT"); // outputs confirmation of movement to serial monitor
delay(3000); // 2 second delay to simulate 2 left or right turns and movement to next cell
prev_cell = prev_cell2; // updates the previous cell of the rover to the cell set by maze_sensor function
current_cell = current_cell2; // updates the current cell of the rover to the cell set by the maze_sensor function
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets) / sizeof(targets[0]); i++) // for loop to iterate through each target in the targets array for map 1
{
if (targets[i] == current_cell) // checks to see if the cell the rover is on is a target cell to drill
{
performDrilling(); // drills the current cell if it is in the targets array for map 1
targets[i] = -1; // sets that target in the array to null (-1 in the case of this program)
}
}
}
}
}
}
Serial.print("Current cell: "); // outputs the current cell the rover is on to the serial monitor
Serial.println(current_cell); // addition to previous line
for (int i = 0; i < sizeof(targets) / sizeof(targets[0]); i++) // for loop to iterate through each target in the targets array for map 1
{
if (targets[i] != -1) // checks to see if any targets are left to be drilled
{
running = true; // keeps while loop running
break; // breaks out of for loop to stop further iterations
}
else // if no targets left to be drilled
{
running = false; // sets running to false to ends while loop for map 1
}
}
}
/////////////////////////////////////////////////////////////////////////////////////
Serial.print("Final cell of map: "); // outputs the final cell the rover finished on for the map
Serial.println(current_cell); //part of previous line
delay(2000); // delay for readability of serial monitor
Serial.println("All sites mined"); // outputs confirmation of all sites having being mined
Serial.println("moving to map 2"); // outputs next map in list
delay(2000); // delay for readibility of serial monitor
Serial.print("Starting Cell: "); // outputs the starting cell of the rover for the next map
Serial.println(current_cell);
//all remaining lines of code are repeats of the previous loop and code above with maze_map and targets[] being changed to their corresponding numbers for their maps
////////////////////////////////////////////////////////////////////////////////
running = true;
while (running)
{
maze_sensor(maze_map2,prev_cell, current_cell, 'r', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && !(prev_cell == 11 && current_cell == 12))
{
Serial.println("ROTATING RIGHT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 35.5°, N2 = 47.2°, N3 = 0°, N4 = -47.2°, N5 = -35.5°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets2) / sizeof(targets2[0]); i++)
{
if (targets2[i] == current_cell)
{
performDrilling();
targets2[i] = -1;
}
}
}
else
{
maze_sensor(maze_map2,prev_cell, current_cell, 'f', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("MOVING FORWARDS");
delay(1000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets2) / sizeof(targets2[0]); i++)
{
if (targets2[i] == current_cell)
{
performDrilling();
targets2[i] = -1;
}
}
}
else
{
maze_sensor(maze_map2,prev_cell, current_cell, 'l', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("ROTATING LEFT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets2) / sizeof(targets2[0]); i++)
{
if (targets2[i] == current_cell)
{
performDrilling();
targets2[i] = -1;
}
}
}
else
{
maze_sensor(maze_map2,prev_cell, current_cell, 'b', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("MOVING BACK BY ROTATING LEFT 180°");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING BACKWARDS MOVEMENT");
delay(3000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets2) / sizeof(targets2[0]); i++)
{
if (targets2[i] == current_cell)
{
performDrilling();
targets2[i] = -1;
}
}
}
}
}
}
Serial.print("Current cell: ");
Serial.println(current_cell);
// checking if all spots drilled
for (int i = 0; i < sizeof(targets2) / sizeof(targets2[0]); i++)
{
if (targets2[i] != -1)
{
running = true;
break;
}
else
{
running = false;
}
}
}
/////////////////////////////////////////////////////////////////////////////////////
Serial.print("Final cell of map: ");
Serial.println(current_cell);
delay(2000);
Serial.println("All sites mined");
Serial.println("moving to map 3");
delay(2000);
Serial.print("Starting Cell: ");
Serial.println(current_cell);
////////////////////////////////////////////////////////////////////////////////
running = true;
while (running)
{
maze_sensor(maze_map3,prev_cell, current_cell, 'r', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && !(prev_cell == 11 && current_cell == 12))
{
Serial.println("ROTATING RIGHT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 35.5°, N2 = 47.2°, N3 = 0°, N4 = -47.2°, N5 = -35.5°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets3) / sizeof(targets3[0]); i++)
{
if (targets3[i] == current_cell)
{
performDrilling();
targets3[i] = -1;
}
}
}
else
{
maze_sensor(maze_map3,prev_cell, current_cell, 'f', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("MOVING FORWARDS");
delay(1000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets3) / sizeof(targets3[0]); i++)
{
if (targets3[i] == current_cell)
{
performDrilling();
targets3[i] = -1;
}
}
}
else
{
maze_sensor(maze_map3,prev_cell, current_cell, 'l', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("ROTATING LEFT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets3) / sizeof(targets3[0]); i++)
{
if (targets3[i] == current_cell)
{
performDrilling();
targets3[i] = -1;
}
}
}
else
{
maze_sensor(maze_map3,prev_cell, current_cell, 'b', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("MOVING BACK BY ROTATING LEFT 180°");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING BACKWARDS MOVEMENT");
delay(3000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets3) / sizeof(targets3[0]); i++)
{
if (targets3[i] == current_cell)
{
performDrilling();
targets3[i] = -1;
}
}
}
}
}
}
Serial.print("Current cell: ");
Serial.println(current_cell);
// checking if all spots drilled
for (int i = 0; i < sizeof(targets3) / sizeof(targets3[0]); i++)
{
if (targets3[i] != -1)
{
running = true;
break;
}
else
{
running = false;
}
}
}
/////////////////////////////////////////////////////////////////////////////////////
Serial.print("Final cell of map: ");
Serial.println(current_cell);
delay(2000);
Serial.println("All sites mined");
Serial.println("moving to map 4");
delay(2000);
Serial.print("Starting Cell: ");
Serial.println(current_cell);
////////////////////////////////////////////////////////////////////////////////
running = true;
while (running)
{
maze_sensor(maze_map4,prev_cell, current_cell, 'r', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("ROTATING RIGHT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 35.5°, N2 = 47.2°, N3 = 0°, N4 = -47.2°, N5 = -35.5°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets4) / sizeof(targets4[0]); i++)
{
if (targets4[i] == current_cell)
{
performDrilling();
targets4[i] = -1;
}
}
}
else
{
maze_sensor(maze_map4,prev_cell, current_cell, 'f', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("MOVING FORWARDS");
delay(1000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets4) / sizeof(targets4[0]); i++)
{
if (targets4[i] == current_cell)
{
performDrilling();
targets4[i] = -1;
}
}
}
else
{
maze_sensor(maze_map4,prev_cell, current_cell, 'l', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("ROTATING LEFT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets4) / sizeof(targets4[0]); i++)
{
if (targets4[i] == current_cell)
{
performDrilling();
targets4[i] = -1;
}
}
}
else
{
maze_sensor(maze_map4,prev_cell, current_cell, 'b', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("MOVING BACK BY ROTATING LEFT 180°");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING BACKWARDS MOVEMENT");
delay(3000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets4) / sizeof(targets4[0]); i++)
{
if (targets4[i] == current_cell)
{
performDrilling();
targets4[i] = -1;
}
}
}
}
}
}
Serial.print("Current cell: ");
Serial.println(current_cell);
// checking if all spots drilled
for (int i = 0; i < sizeof(targets4) / sizeof(targets4[0]); i++)
{
if (targets4[i] != -1)
{
running = true;
break;
}
else
{
running = false;
}
}
}
/////////////////////////////////////////////////////////////////////////////////////
Serial.print("Final cell of map: ");
Serial.println(current_cell);
delay(2000);
Serial.println("All sites mined");
Serial.println("moving to map 5");
delay(2000);
Serial.print("Starting Cell: ");
Serial.println(current_cell);
////////////////////////////////////////////////////////////////////////////////
running = true;
while (running)
{
maze_sensor(maze_map5,prev_cell, current_cell, 'r', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("ROTATING RIGHT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 35.5°, N2 = 47.2°, N3 = 0°, N4 = -47.2°, N5 = -35.5°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets5) / sizeof(targets5[0]); i++)
{
if (targets5[i] == current_cell)
{
performDrilling();
targets5[i] = -1;
}
}
}
else
{
maze_sensor(maze_map5,prev_cell, current_cell, 'f', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("MOVING FORWARDS");
delay(1000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets5) / sizeof(targets5[0]); i++)
{
if (targets5[i] == current_cell)
{
performDrilling();
targets5[i] = -1;
}
}
}
else
{
maze_sensor(maze_map5,prev_cell, current_cell, 'l', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("ROTATING LEFT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets5) / sizeof(targets5[0]); i++)
{
if (targets5[i] == current_cell)
{
performDrilling();
targets5[i] = -1;
}
}
}
else
{
maze_sensor(maze_map5,prev_cell, current_cell, 'b', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("MOVING BACK BY ROTATING LEFT 180°");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING BACKWARDS MOVEMENT");
delay(3000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets5) / sizeof(targets5[0]); i++)
{
if (targets5[i] == current_cell)
{
performDrilling();
targets5[i] = -1;
}
}
}
}
}
}
Serial.print("Current cell: ");
Serial.println(current_cell);
// checking if all spots drilled
for (int i = 0; i < sizeof(targets5) / sizeof(targets5[0]); i++)
{
if (targets5[i] != -1)
{
running = true;
break;
}
else
{
running = false;
}
}
}
/////////////////////////////////////////////////////////////////////////////////////
Serial.print("Final cell of map: ");
Serial.println(current_cell);
delay(2000);
Serial.println("All sites mined");
Serial.println("moving to map 6");
delay(2000);
Serial.print("Starting Cell: ");
Serial.println(current_cell);
////////////////////////////////////////////////////////////////////////////////
running = true;
while (running)
{
maze_sensor(maze_map6,prev_cell, current_cell, 'r', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && !(prev_cell == 11 && current_cell == 12))
{
Serial.println("ROTATING RIGHT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 35.5°, N2 = 47.2°, N3 = 0°, N4 = -47.2°, N5 = -35.5°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets6) / sizeof(targets6[0]); i++)
{
if (targets6[i] == current_cell)
{
performDrilling();
targets6[i] = -1;
}
}
}
else
{
maze_sensor(maze_map6,prev_cell, current_cell, 'f', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true && current_cell != 12)
{
Serial.println("MOVING FORWARDS");
delay(1000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets6) / sizeof(targets6[0]); i++)
{
if (targets6[i] == current_cell)
{
performDrilling();
targets6[i] = -1;
}
}
}
else
{
maze_sensor(maze_map6,prev_cell, current_cell, 'l', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("ROTATING LEFT");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING FORWARD MOVEMENT");
delay(2000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets6) / sizeof(targets6[0]); i++)
{
if (targets6[i] == current_cell)
{
performDrilling();
targets6[i] = -1;
}
}
}
else
{
maze_sensor(maze_map6,prev_cell, current_cell, 'b', prev_cell2, current_cell2, flag_move2);
if (flag_move2 == true)
{
Serial.println("MOVING BACK BY ROTATING LEFT 180°");
Serial.println("STEERING ANGLES AS FOLLOWS: N1 = 47.2°, N2 = 35.5°, N3 = 0°, N4 = -35.5°, N5 = -47.2°, N6 = 0°");
Serial.println("ROTATION COMPLETE, EXECUTING BACKWARDS MOVEMENT");
delay(3000);
prev_cell = prev_cell2;
current_cell = current_cell2;
// checking to see if spot can be drilled
for (int i = 0; i < sizeof(targets6) / sizeof(targets6[0]); i++)
{
if (targets6[i] == current_cell)
{
performDrilling();
targets6[i] = -1;
}
}
}
}
}
}
Serial.print("Current cell: ");
Serial.println(current_cell);
// checking if all spots drilled
for (int i = 0; i < sizeof(targets6) / sizeof(targets6[0]); i++)
{
if (targets6[i] != -1)
{
running = true;
break;
}
else
{
running = false;
}
}
}
Serial.println("All sites mined");
Serial.println("All maps complete");
while (true){
}
//*/
}