API tools faq
paste
Advertisement
Guest User

robot

a guest
Dec 4th, 2016
140
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Robots 18.33 KB | None | 0 0
raw download clone embed print report
  1. #pragma config(Sensor, S1,     L,              sensorEV3_Color)
  2. #pragma config(Sensor, S2,     sonar,          sensorEV3_Ultrasonic)
  3. #pragma config(Sensor, S3,     R,              sensorEV3_Color)
  4. #pragma config(Sensor, S4,     color,          sensorLightActive)
  5. #pragma config(Motor,  motorB,          L,             tmotorEV3_Large, PIDControl, encoder)
  6. #pragma config(Motor,  motorC,          R,             tmotorEV3_Large, PIDControl, encoder)
  7. #pragma config(Motor,  motorD,          sonarM,        tmotorEV3_Medium, PIDControl, encoder)
  8. //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  9.  
  10. #define MAX 10
  11. #define dx1 3
  12. #define dy1 3
  13. #define dx2 2
  14. #define dy2 8
  15. #define dx3 7
  16. #define dy3 2
  17. #define thres 27
  18. #define Dthres 20
  19.  
  20. //sturct
  21. typedef struct node{
  22.     bool block;
  23.     short tBlock;                                                                                   //1 Blocks //2 Hedges
  24.     short des,blo;
  25.    
  26.     short fromX,fromY;
  27.     short dist;
  28.     bool know;
  29.  
  30.     short bfromX,bfromY;
  31.     short bdist;
  32.     bool bknow;
  33. }node;
  34.  
  35. //////////////////////////////////////////////////////////////////
  36. //                                                                                                                          //
  37. //                                  global variable declaration                                 //
  38. //                                                                                                                          //
  39. //////////////////////////////////////////////////////////////////
  40. short x,y; //x, y for robot position
  41. short direction; //n : 0 // e : 1 // s : 2 // w : 3
  42. short dx[3] = {3,2,7};
  43. short dy[3] = {3,8,2};
  44. short bx[3] = {0,0,0};
  45. short by[3] = {0,0,0};
  46. bool push = false;
  47. node table[MAX][MAX];
  48.  
  49. //////////////////////////////////////////////////////////////////
  50. //                                                                                                                          //
  51. //                                      function declaration                                            //
  52. //                                                                                                                          //
  53. //////////////////////////////////////////////////////////////////
  54. //void init(){};
  55. //short readBox(){};
  56. //void forward(){};
  57. //void left(){};
  58. //void right(){};
  59. //void trackline(){};   //forward until
  60.  
  61. //void search(){};
  62.  
  63. ////////////////////////// move robot //////////////////////////
  64.  
  65. //void distToPoint(){};
  66. //void clearFindPath(){};
  67. //void nodeToNode(short fx,short fy,short tx,short ty){};
  68.  
  69. //////////////////////////////////////////////////////////////////
  70. //                                                                                                                          //
  71. //                                      function definition                                             //
  72. //                                                                                                                          //
  73. //////////////////////////////////////////////////////////////////
  74. void init(){
  75.     displayTextLine(9, "init");
  76.     short i,j;
  77.     for(i = 0; i < MAX ; i++){
  78.         for(j = 0; j < MAX ; j++){
  79.             table[i][j].block = false;
  80.             table[i][j].tBlock = -1;
  81.             table[i][j].blo = -1;
  82.             table[i][j].des = -1;
  83.  
  84.             table[i][j].fromX = -1;
  85.             table[i][j].fromY = -1;
  86.             table[i][j].dist = -1;
  87.             table[i][j].know = false;
  88.  
  89.             table[i][j].bfromX = -1;
  90.             table[i][j].bfromY = -1;
  91.             table[i][j].bdist = -1;
  92.             table[i][j].bknow = false;
  93.         }
  94.     }
  95.     x = 0;
  96.     y = 0;
  97.     direction = 0;
  98.  
  99.  
  100. }
  101.  
  102. void forward(){
  103.     switch (direction){
  104.         case 0: y++; break;
  105.         case 1: x--; break;
  106.         case 2: y--; break;
  107.         case 3: x++; break;
  108.     }
  109.     if((SensorValue[S1] > thres ) || (SensorValue[S3] > thres)){
  110.         motor[motorB]=29;
  111.         motor[motorC]=30;
  112.         wait1Msec(600);
  113.     }
  114.  
  115.     while(true){
  116.         //displayBigTextLine(1, "Reflected: %d", SensorValue[S1]);
  117.         //displayBigTextLine(2, "Reflected: %d", SensorValue[S3]);
  118.         if((SensorValue[S1] > thres )&&( SensorValue[S3] < thres)){
  119.             //displayBigTextLine(5, "LEFT");
  120.             motor[motorB]=-10;
  121.             motor[motorC]=10;
  122.         }
  123.         else if((SensorValue[S1] < thres )&&( SensorValue[S3] > thres)){
  124. //          displayBigTextLine(5, "RIGHT");
  125.             motor[motorB]=10;
  126.             motor[motorC]=-10;
  127.         }
  128.         else if((SensorValue[S1] < thres )&& (SensorValue[S3] < thres)){
  129. //          displayBigTextLine(5, "forward");
  130.             motor[motorB]=30;
  131.             motor[motorC]=30;
  132.         }
  133.         else if((SensorValue[S1] > thres )&& (SensorValue[S3] > thres)){
  134.             displayBigTextLine(5, "break");
  135.             //motor[motorB]=0;
  136.             //motor[motorC]=0;
  137.             //wait1Msec(300);
  138.             break;
  139.         }
  140.         //if((SensorValue[S1] > thres) || (SensorValue[S3] > thres)) break;
  141.     }
  142.  
  143.     /*while(true){
  144.         if((SensorValue[S1] > thres )&& (SensorValue[S3] > thres)){
  145.             motor[motorB]=30;
  146.             motor[motorC]=30;
  147.         }
  148.         else
  149.             break;
  150.     }*/
  151. }
  152. void robotStop(int t){
  153.     motor[motorB] = 0;
  154.     motor[motorC] = 0;
  155.     wait1Msec(t);
  156. }
  157.  
  158. short pushblock(){
  159.     short read;
  160.  
  161.     motor[motorB] = 30;
  162.     motor[motorC] = 30;
  163.     wait1Msec(750);
  164.     robotStop(150);
  165.  
  166.     read = SensorValue(S4);
  167.  
  168.     robotStop(150);
  169.     motor[motorB] = -30;
  170.     motor[motorC] = -30;
  171.     wait1Msec(750);
  172.  
  173.     push = false;
  174.  
  175.     return read;
  176. }
  177. void left(){
  178.     if(push){
  179.         pushblock();
  180.     }
  181.  
  182.     if(direction == 0) direction = 3;                                                       //change run to left direction
  183.     else direction--;
  184.  
  185.     motor[motorB] = 30;
  186.     motor[motorC] = 30;
  187.     wait1Msec(500);
  188.  
  189.     while(true){
  190.         if(SensorValue(S3) > thres){
  191.             break;
  192.         }
  193.         else{
  194.             motor[motorB] = -13;
  195.             motor[motorC] = 13;
  196.         }
  197.     }
  198.  
  199.     while(true){
  200.         if(SensorValue(S1) > thres){
  201.             break;
  202.         }
  203.         else{
  204.             motor[motorB] = -13;
  205.             motor[motorC] = 13;
  206.         }
  207.     }
  208.     while(SensorValue(S1) > thres){
  209.         motor[motorB] = -13;
  210.         motor[motorC] = 13;
  211.         wait1Msec(50);
  212.     }
  213.     while(SensorValue(S1) < thres || SensorValue(S3) < thres){
  214.         if(SensorValue(S1) < thres && SensorValue(S3) < thres){
  215.             motor[motorB] = -15;
  216.             motor[motorC] = -15;
  217.             wait1Msec(10);
  218.         }
  219.         else if(SensorValue(S1) > thres && SensorValue(S3) < thres){
  220.             motor[motorB] = 0;
  221.             motor[motorC] = -8;
  222.             wait1Msec(10);
  223.         }
  224.         else{                                                                                                               //if(SensorValue(S1) < thres && SensorValue(S3) > thres)
  225.             motor[motorB] = -8;
  226.             motor[motorC] = 0;
  227.             wait1Msec(10);
  228.         }
  229.     }
  230. }
  231.  
  232. void right(){
  233.     if(push){
  234.         pushblock();
  235.     }
  236.  
  237.     if(direction == 3) direction = 0;                                       //change run to right direction
  238.     else direction++;
  239.     motor[motorB] = 30;
  240.     motor[motorC] = 30;
  241.     wait1Msec(500);
  242.  
  243.     while(true){
  244.         if(SensorValue(S1) > thres){
  245.             break;
  246.         }
  247.         else{
  248.             motor[motorB] = 13;
  249.             motor[motorC] = -13;
  250.         }
  251.     }
  252.     while(true){
  253.         if(SensorValue(S3) > thres){
  254.             break;
  255.         }
  256.         else{
  257.             motor[motorB] = 13;
  258.             motor[motorC] = -13;
  259.         }
  260.     }
  261.     while(SensorValue(S3) > thres){
  262.             motor[motorB] = 13;
  263.             motor[motorC] = -13;
  264.             wait1Msec(50);
  265.     }
  266.  
  267.     while(SensorValue(S1) < thres || SensorValue(S3) < thres)
  268.         if(SensorValue(S1) < thres && SensorValue(S3) < thres){
  269.             motor[motorB] = -10;
  270.             motor[motorC] = -14;
  271.             wait1Msec(10);
  272.         }
  273.         else if(SensorValue(S1) > thres && SensorValue(S3) < thres){
  274.             motor[motorB] = 0;
  275.             motor[motorC] = -8;
  276.             wait1Msec(10);
  277.         }
  278.         else{
  279.             motor[motorB] = -8;
  280.             motor[motorC] = 0;
  281.         }
  282. }
  283.  
  284. void turnDirection(short from,short to){
  285.     displayTextLine(8, "from : %d , to : %d",from,to);
  286.     if((from==0&&to==1)||(from==1&&to==2)||(from==2&&to==3)||(from==3&&to==0)){
  287.         right();
  288.     }
  289.     else if((from==0&&to==3)||(from==1&&to==0)||(from==2&&to==1)||(from==3&&to==2)){
  290.         left();
  291.     }
  292.     else if((from==0&&to==2)||(from==1&&to==3)||(from==2&&to==0)||(from==3&&to==1)){
  293.         right();
  294.         right();
  295.     }
  296. }
  297.  
  298.  
  299. ////////////////////////////////// search //////////////////////////////////
  300. bool canLeft(){
  301.     switch (direction){
  302.         case 0: return !table[x+1][y].block;
  303.         case 1: return !table[x][y+1].block;
  304.         case 2: return !table[x-1][y].block;
  305.         case 3: return !table[x][y-1].block;
  306.     }
  307.     return true;            //not reachable
  308. }
  309. bool canCenter(){
  310.     switch (direction){
  311.         case 0: return !table[x][y+1].block;
  312.         case 1: return !table[x-1][y].block;
  313.         case 2: return !table[x][y-1].block;
  314.         case 3: return !table[x+1][y].block;
  315.     }
  316.     return true;            //not reachable
  317. }
  318. bool canRight(){
  319.     switch (direction){
  320.         case 0: return !table[x-1][y].block;
  321.         case 1: return !table[x][y-1].block;
  322.         case 2: return !table[x+1][y].block;
  323.         case 3: return !table[x][y+1].block;
  324.     }
  325.     return true;            //not reachable
  326. }
  327. void search(){
  328.     short block = 7;
  329.     short box = 0;
  330.     short blockColor;
  331.     //short tempY = y;
  332.     //bool box;
  333.     while(block){
  334.         short tempX = x;
  335.         short tempY = y;
  336.         if(SensorValue(sonar) > Dthres){                                    //sonar not found block
  337.             forward();
  338.         }
  339.         else{
  340.             block--;
  341.             blockColor = pushblock();
  342.             if(blockColor > 27){
  343.                 blockColor = 1;
  344.                 bx[box] = x;
  345.             }
  346.             else
  347.                 blockColor = 2;
  348.  
  349.             switch(direction){
  350.                 case 0 : table[x][y+1].block = true;
  351.                                  table[x][y+1].tBlock = blockColor;
  352.                                  if(blockColor == 1)by[box] = y+1;
  353.                                  break;
  354.                 case 2 : table[x][y-1].block = true;
  355.                                  table[x][y-1].tBlock = blockColor;
  356.                                  if(blockColor == 1)by[box] = y-1;
  357.             }
  358.             if(blockColor == 1) box++;
  359.  
  360.  
  361.             if(direction == 0){
  362.                 right();
  363.                 while(x != tempX || y < tempY+1){                       //left first
  364.                     robotStop(10);
  365.                     if(canLeft()){
  366.                         left();
  367.                     }
  368.                     else if(canCenter()){
  369.  
  370.                     }
  371.                     else if(canRight()){
  372.                         right();
  373.                     }
  374.                     else{
  375.                         right();
  376.                         right();
  377.                     }
  378.                     robotStop(10);
  379.                     if(direction == 3 && x+1 == tempX && SensorValue(sonar) < Dthres){
  380.                         block--;
  381.                         blockColor = pushblock();
  382.                         robotStop(10);
  383.                         if(blockColor > 27){
  384.                             blockColor = 1;
  385.                             bx[box] = tempX;
  386.                         }
  387.                         else
  388.                             blockColor = 2;
  389.  
  390.                         table[x+1][y].block = true;
  391.                         table[x+1][y].tBlock = blockColor;
  392.                         if(blockColor == 1) by[box] = y;
  393.  
  394.                         if(blockColor == 1) box++;
  395.                         right();
  396.                     }
  397.                     else{
  398.                         forward();
  399.                     }
  400.                 }
  401.                 right();
  402.             }
  403.             else{
  404.                 left();
  405.                 while(x != tempX || y > tempY-1){                       //left first
  406.                     robotStop(10);
  407.                     if(canRight()){
  408.                         right();
  409.                     }
  410.                     else if(canCenter()){
  411.  
  412.                     }
  413.                     else if(canLeft()){
  414.                         left();
  415.                     }
  416.                     else{
  417.                         right();
  418.                         right();
  419.                     }
  420.                     robotStop(10);
  421.                     if(direction == 3 && x+1 == tempX && SensorValue(sonar) < Dthres){
  422.                         block--;
  423.                         blockColor = pushblock();
  424.                         robotStop(10);
  425.                         if(blockColor > 27){
  426.                             blockColor = 1;
  427.                             bx[box] = tempX;
  428.                         }
  429.                         else
  430.                             blockColor = 2;
  431.  
  432.                         table[x+1][y].block = true;
  433.                         table[x+1][y].tBlock = blockColor;
  434.                         if(blockColor == 1) by[box] = y;
  435.  
  436.                         if(blockColor == 1) box++;
  437.                         left();
  438.                     }
  439.                     else{
  440.                         forward();
  441.                     }
  442.                 }
  443.                 left();
  444.             }
  445.         }
  446.         if(direction==0 && y==MAX-1){
  447.             left();
  448.             forward();
  449.             left();
  450.         }
  451.         else if(direction==2 && y==0){
  452.             right();
  453.             forward();
  454.             right();
  455.         }
  456.     }
  457. }
  458. //move robot
  459. short findNext(short toX,short toY){
  460.     displayTextLine(9, "findNext");
  461.     short k,j;
  462.     short ret = -1;
  463.     short toNodeDist = 555;                                                         //most of possible
  464.     short Dist = 555;               //
  465.     short tempDist;
  466.     for(k = 0; k < MAX ; k++){
  467.         for(j = 0; j < MAX ; j++){
  468.  
  469.             if(!table[k][j].know){
  470.                 tempDist = abs((toX-k))+abs((toY-j));
  471.                 if(table[k][j].dist < Dist && table[k][j].dist != -1){
  472.                     if(tempDist < toNodeDist){
  473.                             toNodeDist = tempDist;
  474.                             Dist = table[k][j].dist;
  475.                             ret = j*MAX + k;
  476.                     }
  477.                 }
  478.             }
  479.         }
  480.     }
  481.     return ret;
  482.     // ret = MAX*y + x
  483.     /*
  484.         to use ret
  485.         x = ret%MAX;
  486.         y = ret/MAX;
  487.     */
  488. }
  489. void clearFindPath(){
  490.     short k,j;
  491.     for(k = 0; k < MAX ; k++){
  492.         for(j = 0; j < MAX ; j++){
  493.             table[k][j].fromX = -1;
  494.             table[k][j].fromY = -1;
  495.             table[k][j].dist = -1;
  496.             table[k][j].know = false;
  497.         }
  498.     }
  499. }
  500.  
  501. void findpath(short fx,short fy,short tx,short ty){
  502.     displayTextLine(9, "findpath");
  503.     short k,j;
  504.     short next;
  505.     table[fx][fy].know = false;
  506.     table[fx][fy].dist = 0;
  507.  
  508.     while(true){
  509.         next = findNext(tx,ty);
  510.         if(next == -1) break;
  511.  
  512.         k = next%MAX;
  513.         j = next/MAX;
  514.         table[k][j].know = true;
  515.         if(k == tx && j == ty) break;
  516.  
  517.         //check north
  518.         if(j < MAX-1){
  519.             if(!table[k][j+1].block && !table[k][j+1].know){
  520.                     if(table[k][j+1].dist == -1 || table[k][j+1].dist > table[k][j].dist+1){
  521.                         table[k][j+1].dist = table[k][j].dist+1;
  522.                         table[k][j+1].fromX = k;
  523.                         table[k][j+1].fromY = j;
  524.                     }
  525.             }
  526.         }
  527.         //check east
  528.         if(k > 0){
  529.             if(!table[k-1][j].block && !table[k-1][j].know){
  530.                     if(table[k-1][j].dist == -1 || table[k-1][j].dist > table[k][j].dist+1){
  531.                         table[k-1][j].dist = table[k][j].dist+1;
  532.                         table[k-1][j].fromX = k;
  533.                         table[k-1][j].fromY = j;
  534.                     }
  535.             }
  536.         }
  537.         //check south
  538.         if(j > 0){
  539.             if(!table[k][j-1].block && !table[k][j-1].know){
  540.                     if(table[k][j-1].dist == -1 || table[k][j-1].dist > table[k][j].dist+1){
  541.                         table[k][j-1].dist = table[k][j].dist+1;
  542.                         table[k][j-1].fromX = k;
  543.                         table[k][j-1].fromY = j;
  544.                     }
  545.             }
  546.         }
  547.         //check west
  548.         if(k < MAX-1){
  549.             if(!table[k+1][j].block && !table[k+1][j].know){
  550.                     if(table[k+1][j].dist == -1 || table[k+1][j].dist > table[k][j].dist+1){
  551.                         table[k+1][j].dist = table[k][j].dist+1;
  552.                         table[k+1][j].fromX = k;
  553.                         table[k+1][j].fromY = j;
  554.                     }
  555.             }
  556.         }
  557.     }//while
  558. }
  559. void nToN(short fx,short fy,short tx,short ty){
  560.     displayTextLine(9, "nTON");
  561.     if(ty>fy){ //go to north
  562.         turnDirection(direction,0);
  563.     }
  564.     else if(ty<fy){ //go to south
  565.         turnDirection(direction,2);
  566.     }
  567.     else if(tx>fx){ //go to east
  568.         turnDirection(direction,3);
  569.     }
  570.     else{   //go to west
  571.         turnDirection(direction,1);
  572.     }
  573.     forward();
  574. }
  575.  
  576. void toNode(short fx,short fy,short tx,short ty){
  577.     displayTextLine(9, "toNode");
  578.         if(!(tx == fx && ty == fy)){
  579.             toNode(fx,fy,table[tx][ty].fromX,table[tx][ty].fromY);
  580.             nToN(table[tx][ty].fromX,table[tx][ty].fromY,tx,ty);
  581.         }
  582. }
  583.  
  584. void nodeToNode(short fx,short fy,short tx,short ty){
  585.     displayTextLine(9, "nodeToNode");
  586.     findpath(fx,fy,tx,ty);
  587.  
  588.     toNode(fx,fy,tx,ty);
  589.     clearFindPath();
  590. }
  591.  
  592. void matchblock(){
  593.     short j,k;
  594.     short dis = 555;
  595.     short disToDes;
  596.     for(j = 0;j < 3; j++){
  597.         dis = 555;
  598.         for(k = 0;k < 3; k++){
  599.             disToDes = abs((bx[j] - dx[k])) + abs((by[j] - dy[k]));
  600.             if(table[dx[k]][dy[k]].blo != -1){
  601.                 continue;
  602.             }
  603.             if(disToDes < dis && table[dx[k]][dy[k]].blo == -1){
  604.                 dis = disToDes;
  605.                 if(table[bx[j]][by[j]].des != -1 && table[dx[table[bx[j]][by[j]].des]][dy[table[bx[j]][by[j]].des]].blo != -1){
  606.                     table[dx[table[bx[j]][by[j]].des]][dy[table[bx[j]][by[j]].des]].blo = -1;
  607.                 }
  608.                 table[bx[j]][by[j]].des = k;
  609.                 table[dx[k]][dy[k]].blo = j;
  610.             }
  611.         }
  612.     }
  613. }
  614. short findNextBlock(short toX,short toY){
  615.     displayTextLine(9, "findNext");
  616.     short k,j;
  617.     short ret = -1;
  618.     short toNodeDist = 555;                                                         //most of possible
  619.     short Dist = 555;               //
  620.     short tempDist;
  621.     for(k = 0; k < MAX ; k++){
  622.         for(j = 0; j < MAX ; j++){
  623.             if(!table[k][j].bknow){
  624.                 tempDist = abs((toX-k))+abs((toY-j));
  625.                 if(table[k][j].bdist < Dist && table[k][j].bdist != -1){
  626.                     if(tempDist < toNodeDist){
  627.                             toNodeDist = tempDist;
  628.                             Dist = table[k][j].bdist;
  629.                             ret = j*MAX + k;
  630.                     }
  631.                 }
  632.             }
  633.         }
  634.     }
  635.     return ret;
  636.     // ret = MAX*y + x
  637.     /*
  638.         to use ret
  639.         x = ret%MAX;
  640.         y = ret/MAX;
  641.     */
  642. }
  643.  
  644. void bToB(short fx,short fy,short tx,short ty){
  645.     displayTextLine(9, "nTON");
  646.  
  647.     if(ty>fy){ //go to north
  648.         nodeToNode(x,y,fx,fy-1);
  649.     }
  650.     else if(ty<fy){ //go to south
  651.          nodeToNode(x,y,fx,fy+1);
  652.     }
  653.     else if(tx>fx){ //go to west
  654.         nodeToNode(x,y,fx-1,fy);
  655.     }
  656.     else{   //go to east
  657.         nodeToNode(x,y,fx+1,fy);
  658.     }
  659.     table[fx][fy].block = false;
  660.     table[tx][ty].block = true;
  661.     nodeToNode(x,y,fx,fy);
  662.     push = true;
  663.  
  664. }
  665.  
  666. void toNodeBlock(short fx,short fy,short tx,short ty){
  667.     //displayTextLine(9, "toNode");
  668.     if(!(tx == fx && ty == fy)){
  669.         toNodeBlock(fx,fy,table[tx][ty].bfromX,table[tx][ty].bfromY);
  670.         //if((abs(fx-tx)+abs(fy-ty)) == 1)return;
  671.         bToB(table[tx][ty].bfromX,table[tx][ty].bfromY,tx,ty);
  672.     }
  673. }
  674. void findpathBlock(short fx,short fy,short tx,short ty){
  675.     displayTextLine(9, "findpath");
  676.     short k,j;
  677.     short next;
  678.     table[fx][fy].bknow = false;
  679.     table[fx][fy].bdist = 0;
  680.  
  681.     while(true){
  682.         next = findNextBlock(tx,ty);
  683.         if(next == -1) break;
  684.  
  685.         k = next%MAX;
  686.         j = next/MAX;
  687.         table[k][j].bknow = true;
  688.         if(k == tx && j == ty) break;
  689.  
  690.         //check north
  691.         if(j < MAX-1 && j > 0){
  692.             if(!table[k][j+1].block && !table[k][j+1].bknow && !table[k][j-1].block ){
  693.                     if(table[k][j+1].bdist == -1 || table[k][j+1].bdist > table[k][j].bdist+1){
  694.                         table[k][j+1].bdist = table[k][j].bdist+1;
  695.                         table[k][j+1].bfromX = k;
  696.                         table[k][j+1].bfromY = j;
  697.                     }
  698.             }
  699.             if(!table[k][j-1].block && !table[k][j-1].bknow && !table[k][j+1].block){
  700.                     if(table[k][j-1].bdist == -1 || table[k][j-1].bdist > table[k][j].bdist+1){
  701.                         table[k][j-1].bdist = table[k][j].bdist+1;
  702.                         table[k][j-1].bfromX = k;
  703.                         table[k][j-1].bfromY = j;
  704.                     }
  705.             }
  706.         }
  707.         //check east
  708.         if(k > 0 && k < MAX-1){
  709.             if(!table[k-1][j].block && !table[k-1][j].bknow  && !table[k+1][j].block){
  710.                     if(table[k-1][j].bdist == -1 || table[k-1][j].bdist > table[k][j].bdist+1){
  711.                         table[k-1][j].bdist = table[k][j].bdist+1;
  712.                         table[k-1][j].bfromX = k;
  713.                         table[k-1][j].bfromY = j;
  714.                     }
  715.             }
  716.             if( !table[k+1][j].block && !table[k+1][j].bknow && !table[k-1][j].block){
  717.                     if(table[k+1][j].bdist == -1 || table[k+1][j].bdist > table[k][j].bdist+1){
  718.                         table[k+1][j].bdist = table[k][j].bdist+1;
  719.                         table[k+1][j].bfromX = k;
  720.                         table[k+1][j].bfromY = j;
  721.                     }
  722.             }
  723.         }
  724.     }//while
  725. }
  726. void clearFindPathBlock(){
  727.     short k,j;
  728.     for(k = 0; k < MAX ; k++){
  729.         for(j = 0; j < MAX ; j++){
  730.             table[k][j].bfromX = -1;
  731.             table[k][j].bfromY = -1;
  732.             table[k][j].bdist = -1;
  733.             table[k][j].bknow = false;
  734.         }
  735.     }
  736. }
  737. void blockToDes(short fx,short fy,short tx,short ty){
  738.     displayTextLine(9, "nodeToNode");
  739.     findpathBlock(fx,fy,tx,ty);
  740.  
  741.     //toNodeBlock(fx,fy,table[tx][ty].bfromX,table[tx][ty].bfromY);
  742.     toNodeBlock(fx,fy,tx,ty);
  743.  
  744.     pushblock();
  745.     push = false;
  746.  
  747.     clearFindPathBlock();
  748. }
  749. //////////////////////////////////////////////////////////////////
  750. //                                                                                                                          //
  751. //                                              main function                                                   //
  752. //                                                                                                                          //
  753. //////////////////////////////////////////////////////////////////
  754. task main()
  755. {
  756.     //main code
  757.     displayTextLine(9, "main");
  758.     init();
  759.  
  760.     search();
  761.     robotStop(100);
  762.    
  763.     matchblock();
  764.     blockToDes(bx[0],by[0],dx[table[bx[0]][by[0]].des],dy[table[bx[0]][by[0]].des]);
  765.     robotStop(300);
  766.     blockToDes(bx[1],by[1],dx[table[bx[1]][by[1]].des],dy[table[bx[1]][by[1]].des]);
  767.     robotStop(300);
  768.     blockToDes(bx[2],by[2],dx[table[bx[2]][by[2]].des],dy[table[bx[2]][by[2]].des]);
  769.     robotStop(300);
  770.    
  771.  
  772.     robotStop(300);
  773.     robotStop(0);
  774.  
  775. }
  776.  
  777. /********************************************
  778. table 10x10
  779.  
  780. Block
  781. -3 Destinations(FIX) {(3,3),(2,8),(7,2)}
  782. -3 Blocks (RANDOM)
  783. -4 Hedge (RANDOM / DON'T MOVE)
  784.  
  785. SCORE
  786. -Algorithm & Idea
  787. -Time & Accuracy
  788. -Report
  789.  
  790. sensor
  791. -light sensor 2
  792. -sonar sensor 1
  793. -color sensor 1 - read box color
  794.  
  795. motor
  796. -Large motor 2 - wheel
  797. -motor ? - crash box
  798. ********************************************/
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!