API tools faq
paste
Advertisement
Guest User

Robot

a guest
Oct 10th, 2016
98
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Robots 17.37 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,     GyroA,          sensorEV3_Gyro)
  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 9
  11. #define goal 5
  12. #define rDegree 88
  13. #define lDegree -89
  14.  
  15. //north-0 east-90
  16. int thres = 27;
  17.  
  18. typedef struct node{
  19.     short x,y;
  20.     short title;
  21.     bool pass;              //check aleady pass?
  22.     bool n,e,w,s;           //check goable direction
  23.     bool left,center,right;
  24.     short fromX,fromY;          //
  25.  
  26.     bool known;             //
  27.     short distance;         //
  28. }node;
  29.  
  30. node table[max][max];
  31. short x,y;
  32. short run;  //n-0 e-1 s-2 w-3
  33. short count;
  34. //short goal = 0;
  35. short countNode;
  36. int degree;
  37. /*init node for first run loop*/
  38. void init(){
  39.     short k,j;
  40.     for(k = 0;k<max;k++){
  41.         for(j = 0;j<max;j++){
  42.             table[k][j].title = k*max + j;
  43.             //displayTextLine(5,"title : %d",table[k][j].title);
  44.             //wait1Msec(1500);
  45.             table[k][j].x = k;
  46.             table[k][j].y = j;
  47.  
  48.             table[k][j].pass = false;
  49.             table[k][j].left = false;
  50.             table[k][j].center = false;
  51.             table[k][j].right = false;
  52.             table[k][j].n = false;
  53.             table[k][j].e = false;
  54.             table[k][j].w = false;
  55.             table[k][j].s = false;
  56.             table[k][j].fromX = -1;
  57.             table[k][j].fromY = -1;
  58.             table[k][j].known = false;
  59.             table[k][j].distance = -1;
  60.         }
  61.     }
  62.  
  63.     x = 0;
  64.     y = 0;
  65.     count = 0;
  66.     countNode = 0;
  67.     resetGyro(GyroA);
  68.     run = 0; //north
  69.     degree = 0;
  70.     /*while(true){
  71.         goal = 2;
  72.         displayTextLine(10,"goal = %d",goal);
  73.         wait1Msec(2000);
  74.         if(SensorValue(t) == 1) break;
  75.         goal = 3;
  76.         displayTextLine(10,"goal = %d",goal);
  77.         wait1Msec(2000);
  78.         if(SensorValue(t) == 1) break;
  79.         goal = 4;
  80.         displayTextLine(10,"goal = %d",goal);
  81.         wait1Msec(2000);
  82.         if(SensorValue(t) == 1) break;
  83.         goal = 5;
  84.         displayTextLine(10,"goal = %d",goal);
  85.         wait1Msec(2000);
  86.         if(SensorValue(t) == 1) break;
  87.     }*/
  88. }
  89.  
  90. void forward(){
  91.     switch (run){
  92.         case 0: y++; break;
  93.         case 1: x++; break;
  94.         case 2: y--; break;
  95.         case 3: x--; break;
  96.     }
  97.  
  98.     //walk until 1 of sensor see line
  99.     while(1){
  100.         //displayBigTextLine(1, "Reflected: %d", SensorValue[S1]);
  101.         //displayBigTextLine(2, "Reflected: %d", SensorValue[S3]);
  102.         if((SensorValue[S1] > thres )&&( SensorValue[S3] < thres)){
  103.             //displayBigTextLine(5, "LEFT");
  104.             motor[motorB]=-1;
  105.             motor[motorC]=1;
  106.         }
  107.         else if((SensorValue[S1] < thres )&&( SensorValue[S3] > thres)){
  108. //          displayBigTextLine(5, "RIGHT");
  109.             motor[motorB]=1;
  110.             motor[motorC]=-1;
  111.         }
  112.         else if((SensorValue[S1] < thres )&& (SensorValue[S3] < thres)){
  113. //          displayBigTextLine(5, "forward");
  114.             motor[motorB]=10;
  115.             motor[motorC]=10;
  116.         }
  117.         else if((SensorValue[S1] > thres )&& (SensorValue[S3] > thres)){
  118.             //displayBigTextLine(5, "break");
  119.             break;
  120.         }
  121.         //if((SensorValue[S1] > thres) || (SensorValue[S3] > thres)) break;
  122.     }
  123.     motor[motorB] = 0;
  124.     motor[motorC] = 0;
  125.  
  126.     moveMotorTarget(L,455,20);
  127.     moveMotorTarget(R,455,20);
  128.     waitUntilMotorStop(R);
  129.     waitUntilMotorStop(L);
  130.     //make two of sensor in line
  131.     /*while(1){
  132.         if((SensorValue[S1] > thres) && (SensorValue[S3] > thres)){
  133.             displayTextLine(9,"sensor %d",SensorValue[S1]);
  134.             displayTextLine(10,"sensor %d",SensorValue[S3]);
  135.             //break;
  136.         }
  137.         else if((SensorValue[S1] > thres) && (SensorValue[S3] < thres)){
  138.             motor(L) = 0;
  139.             motor(R) = 1;
  140.         }
  141.         else if((SensorValue[S1] < thres )&&( SensorValue[S3] > thres)){
  142.             motor(L) = 1;
  143.             motor(R) = 0;
  144.         }
  145.         motor(R) = 0;
  146.         motor(L) = 0;
  147.     }
  148.     setMotorSpeed(L,0);
  149.     setMotorSpeed(R,0);
  150.  
  151.     return ;
  152.     //walk out of yellow line with 90degree line
  153.     while(1){
  154.         if(SensorValue[S3] < thres && SensorValue[S1] < thres && -2 < SensorValue[S3] - SensorValue[S1] && SensorValue[S3] - SensorValue[S1] < 2){
  155.             break;
  156.         }
  157.         else if(SensorValue[S3] < thres && SensorValue[S1] < thres){
  158.             motor(L) = -1;
  159.             motor(R) = -1;
  160.         }
  161.         else if(SensorValue[S1] > thres && SensorValue[S3] > thres){
  162.             motor(L) = 1;
  163.             motor(R) = 1;
  164.         }
  165.         else if(SensorValue[S1] > thres && SensorValue[S3] < thres){
  166.             motor(L) = 1;
  167.             motor(R) = -1;
  168.         }
  169.         else if(SensorValue[S1] < thres && SensorValue[S3] > thres){
  170.             motor(L) = -1;
  171.             motor(R) = 1;
  172.         }
  173.     }
  174.     motor(L) = 0;
  175.     motor(R) = 0;
  176.     //walk half line
  177. */
  178.     return ;
  179. /*
  180.     moveMotorTarget(L,619,20.98);
  181.     moveMotorTarget(R,619,20);
  182.     waitUntilMotorStop(R);
  183.     waitUntilMotorStop(L);
  184.  
  185.  
  186. /*  int n = degree;
  187.     repeatUntil(getGyroDegrees(GyroA) == n){
  188.         if(getGyroDegrees(GyroA)<n){
  189.             setMotorSpeed(L,5);
  190.             setMotorSpeed(R,-5);
  191.         }
  192.         else{
  193.             setMotorSpeed(L,-5);
  194.             setMotorSpeed(R,5);
  195.         }
  196.         displayTextLine(1,"GyroValue = %d",getGyroDegrees(GyroA));
  197.     }
  198. */
  199.     setMotorSpeed(L,0);
  200.     setMotorSpeed(R,0);
  201.  
  202. }
  203. void right(){
  204.     //displayTextLine(10, "Gyro = %d",getGyroDegrees(GyroA));
  205.     //wait1Msec(3000);
  206.     resetGyro(GyroA);
  207.     int n = rDegree;
  208.     /*repeatUntil(getGyroDegrees(GyroA) > n-1 /*&& getGyroDegrees(GyroA) < n+1){
  209.         if(getGyroDegrees(GyroA)<n ){
  210.             setMotorSpeed(L,5);
  211.             setMotorSpeed(R,-5);
  212.         }
  213.     }*/
  214.     repeatUntil(getGyroDegrees(GyroA) == n){
  215.         if(getGyroDegrees(GyroA)<n){
  216.             setMotorSpeed(L,5);
  217.             setMotorSpeed(R,-5);
  218.         }
  219.         else{
  220.             setMotorSpeed(L,-5);
  221.             setMotorSpeed(R,5);
  222.         }
  223.         displayTextLine(1,"GyroValue = %d",getGyroDegrees(GyroA));
  224.     }
  225.  
  226.     setMotorSpeed(L,0);
  227.     setMotorSpeed(R,0);
  228.  
  229.     if(run == 3) run = 0;   //change run to right direction
  230.     else run++;
  231.  
  232.     degree = getGyroDegrees(GyroA);
  233.     //displayTextLine(10, "Gyro = %d",getGyroDegrees(GyroA));
  234.     //wait1Msec(3000);
  235. }
  236.  
  237. void left(){
  238.     resetGyro(GyroA);
  239.     int n = lDegree;
  240.     /*repeatUntil(getGyroDegrees(GyroA) <= n+1){
  241.         if(getGyroDegrees(GyroA)>n ){
  242.             setMotorSpeed(L,-5);
  243.             setMotorSpeed(R,5);
  244.         }
  245.     }*/
  246.     repeatUntil(getGyroDegrees(GyroA) == n){
  247.         if(getGyroDegrees(GyroA)>n ){
  248.             setMotorSpeed(L,-5);
  249.             setMotorSpeed(R,5);
  250.         }
  251.         else{
  252.             setMotorSpeed(L,5);
  253.             setMotorSpeed(R,-5);
  254.         }
  255.         displayTextLine(1,"GyroValue = %d",getGyroDegrees(GyroA));
  256.     }
  257.  
  258.  
  259.     setMotorSpeed(L,0);
  260.     setMotorSpeed(R,0);
  261.  
  262.     if(run == 0) run = 3;   //change run to left direction
  263.     else run--;
  264.  
  265.     degree = getGyroDegrees(GyroA);
  266. }
  267.  
  268. void turnAround(){
  269.     int n = getGyroDegrees(GyroA) + rDegree*2;
  270.     /*repeatUntil(getGyroDegrees(GyroA) >= n-1){
  271.         if(getGyroDegrees(GyroA)<n ){
  272.             setMotorSpeed(L,5);
  273.             setMotorSpeed(R,-5);
  274.         }
  275.     }*/
  276.  
  277.     repeatUntil(getGyroDegrees(GyroA) == n){
  278.         if(getGyroDegrees(GyroA)<n){
  279.             setMotorSpeed(L,5);
  280.             setMotorSpeed(R,-5);
  281.         }
  282.         else{
  283.             setMotorSpeed(L,-5);
  284.             setMotorSpeed(R,5);
  285.         }
  286.         displayTextLine(1,"GyroValue = %d",getGyroDegrees(GyroA));
  287.     }
  288.     setMotorSpeed(L,0);
  289.     setMotorSpeed(R,0);
  290.  
  291.     if(run == 3) run = 0;   //change run to right direction
  292.     else run++;
  293.     if(run == 3) run = 0;   //change run to right direction
  294.     else run++;
  295.  
  296.     degree = getGyroDegrees(GyroA);
  297. }
  298.  
  299. void turnDirection(short from,short to){
  300.     displayTextLine(8, "from : %d , to : %d",from,to);
  301.     if((from==0&&to==1)||(from==1&&to==2)||(from==2&&to==3)||(from==3&&to==0)){
  302.         displayTextLine(9, "turn right");
  303.         right();
  304.     }
  305.     else if((from==0&&to==3)||(from==1&&to==0)||(from==2&&to==1)||(from==3&&to==2)){
  306.         displayTextLine(9, "turn left");
  307.         left();
  308.     }
  309.     else if((from==0&&to==2)||(from==1&&to==3)||(from==2&&to==0)||(from==3&&to==1)){
  310.         displayTextLine(9, "turn turnAround");
  311.         turnAround();
  312.     }
  313. }
  314.  
  315. void checkWall(short thisX,short thisY){
  316.  
  317.     //turn right
  318.     motor(sonarM) = 20;
  319.     wait1Msec(350);
  320.     motor(sonarM) = 0;
  321.     /*
  322.     switch (run){
  323.         case 0: table[thisX][thisY].s = true; break;
  324.         case 1: table[thisX][thisY].w = true; break;
  325.         case 2: table[thisX][thisY].n = true; break;
  326.         case 3: table[thisX][thisY].e = true;
  327.     }*/
  328.  
  329.     if(SensorValue(sonar) > 20){
  330.         displayTextLine(10, "sonar = %d",SensorValue(sonar));
  331.         //wait1Msec(3000);
  332.         table[thisX][thisY].right = true;
  333.  
  334.         switch (run){
  335.             case 0: table[thisX][thisY].e = true; break;
  336.             case 1: table[thisX][thisY].s = true; break;
  337.             case 2: table[thisX][thisY].w = true; break;
  338.             case 3: table[thisX][thisY].n = true;
  339.         }
  340.     }
  341.  
  342.     //turn center
  343.     motor(sonarM) = -20;
  344.     wait1Msec(350);
  345.     motor(sonarM) = 0;
  346.     if(SensorValue(sonar) > 20){
  347.         displayTextLine(10, "sonar = %d",SensorValue(sonar));
  348.         //wait1Msec(3000);
  349.         table[thisX][thisY].center = true;
  350.  
  351.         switch (run){
  352.             case 0: table[thisX][thisY].n = true; break;
  353.             case 1: table[thisX][thisY].e = true; break;
  354.             case 2: table[thisX][thisY].s = true; break;
  355.             case 3: table[thisX][thisY].w = true;
  356.         }
  357.     }
  358.  
  359.     //turn left
  360.     motor(sonarM) = -20;
  361.     wait1Msec(350);
  362.     motor(sonarM) = 0;
  363.     if(SensorValue(sonar) > 20){
  364.         displayTextLine(10, "sonar = %d",SensorValue(sonar));
  365.         //wait1Msec(3000);
  366.         table[thisX][thisY].left = true;
  367.  
  368.         switch (run){
  369.             case 0: table[thisX][thisY].w = true; break;
  370.             case 1: table[thisX][thisY].n = true; break;
  371.             case 2: table[thisX][thisY].e = true; break;
  372.             case 3: table[thisX][thisY].s = true;
  373.         }
  374.     }
  375.  
  376.     //turn back to center
  377.     motor(sonarM) = 20;
  378.     wait1Msec(340);
  379.     motor(sonarM) = 0;
  380.  
  381.  
  382. }
  383.  
  384. void travelAll(short thisX,short thisY){
  385.     // ( x , y )
  386.     switch (run){
  387.         case 0 : table[thisX][thisY].s = true; break;
  388.         case 1 : table[thisX][thisY].w = true; break;
  389.         case 2 : table[thisX][thisY].n = true; break;
  390.         case 3 : table[thisX][thisY].e = true;
  391.     }
  392.  
  393.     checkWall(thisX,thisY);
  394.     table[thisX][thisY].pass = true; //not use now
  395.     countNode++;                                            //count Node ++
  396.     displayTextLine(1, "node %d,%d pass",thisX,thisY);
  397.     short runtemp = run;
  398.     short tempX,tempY;
  399.     if((table[thisX][thisY].right || table[thisX][thisY].center || table[thisX][thisY].left)/* && !table[thisX][thisY].pass*/){
  400.         /////////////////////////////right//////////////////////////////////
  401.         if(table[thisX][thisY].right){
  402.             //turn right
  403.             right();                //turn right
  404.  
  405.             tempX = x;
  406.             tempY = y;
  407.  
  408.             switch (run){
  409.                 case 0: tempY = y+1; break;
  410.                 case 1: tempX = x+1; break;
  411.                 case 2: tempY = y-1; break;
  412.                 case 3: tempX = x-1;
  413.             }
  414.             if(table[tempX][tempY].pass){
  415.                 displayTextLine(2, "node %d,%d pass!!",x,y);
  416.                 wait1Msec(1000);
  417.                 left();
  418.             }
  419.             else{
  420.                 forward();
  421.                 travelAll(x,y);
  422.  
  423.                 //next node
  424.                 displayTextLine(5, "run : %d , runtemp : %d",run,(runtemp+3)%4);
  425.                 displayTextLine(6, "run : %d , runtemp : %d",run, runtemp);
  426.                 //wait1Msec(3000);
  427.                 turnDirection(run,(runtemp+3)%4);
  428.                 forward();
  429.  
  430.                 //this node
  431.                 //right();
  432.             }
  433.  
  434.         }
  435.         /////////////////////////////center//////////////////////////////////
  436.         if(table[thisX][thisY].center){
  437.             displayTextLine(3, "run : %d , runtemp : %d",run,runtemp);
  438.             //wait1Msec(3000);
  439.  
  440.  
  441.             turnDirection(run,runtemp);
  442.             tempX = x;
  443.             tempY = y;
  444.  
  445.             switch (run){
  446.                 case 0: tempY = y+1; break;
  447.                 case 1: tempX = x+1; break;
  448.                 case 2: tempY = y-1; break;
  449.                 case 3: tempX = x-1;
  450.             }
  451.             if(table[tempX][tempY].pass){
  452.                 displayTextLine(2, "node %d,%d pass!!",x,y);
  453.             }
  454.             else{
  455.                 forward();
  456.                 travelAll(x,y);
  457.  
  458.                 //next node
  459.                 displayTextLine(5, "run : %d , runtemp : %d",run,(runtemp+2)%4);
  460.                 displayTextLine(6, "run : %d , runtemp : %d",run, runtemp);
  461.                 //wait1Msec(3000);
  462.                 turnDirection(run,(runtemp+2)%4);
  463.                 //wait1Msec(3000);
  464.                 forward();
  465.  
  466.                 //this node
  467.                 //turnAround();
  468.             }
  469.         }
  470.         /////////////////////////////left//////////////////////////////////
  471.         if(table[thisX][thisY].left){
  472.             //turn left
  473.             displayTextLine(3, "run : %d , runtemp : %d",run,(runtemp+3)%4);
  474.             //wait1Msec(3000);
  475.             turnDirection(run,(runtemp+3)%4);                   //left
  476.  
  477.             tempX = x;
  478.             tempY = y;
  479.  
  480.             switch (run){
  481.                 case 0: tempY = y+1; break;
  482.                 case 1: tempX = x+1; break;
  483.                 case 2: tempY = y-1; break;
  484.                 case 3: tempX = x-1;
  485.             }
  486.             if(table[tempX][tempY].pass){
  487.                 displayTextLine(2, "node %d,%d pass!!",x,y);
  488.                 turnDirection(run,runtemp);
  489.             }
  490.             else{
  491.                 forward();
  492.                 travelAll(x,y);
  493.  
  494.                 //next node
  495.                 displayTextLine(5, "run : %d , runtemp : %d",run,(runtemp+1)%4);
  496.                 //wait1Msec(3000);
  497.                 turnDirection(run,(runtemp+1)%4);
  498.                 forward();
  499.  
  500.                 //this node
  501.                 //left();
  502.             }
  503.         }
  504.     }
  505. }
  506.  
  507.  
  508. ////////////////////////////////////////////////////////////////////////////////////////////////
  509. //                                                              find shortest path                                                                                  //
  510. ////////////////////////////////////////////////////////////////////////////////////////////////
  511.  
  512. short findNext(){
  513.     short dist = 999;
  514.     short indexX,indexY;
  515.     short retNode = -1;
  516.  
  517.     //displayTextLine(2,"in function findNext");
  518.     //wait1Msec(1500);
  519.  
  520.     for(indexX = 0;indexX < max;indexX++){
  521.         for(indexY = 0;indexY < max;indexY++){
  522.             //displayTextLine(5,"name:%d %d %d",table[indexX][indexY].title,indexX,indexY);
  523.             //wait1Msec(2000);
  524.             if(!table[indexX][indexY].known && table[indexX][indexY].distance != -1){
  525.                 //displayTextLine(7,"Unknown!!");
  526.                 //wait1Msec(1500);
  527.                 //displayTextLine(8,"dist: %d",table[indexX][indexY].distance);
  528.                 //wait1Msec(1500);
  529.                 if(table[indexX][indexY].distance < dist){
  530.                     retNode = table[indexX][indexY].title;
  531.                     dist = table[indexX][indexY].distance;
  532.                     //displayTextLine(5,"name:%d %d %d",table[indexX][indexY].title,indexX,indexY);
  533.                     //displayTextLine(1,"retNode: %d",retNode);
  534.                     //wait1Msec(1500);
  535.                 }
  536.                 //else{
  537.                 //  displayTextLine(8,"dist == NULL");
  538.                 //  wait1Msec(1500);
  539.                 //}
  540.             }
  541.         }
  542.     }
  543.     displayTextLine(2,"retnode : %d",retNode);
  544.     //wait1Msec(2000);
  545.  
  546.     return retNode;
  547. }
  548.  
  549. void findPath(){
  550.         displayTextLine(2,"in function findPath");
  551.         table[0][0].distance = 0;
  552.         table[0][0].s = false;
  553.  
  554.         table[0][0].fromX = -1;
  555.         table[0][0].fromY = -1;
  556.  
  557.         while(true){
  558.             short nodep = findNext();
  559.             if(nodep == -1){
  560.                 break;
  561.             }
  562.             x = nodep/max;
  563.             y = nodep%max;
  564.  
  565.             displayTextLine(8,"this node %d",table[x][y].title);
  566.             //wait1Msec(1500);
  567.             displayTextLine(10,"");
  568.             displayTextLine(11,"");
  569.             displayTextLine(12,"");
  570.             displayTextLine(13,"");
  571.  
  572.             table[x][y].known = true;
  573.  
  574.             if(x == goal-1 && y == goal-1){
  575.                 displayTextLine(10,"found!!!!!!!");
  576.                 break;
  577.             }
  578.             //short count;
  579.             //for(count = 0;count<4;count++){
  580.             //}
  581.  
  582.             if(table[x][y].n){
  583.                 displayTextLine(10,"can go n");
  584.                 if(!table[x][y+1].known && ((table[x][y+1].distance == -1) || (table[x][y+1].distance > table[x][y].distance + 1))){
  585.                         table[x][y+1].distance = table[x][y].distance + 1;
  586.                         table[x][y+1].fromX = x;
  587.                         table[x][y+1].fromY = y;
  588.                         displayTextLine(10,"next node %d %d",table[x][y+1].title,table[x][y+1].distance);
  589.                         //wait1Msec(1000);
  590.                 }
  591.             }
  592.             else displayTextLine(10,"cannot go n");
  593.             if(table[x][y].e){
  594.                 displayTextLine(11,"can go e");
  595.                 if(!table[x+1][y].known && ((table[x+1][y].distance == -1) || table[x+1][y].distance > table[x][y].distance + 1)){
  596.                         table[x+1][y].distance = table[x][y].distance + 1;
  597.                         table[x+1][y].fromX = x;
  598.                         table[x+1][y].fromY = y;
  599.                         displayTextLine(11,"next node %d %d",table[x+1][y].title,table[x+1][y].distance);
  600.                         //wait1Msec(1000);
  601.                 }
  602.             }
  603.             else displayTextLine(10,"cannot go e");
  604.             if(table[x][y].s){
  605.                 displayTextLine(12,"can go s");
  606.                 if(!table[x][y-1].known && ((table[x][y-1].distance == -1) || table[x][y-1].distance > table[x][y].distance + 1)){
  607.                         table[x][y-1].distance = table[x][y].distance + 1;
  608.                         table[x][y-1].fromX = x;
  609.                         table[x][y-1].fromY = y;
  610.                         displayTextLine(12,"next node %d %d",table[x][y-1].title,table[x][y-1].distance);
  611.                         //wait1Msec(1000);
  612.                 }
  613.             }
  614.             else displayTextLine(10,"cannot go s");
  615.             if(table[x][y].w){
  616.                 displayTextLine(13,"can go w");
  617.                 if(!table[x-1][y].known && ((table[x-1][y].distance == -1) || table[x-1][y].distance > table[x][y].distance + 1)){
  618.                         table[x-1][y].distance = table[x][y].distance + 1;
  619.                         table[x-1][y].fromX = x;
  620.                         table[x-1][y].fromY = y;
  621.                         displayTextLine(13,"next node %d %d",table[x-1][y].title,table[x-1][y].distance);
  622.                         //wait1Msec(1500);
  623.                 }
  624.             }
  625.             else displayTextLine(10,"cannot go w");
  626.         }
  627.         // end loop while
  628. }
  629.  
  630. void nodeTonode(short x2,short y2,short x1,short y1){ //from node(x1,y1) to node(x2,y2)
  631.     if(x2 == 0 && y2 == 0)
  632.         return ;
  633.     if(y2>y1){ //go to north
  634.         turnDirection(run,0);
  635.         forward();
  636.     }
  637.     else if(y2<y1){ //go to south
  638.         turnDirection(run,2);
  639.         forward();
  640.     }
  641.     else if(x2>x1){ //go to east
  642.         turnDirection(run,1);
  643.         forward();
  644.     }
  645.     else{   //go to west
  646.         turnDirection(run,3);
  647.         forward();
  648.     }
  649. }
  650.  
  651. void shortPath(short thisX,short thisY){ //walk from start to end node
  652.     displayTextLine(2,"in func short path");
  653.     if(thisX >= 0 && thisY >= 0){
  654.         shortPath(table[thisX][thisY].fromX,table[thisX][thisY].fromY);
  655.         nodeTonode(thisX,thisY,table[thisX][thisY].fromX,table[thisX][thisY].fromY);
  656.         displayTextLine(4, "node title = %d",table[thisX][thisY].title);
  657.         displayTextLine(5, "node x,y = %d %d",thisX,thisY);
  658.         //wait1Msec(1500);
  659.     }
  660. }
  661.  
  662. task main()
  663. {
  664.     init();
  665.     //forward();
  666.  
  667.  
  668.     travelAll(x,y);
  669.  
  670.  
  671.     findPath();
  672.  
  673.  
  674.     shortPath(goal-1,goal-1);
  675.     //displayTextLine(10, "Gyross = %d",getGyroDegrees(Gyross));
  676.     for(count = 0;count<=20;count++){
  677.         displayTextLine(count,"");
  678.     }
  679.  
  680.     displayTextLine(8, " Shortest Path Block = %d",table[goal-1][goal-1].distance);
  681.     displayTextLine(10, " Available Block = %d",countNode);
  682.  
  683.     while(1){}
  684.     /*while(SensorValue(t)!= 1){*/
  685.  
  686.  
  687. }
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!