Final Project Document - oboywei/ee240500 GitHub Wiki
本project目標為使車子第一次行走一迷宮時，會自動偵測障礙物並尋找新方向。行走完之後會建立一個虛擬的路線圖，並將其簡化。
之後在板子上經由XBee輸入一指令後，車子會自動依據簡化路線圖行駛，以最快的路線走出迷宮。
###遇到困境：
- 車子轉彎角度不理想，可能是因為在旋轉完之後沒有讓servo停止，因此若超過pause的時間後在到達回圈終點前還有程式馬要執行就會延長旋轉的時間。
- 無法將資料傳進SD card，可能是fopen的指令跟Xbee的指令互相打架（兩者同時存在時程式會卡住，拿掉其中之一就可以跑）
- Demo的時候simpleide因不明原因當機，導致Demo時間被嚴重影響。在之前重來沒有發生此狀況，到現在仍趕到非常不明白為何會發生，這使的當下無法Demo完所有成果也讓我感到非常遺憾
＃＃＃程式碼：
#include "simpletools.h"
#include "servo.h"
#include "servodiffdrive.h"
#include "fdserial.h"
#include "talk.h"
#include "ping.h"
#define PIN_SOUND 26 // pin for the speaker, default=26
#define PIN_XBEE_RX 0
#define PIN_XBEE_TX 1
#define PIN_SERVO_LEFT 14
#define PIN_SERVO_RIGHT 15
#define STANDARD_MOTOR 16
#define PING 17
#define mazeHeight 15
#define mazeWidth 15
#define turnTime 500
#define runTime 400
#define endX 1
#define endY 1
#define startX 8
#define startY 15
char char2lowercase (char c);
int DO = 22, CLK = 23, DI = 24, CS = 25;
char mazeArray[mazeWidth][mazeHeight];
int endpointX = endX;
int endpointY = endY;
int main (void) {
fdserial *xbee;
xbee = fdserial_open(PIN_XBEE_RX, PIN_XBEE_TX, 0, 9600);
drive_pins(PIN_SERVO_LEFT, PIN_SERVO_RIGHT);
`pause(1000);`
`putchar(CLS);`
`fdserial_rxFlush(xbee);/////////`
int direction = 0; //0:forward, -1:left, 1:right, 2,-2:back,
int yaxis = mazeHeight; //starting point
int xaxis = (mazeWidth)/2;
int walk = 0; //0:stop 1:forward -1:backward
int cmDist;
for(int i=0; i<mazeWidth; i++){ //fill the maze with wall
for(int j=0; j<mazeHeight; j++){
mazeArray[i][j]='2';
}
}
servo_angle(STANDARD_MOTOR, 900);
int button = input(9);
while(!button){
print("waiting for the button to be pressed\n");
button = input(9);
pause(1000);
}
while (1) {
cmDist = ping_cm(PING);
print("cmDist = %d\n", cmDist);
if(cmDist <=10){
drive_speeds(0, 0);
servo_angle(STANDARD_MOTOR, 1800);//turn the head to left
pause(1000);
cmDist = ping_cm(PING);
print("in direction -1, cmDist = %d\n", cmDist);
pause(1000);
if(cmDist <=20){
servo_angle(STANDARD_MOTOR, 0);//turn the head to right
pause(1000);
cmDist = ping_cm(PING);
print("in direction 1, cmDist = %d\n", cmDist);
pause(1000);
if(cmDist <= 20){
servo_angle(STANDARD_MOTOR, 900);
drive_speeds(-50,50);//turn back//////////////////
pause(turnTime*2);
direction -=2;
walk = 1;
}
else{
servo_angle(STANDARD_MOTOR, 900);
drive_speeds(50, -50); //turn right////////////////////
pause(turnTime-200);
direction +=1;
walk =1;
}
}
else{
servo_angle(STANDARD_MOTOR, 900);
drive_speeds(-50, 50); //turn left////////////////////
pause(turnTime-50);
direction -=1;
walk =1;
}
}
else{
drive_speeds(50, 50);///////////////////////
walk =1;
}
pause(runTime);
`int ch = fdserial_rxCheck(xbee); `
`ch = char2lowercase(ch);`
`fdserial_rxFlush(xbee);`
`switch ((char) ch) { //for keyboard controll`
`case 'd':`
`print("Stop\n");`
`drive_speeds(0, 0);`
`walk = 0;`
`pause(5000);`
`break;`
`case 'p':`
`print("solve the maze\n");`
`drive_speeds(0,0);`
`endpointX=xaxis+1;`
`endpointY=yaxis+1;`
`print("endpoint:(%d,%d)\n",endpointX,endpointY);`
`runMaze();`
`pause(5000);`
`xaxis = startX+1;`
`yaxis = startY+1;`
/* sd_mount(DO, CLK, DI, CS);
FILE *fp = fopen("mazeArray.txt","w");///////////////////////////
if(fp){
print("in main, success to open file\n");
}
else if(!fp){
print("in main, fail to open file\n");
}
`for(int i = 0; i<mazeWidth; i++){`
`for(int j = 0; j<mazeHeight; j++){`
`fprintf(fp, "%c", mazeArray[i][j]);`
`}`
`fprintf(fp, "\n"); `
`} `
`fprintf(fp, "\n"); `
`fclose(fp); `
*/
pause(5000);
`default:`
`//print("Invalid Command\n");`
`//pause(700);`
`break;`
`}`
`if(direction > 2){direction -= 4;}`
`if(direction < -2){direction +=4;}`
`if(walk == 1){`
`if(direction == 0){yaxis -= 1;}`
`else if(direction == -1){xaxis -=1;}`
`else if(direction == 1){xaxis +=1;}`
`else if(direction == 2 || direction == -2){yaxis += 1;}`
`}`
`if(walk == -1){`
`if(direction == 0){yaxis += 1;}`
`else if(direction == -1){xaxis +=1;}`
`else if(direction == 1){xaxis -=1;}`
`else if(direction == 2 || direction == -2){yaxis -= 1;}`
`}`
`if(yaxis <0){yaxis = 0;}`
`if(yaxis >14){yaxis = 14;}`
`if(xaxis <0){xaxis = 0;}`
`if(xaxis >14){xaxis = 14;}`
`print("direction = %d \n",direction);`
`print("xaxis = %d, yaxis = %d \n", xaxis, yaxis);`
`mazeArray[yaxis][xaxis]='0'; `
`for(int i=0;i<mazeWidth;i++){`
`for(int j=0;j<mazeHeight;j++){`
`print("%c",mazeArray[i][j]);`
`}`
`print("\n");`
`}`
// pause(runTime);
// fdserial_txChar(xbee, ch);
// fdserial_txFlush(xbee);
}
}
char char2lowercase (char c) {
return 'A'<=c && c<='Z' ? c+32 : c;
}
struct stack_node{
int element_x;
int element_y;
struct stack_node *next;
};
struct stack{
struct stack_node *top;
};
void push (int elem_x, int elem_y, struct stack *s1){
struct stack_node *new_node = (struct stack_node *)malloc(sizeof(struct stack_node));
new_node->next = s1->top;
new_node->element_x = elem_x -1;
new_node->element_y = elem_y -1;
s1->top = new_node;
}
void show_stack(struct stack *s, int x, int y){
struct stack_node *tmp = s->top;
while( tmp != NULL){
print("(%d,%d)", tmp->element_x, tmp->element_y);
if(tmp->element_x == x-1 && tmp->element_y == y-1){
break;
}
print("->");
tmp = tmp->next;
}
}
void stack_destroy(struct stack *s){
struct stack_node *tmp;
while( tmp != NULL){
tmp = s->top->next;
free(s->top);
s->top = tmp;
}
}
void runMaze(){
struct stack path;
path.top = NULL;
int i, j, k, x, y, count;
char c;
// FILE *fp;
// fp = fopen("input.txt", "r");
//fscanf(fp, "%d %d", &x, &y);
x=mazeWidth;
y=mazeHeight;
//Each side length of maze is increased by 2, for putting a layer of "wall" around the maze
char **maze = (char**)malloc((y+2)*sizeof(char*));
for(i=0; i<y+2; i++){
maze[i] = (char*)malloc((x+2)*sizeof(char));
}
`//Fill in maze array with "walls" (1)`
`for(i=0; i<y+2; i++){`
`for(j=0; j<x+2; j++){`
`maze[i][j] = '2';`
`}`
`}`
`for(i=0; i<y; i++){`
`for(j=0; j<x; j++){`
`maze[i+1][j+1] = mazeArray[i][j]; `
`}`
`}`
for(i=0;i<y;i++){
for(j=0;j<x;j++){
print("%c",maze[i+1][j+1]);
}
print("\n");
}
`//If exit is wall, then there is no way out.`
`if(maze[endpointY][endpointX] == '2'){`
`print("No way out!1");`
`}`
`else{`
`//If a point has less than 2 sides with open space, then it is a dead end. After filling in all dead ends, the space left is the path.`
`//Entrance and exit must stay open `
`for(k=0; k<(x*y); k++){`
`for(i=1; i<y+1; i++){`
`for(j=1; j<x+1; j++){`
`count = 0;`
`if(maze[i + 1][j] == '0'){`
`count++;`
`}`
`if(maze[i - 1][j] == '0'){`
`count++;`
`}`
`if(maze[i][j + 1] == '0'){`
`count++;`
`}`
`if(maze[i][j - 1] == '0'){`
`count++;`
`}`
`if(count < 2 ){`
`maze[i][j] = '2';`
`}`
`maze[endpointY][endpointX] = '0';`
`maze[startY][startX] = '0';`
`}`
`}`
`}`
print("\n");
for(i=0;i<y;i++){ //print the solved maze routh
for(j=0;j<x;j++){
print("%c",maze[i+1][j+1]);
}
print("\n");
}
//run the solved maze routh
int x_pos = startX;
int y_pos = startY;
int rundirection=0;
int pastdirection=0;
while(1){///////
if( x_pos == endpointX && y_pos == endpointY){
drive_speeds(0, 0);
freqout(4, 500, 1568);
pause(10);
freqout(4, 500, 1318);
pause(10);
freqout(4, 1000, 1318);
pause(10);
freqout(4, 500, 1397);
pause(10);
freqout(4, 500, 1175);
pause(10);
freqout(4, 1000, 1175);
pause(10);
freqout(4, 500, 1046);
pause(10);
freqout(4, 500, 1175);
pause(10);
freqout(4, 500, 1318);
pause(10);
freqout(4, 500, 1397);
pause(10);
freqout(4, 500, 1568);
pause(10);
freqout(4, 500, 1568);
pause(10);
freqout(4, 1000, 1568);
break;
}
else if( maze[y_pos - 1][x_pos] == '0'){ //up
maze[y_pos][x_pos] = '1';
y_pos = y_pos-1;
pastdirection=rundirection;
rundirection = 0;
}
else if( maze[y_pos][x_pos -1] == '0'){ //left
maze[y_pos][x_pos] = '1';
x_pos = x_pos -1;
pastdirection=rundirection;
rundirection = -1;
}
else if( maze[y_pos + 1][x_pos] == '0'){ //down
maze[y_pos][x_pos] = '1';
y_pos = y_pos+1;
pastdirection=rundirection;
rundirection = 2;
}
else if( maze[y_pos][x_pos +1] == '0'){ //right
maze[y_pos][x_pos] = '1';
x_pos = x_pos +1;
pastdirection=rundirection;
rundirection = 1;
}
else{
print("No way out!2");
break;
}
print("\n");
for(i=0;i<y;i++){ //print the running maze routh
for(j=0;j<x;j++){
print("%c",maze[i+1][j+1]);
}
print("\n");
}
`if(rundirection == pastdirection){`
`drive_speeds(50, 50);`
`pause(100);`
`}`
`else if(rundirection == pastdirection-1 || rundirection == pastdirection+3){//turnleft`
`drive_speeds(0, 0);pause(100);`
`drive_speeds(-50, 50);`
`pause(turnTime*2-50);`
`drive_speeds(50, 50);`
`pause(runTime);`
`}`
`else if(rundirection == pastdirection+1 || rundirection == pastdirection-3){//turnright`
`drive_speeds(0, 0);pause(100);`
`drive_speeds(50, -50);`
`pause(turnTime*2-200);`
`drive_speeds(50, 50);`
`pause(runTime);`
`}`
`else if(rundirection == pastdirection-2 || rundirection == pastdirection+2){//turnback`
`drive_speeds(0, 0);pause(100);`
`drive_speeds(-50, 50);`
`pause(turnTime*4);`
`drive_speeds(50, 50);`
`pause(runTime);`
`} `
`pause(runTime);`
`}`
`show_stack(&path, endpointX, endpointY);`
`free(maze);`
`stack_destroy(&path);`
}
}