Last active
December 11, 2015 15:48
-
-
Save Lauszus/4623316 to your computer and use it in GitHub Desktop.
Robocup
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
| lineDist = 0.2 | |
| irDist = 0.25 | |
| gateDist = 0.35 | |
| gateLength = 0.55 | |
| gateFoundDist = 0.2 | |
| %%%%%%%%%%%%% Husk at ændre!!!! | |
| whiteLineValue = 0.70 | |
| %%%%%%%%%%%%% | |
| whiteTurnValue = -60 | |
| whiteLineFirstRun = 1 | |
| call "measureDistance" | |
| speak "test one complete" | |
| call "boxTunnel" | |
| speak "test two complete" | |
| call "findGate" | |
| speak "test three complete" | |
| %%%%%%%%%%call "wallGate" | |
| call "wallGate2" | |
| speak "test four complete" | |
| call "whiteLine" | |
| speak "test five complete" | |
| call "goal" | |
| speak "daddy's home" | |
| goto "end" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "goal" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| %%%%% Kør hen mod tunnel | |
| followline "bm" @v0.3 :($crossingblackline == 1) | |
| followline "bm" @v0.2 :($line2 > 0.75 & $line3 > 0.75 & $line4 > 0.75 & $line5 > 0.75) | |
| ignoreobstacles | |
| drive :($crossingblackline == 1) | |
| ignoreobstacles | |
| fwd lineDist @v0.3 | |
| ignoreobstacles | |
| followline "bm" @v0.3 :($crossingblackline == 1) | |
| ignoreobstacles | |
| fwd 0.1 | |
| turn -70 | |
| %%%%% Kør mod mål | |
| %followline "bm" :($crossingblackline == 1) | |
| %fwd lineDist | |
| %turn -90 | |
| %followline "bm" :($irdistfrontleft < 0.3 & $irdistfrontmiddle < 0.3 & $irdistfrontright < 0.3) | |
| %turn 90 | |
| %wallDist = $irdistright | |
| %eval wallDist | |
| %followwall "r" wallDist @v0.15 :($irdistright > 0.4) | |
| %eval $irdistright | |
| %drive :($crossingblackline == 1) | |
| %fwd lineDist @v0.3 | |
| %turn 90 | |
| %%%%% Ny kode | |
| followline "bm" :($crossingblackline == 1) | |
| fwd lineDist | |
| drive :($crossingblackline == 1) | |
| fwd lineDist | |
| turn 90 | |
| fwd -0.5 @v0.15 | |
| %%%%% Åben mål | |
| label "openFinish" | |
| followline "bm" @v0.3 :($crossingblackline == 1) | |
| fwd lineDist | |
| turn -90 | |
| ignoreobstacles | |
| followline "bm" @v0.2 :($irdistfrontleft < 0.3 & $irdistfrontmiddle < 0.3 & $irdistfrontright < 0.3) | |
| ignoreobstacles | |
| turn 90 | |
| fwd 0.7 | |
| turn -90 | |
| fwd 0.83 | |
| ignoreobstacles | |
| turn -90 | |
| stop | |
| wait 1 | |
| ignoreobstacles | |
| turnr 0.14 -85 | |
| ignoreobstacles | |
| drive @v0.3 :($drivendist > 0.3) | |
| stop | |
| wait 1 | |
| ignoreobstacles | |
| fwd -0.20 @v0.2 | |
| speak "This is Sparta" | |
| ignoreobstacles | |
| turnr 0.6 90 @v0.3 | |
| ignoreobstacles | |
| fwd -0.8 @v0.2 :($crossingblackline == 1) | |
| ignoreobstacles | |
| drive @v-0.2:($drivendist > 0.10) | |
| ignoreobstacles | |
| drive @v0.3 :($crossingblackline == 1) | |
| ignoreobstacles | |
| fwd lineDist | |
| ignoreobstacles | |
| turn 90 | |
| ignoreobstacles | |
| followline "bm" :($irdistfrontleft < 0.3 & $irdistfrontmiddle < 0.3 & $irdistfrontright < 0.3) | |
| fwd 0.10 | |
| stop | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "whiteLine" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| ignoreobstacles | |
| followline "bm" @v0.3 :($crossingblackline == 1) | |
| stop | |
| wait 1 | |
| ignoreobstacles | |
| fwd -0.15 @v0.1 | |
| turn -90 | |
| currentAngle = $odoth | |
| eval currentAngle | |
| label "checkWhiteLine" | |
| speak "A white line is not gonna stop me" | |
| eval $line0;$line1;$line2;$line3;$line4;$line5;$line6;$line7 | |
| if($line3 > whiteLineValue & $line4 > whiteLineValue) "followWhiteLine" | |
| if($line0 > whiteLineValue | $line1 > whiteLineValue | $line2 > whiteLineValue) "whiteLineTurnRight" | |
| if($line5 > whiteLineValue | $line6 > whiteLineValue | $line7 > whiteLineValue) "whiteLineTurnLeft" | |
| %else | |
| if(whiteLineFirstRun == 1) "firstWhiteLineRun" | |
| %else | |
| turn whiteTurnValue | |
| whiteTurnValue = -whiteTurnValue | |
| goto "checkWhiteLine" | |
| label "firstWhiteLineRun" | |
| turn 30 | |
| whiteLineFirstRun = 0 | |
| goto "checkWhiteLine" | |
| label "whiteLineTurnRight" | |
| turn -5 | |
| stop | |
| wait 0.5 | |
| goto "checkWhiteLine" | |
| label "whiteLineTurnLeft" | |
| turn 5 | |
| stop | |
| wait 0.5 | |
| goto "checkWhiteLine" | |
| %%%%%%%%%% Følg hvid linje | |
| label "followWhiteLine" | |
| startLength = $ododist | |
| followline "wm" @v0.3 :($drivendist > 2.55) | |
| %followline "wm" @v0.3 :($blacklinefound == 1) | |
| drivenLength = $ododist - startLength | |
| eval drivenLength | |
| turnAngleRad = -($odoth - currentAngle) | |
| eval turnAngleRad | |
| turn turnAngleRad "rad" | |
| drive :($blacklinefound == 1) | |
| fwd lineDist @v0.3 | |
| turn -90 | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "wallGate2" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| turn 90 | |
| speak "Mr. Gorbachev, tear down this wall!" | |
| drive @v0.3 :($irdistfrontleft < 0.3 & $irdistfrontmiddle < 0.3 & $irdistfrontright < 0.3) | |
| turn -90 | |
| wallDist = $irdistleft | |
| eval wallDist | |
| followwall "l" wallDist @v0.15 :($irdistleft > (wallDist+0.1)) | |
| eval $irdistleft | |
| drive :($crossingblackline == 1) | |
| fwd 0.1 @v0.3 | |
| turn 70 | |
| followline "bm" :($crossingblackline == 1) | |
| fwd lineDist | |
| turn 90 | |
| drive :($crossingblackline == 1) | |
| fwd lineDist | |
| turn 90 | |
| followline "bm" :($irdistfrontleft < 0.3 & $irdistfrontmiddle < 0.3 & $irdistfrontright < 0.3) | |
| turn -90 | |
| wallDist = $irdistleft+0.1 | |
| eval wallDist | |
| followwall "l" wallDist @v0.15 :($irdistleft > (wallDist+0.1)) | |
| fwd 0.1 | |
| drive @v-0.1 :($irdistleft < 0.35) | |
| fwd 0.45 @v0.2 | |
| turn 90 | |
| drive @v0.3 :($crossingblackline == 1) | |
| fwd lineDist | |
| turn 90 | |
| followline "bm" :($crossingblackline == 1) | |
| fwd lineDist | |
| drive :($crossingblackline == 1) | |
| fwd lineDist | |
| turn -90 | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "wallGate" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| turn 90 | |
| drive @v0.3 :($irdistfrontleft < 0.3 & $irdistfrontmiddle < 0.3 & $irdistfrontright < 0.3) | |
| turn 90 | |
| %%%%%%% Oprindelig kode | |
| followwall "r" 0.20 @v0.15 :($irdistright > 0.4) | |
| stop | |
| wait 1 | |
| fwd 0.2 | |
| drive @v-0.1 :($irdistright < 0.3) | |
| stop | |
| wait 1 | |
| %fwd 0.45 @v0.20 | |
| %turn -90 | |
| fwd 0.70 | |
| turn -90 | |
| drive :($irdistright < 0.4) | |
| fwd 0.1 | |
| wallDist = $irdistright | |
| eval wallDist | |
| followwall "r" wallDist @v0.15 :($crossingblackline == 1) | |
| fwd lineDist | |
| followwall "r" wallDist :($irdistright > 0.4) | |
| eval $irdistright | |
| drive :($crossingblackline == 1) | |
| fwd lineDist @v0.3 | |
| turn 90 | |
| followline "bm" :($crossingblackline == 1) | |
| turn 180 | |
| %radius = $irdistright + 0.15 | |
| %turnr radius 180 | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "findGate" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| fwd 0.10 | |
| followline "bm" @v0.3 :($crossingblackline == 1) | |
| stop | |
| wait 1 | |
| lineFoundDist = $ododist | |
| %%%%%%%%%% Find første ben loop | |
| label "findGateLeft" | |
| followline "bm" @v0.1 :($line2 > 0.75 & $line3 > 0.75 & $line4 > 0.75 & $line5 > 0.75) | ($irdistleft < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistleft | |
| if(stopValue == 2) "firstLegFoundLeft" | |
| %else | |
| fwd 0.05 | |
| followline "wm" :($line3 < 0.3 | $line4 < 0.3) | ($irdistleft < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistleft | |
| if(stopValue == 2) "firstLegFoundLeft" | |
| %else | |
| fwd 0.05 | |
| followline "bm" :($crossingblackline == 1) | ($irdistleft < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistleft | |
| if(stopValue == 2) "firstLegFoundLeft" | |
| %else | |
| followline "bm" @v0.3 :($drivendist > 0.7) | |
| turn 180 | |
| followline "bm" @v0.2 :($crossingblackline == 1) | |
| stop | |
| lineFoundDist = $ododist | |
| %%%%% | |
| label "findGateRight" | |
| followline "bm" @v0.1 :($line2 > 0.75 & $line3 > 0.75 & $line4 > 0.75 & $line5 > 0.75) | ($irdistright < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistright | |
| if(stopValue == 2) "firstLegFoundRight" | |
| %else | |
| fwd 0.05 | |
| followline "wm" :($line3 < 0.3 | $line4 < 0.3) | ($irdistright < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistright | |
| if(stopValue == 2) "firstLegFoundRight" | |
| %else | |
| fwd 0.05 | |
| followline "bm" :($crossingblackline == 1) | ($irdistright < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistright | |
| if(stopValue == 2) "firstLegFoundRight" | |
| %else | |
| followline "bm" @v0.3 :($drivendist > 0.7) | |
| turn 180 | |
| followline "bm" @v0.2 :($crossingblackline == 1) | |
| stop | |
| lineFoundDist = $ododist | |
| goto "findGateLeft" | |
| %%%%%%%%%% Find andet ben loop | |
| label "firstLegFoundLeft" | |
| speak "Got it" | |
| fwd gateFoundDist | |
| drive :($drivendist > gateLength) | ($irdistleft < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistleft | |
| if(stopValue == 2) "gateFoundLeft" | |
| %else | |
| fwdDist = 1.75-($ododist-lineFoundDist) | |
| eval fwdDist | |
| followline "bm" @v0.3 :($drivendist > fwdDist) | |
| turn 180 | |
| followline "bm" @v0.2 :($crossingblackline == 1) | |
| stop | |
| lineFoundDist = $ododist | |
| goto "findGateRight" | |
| %%%%% | |
| label "firstLegFoundRight" | |
| speak "Got it" | |
| fwd gateFoundDist | |
| fwd 0.15 | |
| drive :($drivendist > gateLength) | ($irdistright < gateDist) | |
| stopValue = $condition | |
| eval stopValue | |
| eval $irdistright | |
| if(stopValue == 2) "gateFoundRight" | |
| %else | |
| fwdDist = 1.75-($ododist-lineFoundDist) | |
| eval fwdDist | |
| followline "bm" @v0.3 :($drivendist > fwdDist) | |
| turn 180 | |
| followline "bm" @v0.2 :($crossingblackline == 1) | |
| stop | |
| lineFoundDist = $ododist | |
| goto "findGateLeft" | |
| %%%%%%%%%% Ben fundet | |
| label "gateFoundLeft" | |
| speak "Gate found" | |
| stop | |
| fwd -0.05 @v0.1 | |
| turn 90 | |
| fwd 1.90 @v0.3 | |
| return | |
| label "gateFoundRight" | |
| speak "Gate found" | |
| stop | |
| fwd -0.05 @v0.1 | |
| turn -90 | |
| fwd 1.90 @v0.3 | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "measureDistance" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| ignoreobstacles | |
| followline "bm" @v0.2 :($irdistfrontleft < 0.30 & $irdistfrontmiddle < 0.30 & $irdistfrontright < 0.30) | |
| stop | |
| ignoreobstacles | |
| followline "bm" @v0.1 :($irdistfrontleft < 0.18 & $irdistfrontmiddle < 0.18 & $irdistfrontright < 0.18) | |
| stop | |
| wait 1 | |
| dist1 = $irdistfrontleft | |
| dist2 = $irdistfrontmiddle | |
| dist3 = $irdistfrontright | |
| eval $odox;$odoy;dist1;dist2;dist3 | |
| if(dist1*2 > dist2+dist3+0.15) "dist1Wrong" | |
| if(dist2*2 > dist1+dist3+0.15) "dist2Wrong" | |
| if(dist3*2 > dist1+dist2+0.15) "dist3Wrong" | |
| %else | |
| average = (dist1+dist2+dist3)/3 | |
| goto "calcDist" | |
| label "dist1Wrong" | |
| average = (dist2+dist3)/2 | |
| goto "calcDist" | |
| label "dist2Wrong" | |
| average = (dist1+dist3)/2 | |
| goto "calcDist" | |
| label "dist3Wrong" | |
| average = (dist1+dist2)/2 | |
| goto "calcDist" | |
| label "calcDist" | |
| distanceTemp = (-$odoy)+average | |
| if (distanceTemp > 2.65) "distanceSuperLong" | |
| if (distanceTemp > 2.40) "distanceLong" | |
| %else | |
| distance = distanceTemp+0.25 | |
| goto "speakDistance" | |
| label "distanceLong" | |
| distance = distanceTemp+0.23 | |
| goto "speakDistance" | |
| label "distanceSuperLong" | |
| distance = distanceTemp+0.21 | |
| label "speakDistance" | |
| eval distanceTemp | |
| eval distance | |
| stringcat "Distance is " distance " meters" | |
| speak "$string" | |
| %distance = distanceTemp+((-0.036866333)*distanceTemp+0.337032867) | |
| %distance = distanceTemp+((-0.026702)*distanceTemp+0.081327) | |
| %distance = distanceTemp+0.2175 | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "boxTunnel" | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| ignoreobstacles | |
| turn 180 @v0.3 | |
| followline "bm" :($crossingblackline == 1) | |
| fwd lineDist | |
| turn -90 | |
| followline "bm" :($crossingblackline == 1) | |
| fwd lineDist | |
| turn -90 | |
| followline "bm" :($irdistleft < irDist) | |
| stop | |
| wait 1 | |
| fwd -0.6 @v0.2 | |
| turn 90 @v0.3 | |
| drive :($crossingblackline == 1) | |
| fwd lineDist | |
| turn -90 | |
| %%%%%%%%%% Kør i tunnel mod kassen | |
| speak "Driving into tunnel" | |
| ignoreobstacles | |
| followline "bm" @v0.2 :($crossingblackline == 1) | |
| %ignoreobstacles | |
| %followline "bm" @v0.2 :($irdistleft < irDist | $irdistright < irDist) | |
| %ignoreobstacles | |
| %followline "bm" :($blacklinefound == 0) | |
| %ignoreobstacles | |
| %drive :($irdistleft > irDist | $irdistright > irDist) | |
| %ignoreobstacles | |
| %followline "wm" :($crossingblackline == 1) | |
| ignoreobstacles | |
| fwd 0.15 | |
| %%%%%%%%%% Bak ud af tunnel | |
| speak "Box moved" | |
| ignoreobstacles | |
| fwd -1 @v0.2 | |
| %drive @v-0.2 :($irdistleft < irDist | $irdistright < irDist) | |
| %ignoreobstacles | |
| %drive :($irdistleft > irDist | $irdistright > irDist) | |
| %fwd -0.5 @v0.2 | |
| turn -90 | |
| ignoreobstacles | |
| drive @v0.3 :($crossingblackline == 1) | |
| ignoreobstacles | |
| fwd lineDist | |
| ignoreobstacles | |
| turn 90 | |
| followline "bm" :($crossingblackline == 1) | |
| fwd lineDist | |
| turn 90 | |
| followline "bm" :($crossingblackline == 1) | |
| return | |
| %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
| label "end" | |
| stop |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment