void getPathComplicated(signed char x1, signed char y1, signed char xDestination, signed char yDestination) { if((x1==xDestination)&&(y1==yDestination)) { stopBoth(); ecrobot_sound_tone(100,500,100); systick_wait_ms(2000); endAll(); addToPath(none); return; } //display_currentNode(); //systick_wait_ms(1000); /* display_clear(1); display_goto_xy(0,0); display_string("gPC called"); display_goto_xy(0,1); display_int((int)x1,1); display_goto_xy(3,1); display_int((int)y1,1); display_goto_xy(0,2); display_int((int)xDestination,1); display_goto_xy(3,2); display_int((int)yDestination,1); display_update(); */ //systick_wait_ms(1000); signed char xCurrent; signed char yCurrent; signed char startReached = 0; signed char endReached = 0; signed char xDistance= xDestination - x1; signed char yDistance= yDestination - y1; signed char yD2=0; signed char xD2=0; if(yDistance<0){yD2=-yDistance;}else{yD2=yDistance;} if(xDistance<0){xD2=-xDistance;}else{xD2=xDistance;} char maxIterations = (xD2+yD2)/2+40;//40 is safety padding char cntLoops; if(maxIterations<0) maxIterations=-maxIterations; //************************************** MAIN BODY ****** int x,y; for(x=0;x<=12;x++) { for(y=0;y<=12;y++) { mazeStart[x][y]=0; mazeEnd[x][y]=0; } } mazeStart[x1][y1]=50; int colX=0; int rowY=0; mazeEnd[xDestination][yDestination]=50; signed char l,r,t,d; for(cntLoops=0; cntLoops<=maxIterations*2; cntLoops++) { if((startReached==1) && (endReached==1)) { cntLoops = (maxIterations*2)+1; } //***************************set mazeStart's values checks now if values overflow for(colX=0;colX<=12;colX++) { for(rowY=12;rowY>=0;rowY--) { if(mazeStart[xDestination][yDestination]!=0){endReached = 1;break;}; //check if the field has the path to the adjecant field l=r=t=d=0; if((hasDirection(left,colX,rowY)==1) && (mazeStart[colX-1][rowY] != 0)) { if(colX>0) l=mazeStart[colX-1][rowY]-1; } if((hasDirection(right,colX,rowY)==1) && (mazeStart[colX+1][rowY] != 0)) { if(colX<12) r=mazeStart[colX+1][rowY]-1; } if((hasDirection(top,colX,rowY)==1) && (mazeStart[colX][rowY+1] != 0)) { if(rowY<12) t=mazeStart[colX][rowY+1]-1; } if((hasDirection(down,colX,rowY)==1) && (mazeStart[colX][rowY-1] != 0)) { if(rowY>0) d=mazeStart[colX][rowY-1]-1; } mazeStart[colX][rowY]=getLargestValue(l,t,r,d,mazeStart[colX][rowY]); } } //*************************** set mazeEnd's values for(colX=0;colX<=12;colX++) { for(rowY=12;rowY>=0;rowY--) { if((mazeEnd[x1][y1]!=0)){startReached = 1;break;}; l=r=t=d=0; if((hasDirection(left,colX,rowY)==1) && (mazeEnd[colX-1][rowY] != 0)) { if(colX>0) l=mazeEnd[colX-1][rowY]-1; } if((hasDirection(right,colX,rowY)==1) && (mazeEnd[colX+1][rowY] != 0)) { if(colX<12) r=mazeEnd[colX+1][rowY]-1; } if((hasDirection(top,colX,rowY)==1) && (mazeEnd[colX][rowY+1] != 0)) { if(rowY<12) t=mazeEnd[colX][rowY+1]-1; } if((hasDirection(down,colX,rowY)==1) && (mazeEnd[colX][rowY-1] != 0)) { if(rowY>0) d=mazeEnd[colX][rowY-1]-1; } mazeEnd[colX][rowY]=getLargestValue(l,t,r,d,mazeEnd[colX][rowY]); } } } //prnt values both matrixes now added into mazeStart int ty, tx; tx=ty=0; for(ty=12; ty>=0; ty--) { for(tx=0; tx<=12; tx++) { mazeStart[tx][ty]=mazeStart[tx][ty]+mazeEnd[tx][ty]; mazeEnd[tx][ty]=0; } } //now add way of highest numbers to path so it can be printed or driven xCurrent = x1; yCurrent = y1; signed char xDisLeft = xDistance; signed char yDisLeft = yDistance; signed char pathValue = mazeStart[x1][y1]; mazeEnd[xCurrent][yCurrent]=1; //repurposing mazeE to store already visited Nodes if(endReached==1) { while((xCurrent!=xDestination) || (yCurrent != yDestination)) { if((xDisLeft>0)||(((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue||(hasDirection(left,xCurrent,yCurrent)==0)))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0)))) { if(hasDirection(right,xCurrent,yCurrent) && (mazeStart[xCurrent+1][yCurrent]==pathValue) && (mazeEnd[xCurrent+1][yCurrent]==0)) { addToPath(right); mazeEnd[xCurrent][yCurrent]=1; xCurrent++; xDisLeft--; } } if((xCurrent==xDestination) && (yCurrent == yDestination)) break; if(xDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0)))) { if(hasDirection(left,xCurrent,yCurrent) && (mazeStart[xCurrent-1][yCurrent]==pathValue) && (mazeEnd[xCurrent-1][yCurrent]==0)) { addToPath(left); mazeEnd[xCurrent][yCurrent]=1; xCurrent--; xDisLeft++; } } if((xCurrent==xDestination) && (yCurrent == yDestination)) break; if(yDisLeft>0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0)))) { if(hasDirection(top,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent+1]==pathValue) && (mazeEnd[xCurrent][yCurrent+1]==0)) { addToPath(top); mazeEnd[xCurrent][yCurrent]=1; yCurrent++; yDisLeft--; } } if((xCurrent==xDestination) && (yCurrent == yDestination)) break; if(yDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0)))) { if(hasDirection(down,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent-1]==pathValue) && (mazeEnd[xCurrent][yCurrent-1]==0)) { addToPath(down); mazeEnd[xCurrent][yCurrent]=1; yCurrent--; yDisLeft++; } } if((xCurrent==xDestination) && (yCurrent == yDestination)) break; } } /* display_goto_xy(0,3); display_string("ret:"); display_goto_xy(4,3); display_int(pathStorage[1],1); display_update();*/ //systick_wait_ms(2000); }
/* * Stops the robot, whatever direction it is moving. */ void halt(){ stopBoth(); stopBothBackward(); }