Created
July 9, 2017 21:16
-
-
Save Menginventor/b942fafecc1d93d67ba122d66e8da1f3 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#define map_size 64 | |
#define traceback_buf_size 16 | |
//char traceback_buf[traceback_buf_size]; | |
int freeRam () { | |
extern int __heap_start, *__brkval; | |
int v; | |
return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); | |
} | |
class robot { | |
public: | |
char head_dir (char num) { | |
while (num < 0)num += 4; | |
while (num > 3)num -= 4; | |
return num; | |
} | |
char dx_dir(char _dir) { //cosine | |
_dir = head_dir(_dir); | |
if (_dir == 0)return 1; | |
if (_dir == 2)return -1; | |
return 0; | |
} | |
char dy_dir(char _dir) { //sine | |
_dir = head_dir(_dir); | |
if (_dir == 1)return 1; | |
if (_dir == 3)return -1; | |
return 0; | |
} | |
class state { | |
char head_dir (char num) { | |
while (num < 0)num += 4; | |
while (num > 3)num -= 4; | |
return num; | |
} | |
public: | |
char pos = 0; | |
char head = 0;//initial heading | |
char front() { | |
return head; | |
} | |
char left() { | |
return head_dir(head + 1); | |
} | |
char right() { | |
return head_dir(head - 1); | |
} | |
char back() { | |
return head_dir(head + 2); | |
} | |
bool operator==(state &comp) { | |
if (pos == comp.pos && head == comp.head)return true; | |
else return false; | |
} | |
bool operator!=(state &comp) { | |
if (pos == comp.pos && head == comp.head)return false; | |
else return true; | |
} | |
}; | |
state crr_state; | |
class maping { | |
public: | |
class map_node { | |
public: | |
char x = -128; | |
char y = -128; | |
char connect[4] = { -1, -1, -1, -1}; // -1 refer to null | |
unsigned int weight[4] = {65535, 65535, 65535, 65535}; | |
void weight_reset() { | |
for (int i = 0; i < 4; i++) { | |
weight [i] = 65535; | |
} | |
} | |
bool is_unvisited() { | |
return connect[0] == -1 || connect[1] == -1 || connect[2] == -1 || connect[3] == -1; | |
} | |
}; | |
char maplength = 0; | |
map_node map_arr[map_size]; | |
char length () { | |
int size_all = sizeof(map_arr); | |
int size_each = sizeof(map_node); | |
return (size_all / size_each); | |
} | |
char _new_ () { | |
char new_node = maplength; | |
maplength++; | |
return new_node; | |
} | |
char find_xy (char _x, char _y) { | |
for (char i = 0; i < map_size; i++) { | |
if (map_arr[i].x == _x && map_arr[i].y == _y) { | |
return i; | |
} | |
} | |
return -1; | |
} | |
}; | |
maping mazemap; | |
char occ[4] = { -1, -1, -1, -1}; //right,up,left,down | |
char gfocc() {//put front sensor read hear // return 0 = clear ,return 1 = occupied | |
Serial.print('F');//demo for simulator | |
while (!Serial.available()); | |
char gdat = Serial.read();//get data | |
return gdat == '1'; | |
} | |
char glocc() { | |
Serial.print('L'); | |
while (!Serial.available()); | |
char gdat = Serial.read();//get data | |
return gdat == '1'; | |
} | |
char grocc() { | |
Serial.print('R'); | |
while (!Serial.available()); | |
char gdat = Serial.read();//get data | |
return gdat == '1'; | |
} | |
void action (char cmd) { | |
if (cmd == 'W') { | |
crr_state.pos = mazemap.map_arr[crr_state.pos].connect[crr_state.head]; | |
} | |
else if (cmd == 'A') { | |
crr_state.head = crr_state.left(); | |
} | |
else if (cmd == 'D') { | |
crr_state.head = crr_state.right(); | |
} | |
else{ | |
Serial.println("err in action"); | |
} | |
Serial.print(cmd); | |
} | |
state new_state(char _pos, char _head) { | |
state result; | |
result.pos = _pos; | |
result.head = _head; | |
return result; | |
} | |
state action_result (char cmd, state _crr) { | |
if (cmd == 'W') { | |
_crr.pos = mazemap.map_arr[_crr.pos].connect[_crr.front()]; | |
} | |
else if (cmd == 'A') { | |
_crr.head = _crr.left(); | |
} | |
else if (cmd == 'D') { | |
_crr.head = _crr.right(); | |
} | |
return _crr; | |
} | |
state back_tracking_result (char cmd, state _crr) {//invert from action result | |
if (cmd == 'W') { | |
_crr.pos = mazemap.map_arr[_crr.pos].connect[_crr.back()]; | |
} | |
else if (cmd == 'D') { | |
_crr.head = _crr.left(); | |
} | |
else if (cmd == 'A') { | |
_crr.head = _crr.right(); | |
} | |
return _crr; | |
} | |
char get_action(state prov, state next) { | |
if (action_result('W', prov) == next)return 'W'; | |
else if (action_result('A', prov) == next)return 'A'; | |
else if (action_result('D', prov) == next)return 'D'; | |
else return '\0'; | |
} | |
void tl() { | |
// Serial.print('A'); | |
action('A'); | |
} | |
void tr() { | |
//Serial.print('D'); | |
action('D'); | |
} | |
void gh() { | |
// Serial.print('W'); | |
action('W'); | |
} | |
void occ_update() { | |
for (char i = 0; i < 4; i++) { | |
occ[i] = -1; | |
} | |
occ[crr_state.front()] = gfocc(); | |
occ[crr_state.right()] = grocc(); | |
occ[crr_state.left()] = glocc(); | |
} | |
char f_occ () { | |
return occ[crr_state.front()]; | |
} | |
char l_occ () { | |
return occ[crr_state.left()]; | |
} | |
char r_occ () { | |
return occ[crr_state.right()]; | |
} | |
void printocc() { | |
for (char i = 0; i < 4; i++) { | |
Serial.print(int(occ[i])); | |
if (i < 3) Serial.print(","); | |
} | |
Serial.println(); | |
} | |
void map_update() { | |
//Serial.println("\nmap update.. at node " + String(int(crr_state.pos))); | |
occ_update(); | |
for (char i = 0; i < 4; i++) { | |
if (occ[i] != -1) { | |
char new_x = mazemap.map_arr[crr_state.pos].x + dx_dir(i); | |
char new_y = mazemap.map_arr[crr_state.pos].y + dy_dir(i); | |
if (occ[i] == 1) { //occupied | |
mazemap.map_arr[crr_state.pos].connect[i] = crr_state.pos; | |
} | |
else if (occ[i] == 0 && mazemap.map_arr[crr_state.pos].connect[i] == -1 ) { | |
char new_node; | |
if ( mazemap.find_xy(new_x, new_y) == -1) { | |
new_node = mazemap._new_(); | |
} | |
else new_node = mazemap.find_xy(new_x, new_y); | |
mazemap.map_arr[crr_state.pos].connect[i] = new_node; | |
mazemap.map_arr[new_node].connect[head_dir(i + 2)] = crr_state.pos; | |
mazemap.map_arr[new_node].x = new_x; | |
mazemap.map_arr[new_node].y = new_y; | |
Serial.println("create node " + String(int(new_node)) + " , x = " + String(int(new_x)) + " , y = " + String(int(new_y))); | |
} | |
else { | |
//Serial.println("We have some Error here!!"); | |
} | |
} | |
} | |
} | |
void startup() { | |
crr_state.pos = 0;//init | |
crr_state.head = 0; | |
char new_node = mazemap._new_ (); | |
mazemap.map_arr[crr_state.pos].x = 0; | |
mazemap.map_arr[crr_state.pos].y = 0; | |
map_update(); | |
if (glocc() == '0') { //left is clear | |
tl(); | |
} | |
else tr(); | |
map_update(); | |
} | |
unsigned int get_weight_state(state _crr) { | |
return mazemap.map_arr[_crr.pos].weight[_crr.head]; | |
} | |
void set_weight_state(state _crr, unsigned int _weight) { | |
mazemap.map_arr[_crr.pos].weight[_crr.head] = _weight; | |
} | |
void run() { | |
const unsigned int W_weight = 5; | |
const unsigned int A_weight = 3; | |
const unsigned int D_weight = 3; | |
//Serial.println("\nrun"); | |
map_update(); | |
Serial.println("now on node = " + String(int(crr_state.pos)) + " , x = " + String(int(mazemap.map_arr[crr_state.pos].x)) + " ,y = " + String(int(mazemap.map_arr[crr_state.pos].y))); | |
//Serial.print("that connect to"); | |
//for (char i = 0; i < 4; i++) { | |
//Serial.print((int) mazemap.map_arr[crr_state.pos].connect[i]); | |
//if (i < 3)Serial.print(","); | |
//} | |
//Serial.println(); | |
if (mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.front()]].is_unvisited() ) { | |
/* | |
Serial.print("on node = " + String( (int)mazemap.map_arr[crr_state.pos].connect[crr_state.front()]) + " , we found "); | |
for (char i = 0; i < 4; i++) { | |
Serial.print((int)mazemap.map_arr[ mazemap.map_arr[crr_state.pos].connect[crr_state.front()]].connect[i]); | |
if (i < 3)Serial.print(","); | |
} | |
Serial.print(" and "); | |
Serial.println((int)mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.front()]].is_unvisited()); | |
*/ | |
gh(); | |
} | |
else if (mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.left()]].is_unvisited() ) { | |
tl(); | |
} | |
else if (mazemap.map_arr[mazemap.map_arr[crr_state.pos].connect[crr_state.right()]].is_unvisited() ) { | |
tr(); | |
} | |
else { | |
Serial.println("start searching!!"); | |
state goal_state = new_state(-1, 0); | |
char unvisited = 0; | |
for (char i = 0; i < mazemap.maplength; i++ ) { | |
mazemap.map_arr[i].weight_reset(); | |
if(mazemap.map_arr[i].is_unvisited())unvisited++; | |
} | |
Serial.println("found "+String(int(unvisited))+" unvisited nodes"); | |
if(unvisited == 0){ | |
Serial.println("exploration complete, stopprogram"); | |
while(1); | |
} | |
mazemap.map_arr[crr_state.pos].weight[crr_state.head] = 0; | |
unsigned int base_weight = 0; | |
///some loop here | |
bool search_loop = true; | |
while (1) { | |
//Serial.println("start serch at weight = " + String(base_weight)); | |
for (char _pos = 0; _pos < mazemap.maplength; _pos++ ) { | |
if (goal_state.pos != -1)break; | |
for (char _head = 0; _head < 4; _head++) { | |
state base_state = new_state(_pos, _head); | |
//Serial.println("searching at state ,pos = "+String(int(_pos)) + " ,head = "+String(int(_head))+ " ,weight = "+String(get_weight_state(base_state))); | |
if (get_weight_state(base_state) == base_weight) { | |
//Serial.println("found base weight = " + String(get_weight_state(base_state)) + "base state , node = " + String(int(base_state.pos)) + " , head = " + String(int(base_state.head))); | |
state w_state = action_result ('W', base_state) ; | |
state a_state = action_result ('A', base_state) ; | |
state d_state = action_result ('D', base_state) ; | |
if (W_weight + base_weight < get_weight_state(w_state)) { | |
set_weight_state(w_state, W_weight + base_weight); | |
if (mazemap.map_arr[w_state.pos].is_unvisited()) { | |
goal_state = w_state; | |
break; | |
} | |
} | |
if (A_weight + base_weight < get_weight_state(a_state)) { | |
set_weight_state(a_state, A_weight + base_weight); | |
if (mazemap.map_arr[a_state.pos].is_unvisited()) { | |
goal_state = a_state; | |
break; | |
} | |
} | |
if (D_weight + base_weight < get_weight_state(d_state)) { | |
set_weight_state(d_state, D_weight + base_weight); | |
if (mazemap.map_arr[d_state.pos].is_unvisited()) { | |
goal_state = d_state; | |
break; | |
} | |
} | |
}//"if (mazemap.map_arr[_pos].weight[_head] == base_weight)" 's statement | |
} | |
}//find match weight with base weight statement | |
if (goal_state.pos == -1) { | |
unsigned int minimum_weight = 65535; | |
for (char _pos = 0; _pos < mazemap.maplength; _pos++ ) { | |
for (char _head = 0; _head < 4; _head++) { | |
state base_state = new_state(_pos, _head); | |
unsigned int _weight = get_weight_state(base_state) ; | |
if (_weight > base_weight && _weight < minimum_weight) { | |
//Serial.println("debug : update state, pos = "+String(int(base_state.pos))+" head = "+String(int(base_state.head))); | |
minimum_weight = _weight; | |
} | |
} | |
} | |
Serial.println("debug : minimum weight = "+String(minimum_weight)); | |
if (minimum_weight == base_weight) { | |
Serial.println("\n!!!!!!stop!!!!!!"); | |
while (1); | |
} | |
else if(minimum_weight == 65535){ | |
Serial.println("stop, exploration complete!"); | |
while (1); | |
} | |
else { | |
base_weight = minimum_weight; | |
} | |
} | |
else {//next pos != -1 , found next pos to go. | |
//Serial.println("\nwholaa ,we found next target at node = " + String(int( goal_state.pos)) + " , head = " + String(int(goal_state.head)) + " , at weight = " + String(base_weight)); | |
//begin backtracking | |
Serial.println("ram:" + String(freeRam())); | |
char traceback_buf_index = 0; | |
char traceback_buf[traceback_buf_size]; | |
Serial.println("ram:" + String(freeRam())); | |
for (char i = 0; i < traceback_buf_size; i++) { | |
traceback_buf[i] = '\0'; | |
} | |
state backtracking = goal_state; | |
//Serial.println("back-tracking start ,node = " + String(int(backtracking.pos)) +" ,head = "+ String(int(backtracking.head))); | |
while ( backtracking != crr_state) { | |
state b_w = back_tracking_result('W', backtracking); | |
state b_a = back_tracking_result('A', backtracking); | |
state b_d = back_tracking_result('D', backtracking); | |
unsigned int w_b_w = get_weight_state(b_w); | |
unsigned int w_b_a = get_weight_state(b_a); | |
unsigned int w_b_d = get_weight_state(b_d); | |
if ( w_b_w <= w_b_a && w_b_w <= w_b_d) { | |
backtracking = b_w; | |
traceback_buf[traceback_buf_index] = 'W'; | |
} | |
else if (w_b_a <= w_b_d) { | |
backtracking = b_a; | |
traceback_buf[traceback_buf_index] = 'A'; | |
} | |
else { | |
backtracking = b_d; | |
traceback_buf[traceback_buf_index] = 'D'; | |
} | |
// Serial.println("back-tracking node = " + String(int(backtracking.pos)) +" ,head = "+ String(int(backtracking.head))); | |
//Serial.println("back-tracking cmd = "+ String(char(traceback_buf[traceback_buf_index]+32))); | |
traceback_buf_index ++; | |
if (traceback_buf_index >= traceback_buf_size)traceback_buf_index -= traceback_buf_size; | |
} | |
for (char i = 0; i < traceback_buf_size; i++) { | |
traceback_buf_index--; | |
if (traceback_buf_index <0)traceback_buf_index += traceback_buf_size; | |
if(traceback_buf[traceback_buf_index]!= '\0'){ | |
action(traceback_buf[traceback_buf_index]); | |
delay(200); | |
} | |
else break; | |
} | |
// map_update(); | |
break; | |
//Serial.println("temporary stop!! 000"); | |
//while (1); | |
} | |
}//while(true) statement | |
} | |
} | |
}; | |
robot mazerunner; | |
void setup() { | |
// put your setup code here, to run once: | |
Serial.begin(9600); | |
Serial.println("robot start!"); | |
Serial.println(sizeof(robot)); | |
while (!Serial.available()); | |
Serial.read(); | |
mazerunner.startup(); | |
} | |
void loop() { | |
// put your main code here, to run repeatedly: | |
mazerunner.run(); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment