From bcf10d5176d75c47e96ae57a200a67ea61e5b637 Mon Sep 17 00:00:00 2001 From: Miguel Angel Astor Romero Date: Thu, 23 Jun 2016 17:24:35 -0400 Subject: [PATCH] DONE! --- README.org | 85 +++++++++++++++++- environment.png | Bin 0 -> 8062 bytes .../ucv/ciens/cicore/icaro/ryabi/RyABI.java | 2 +- .../ryabi/behaviors/CatchBallBehavior.java | 5 +- .../ryabi/behaviors/SearchBallBehavior.java | 34 ++++--- .../sensors/FeatureDetectorsHandler.java | 2 +- 6 files changed, 101 insertions(+), 27 deletions(-) create mode 100644 environment.png diff --git a/README.org b/README.org index 2d4b907..cad64fa 100644 --- a/README.org +++ b/README.org @@ -1,7 +1,84 @@ -* RyABI +* RyABI (/Robótica y Aprendizaje Bio-Inspirado/ - Robotics and Bio-Inspired Learning) ** Abstract -RyABI (/Robótica y Aprendizaje Bio-Inspirado/ - Robotics and Bio-Inspired Learning) is a control program for LEGO Mindstorms NXT using the LeJOS NXJ -firmware. It is developed as part of the homonimous course being taught at the Computer Sciences School of the UCV. -Future verions of this README file will include a technical report on the development of the project. +RyABI is a control program for LEGO Mindstorms NXT using the LeJOS NXJ firmware. The controller is based on the +subsumption reactive control architecture. + +** Objective + +The controller assumes an environment like the following: + +#+BEGIN_SRC ditaa :file environment.png :cmdline -o -E + . +-----------+ + . | | + . |cBLK | + +----+ +------+----+ + | | | | + | | | | + | | +---------+ | | + | | | | | | + | | | | | | + | | | | | | + | | | | | | + | | +-----+ | | | | + | | |Robot| | | | | + | | |cGRE | | | | | + | | +-----+ | | | | + | | | | | | + | | | | +---------+ | | + | | | cBLK | | | | | +-----+ + | | +---------+ | | | | |Ball | + | | | | | | |cYEL | + | | | | | | +-----+ + | | | | | | + | | | | | | + | | | | | | + | | | | | | + | | | cBLK | | | + | | +---------+ | | + | | | | + | | | | + | | | | + | | | | + |cBLU| |cPNK| + +----+ +------+----+ + . | | + . |cBLK | + . +-----------+ + +#+END_SRC + +#+RESULTS: +[[file:environment.png]] + +The goal of the controller is to find a ball (in yellow) that is located on one side of a goal line (in pink), +while avoiding any obstacles (in black) it can find. Once the ball is found, or the robot has collided with any +of the two obstacles at the sides of the goal line, the controller will return to the starting line (in blue). + +The floor and the start/goal lines must be of constrasting colors. Contrary to what the diagram implies, both +the start and goal lines *must* be of a similar color. + +** Robot + +The robot is based on a differential drive and must use the following motors: + + * Motors A and C :: Attached to the wheels. Used to move and steer the robot. + * Motor B :: Attached to a gripper claw. Used to catch the ball. + +It must also use the following sensors: + + * Ultrasonic ranger :: Used to find the ball. Attached to port 1. + * Touch bumper :: Used to detect obstacles. Attached to port 2. + * HiTechnic compass :: Used to steer and move with some precission. Attached to port 3. + * Greyscale light detector :: Used to find the start and goal lines. Attached to port 4. + +** Subsumption + +The controller uses the following five behaviors: + + * Wander :: Simply moves the robot forward. + * Search :: When the goal line is found the robot will start searching for the ball using the range sensor. + * Catch :: When the ball has been found the robot will attempt to grab it with it's gripper. + * Victory :: When the start line is crossed the robot will stop moving a play a little tune. + * Avoid :: If the robot bumped against an obstacle then the robot will move a bit to the right to avoid it. It may take many attempts to clear a wide obstacle. diff --git a/environment.png b/environment.png new file mode 100644 index 0000000000000000000000000000000000000000..109b8a1d6f1e40a9c77534e3b74a9f14432a9f41 GIT binary patch literal 8062 zcmcgxc|6qX{vT3ZMfyeMq)VixqEgCoY!xMSjT7BAS)xs(V>rlKX`_omR5R#kaUDr? z3R!b(5$a0B*v1yJ@50>A^L%H_OgZQDyRYB<`Nwy@-{<-4pU?aAd>-E)DEgvvmdqiM zNTLQi^^8d*p(P}epweFi;Z5|xNGlRaX1jsj*1fJ(oi#=$?+>5Na)^I4_&D74Kt3za zwK}gu=e9A^^|Xnb{atnWrukRT$T}|zOE%m&ZzXHq`S}_NWm^R2IjmnBl3S3mFo8OA zZP(xSs4n`;ak(3hH073yx-CE3e5B>3xlPXND$mzP%l)l4TYndK{C?@|zbbb6jgOV} z2Slt-jWr)uA(65o4!0sX)dfhTr9V*8W|By6W}_bk!X(lb3Vfl0(AULz9?sF>N0ShV zw2aLAz}@uKR=fkz;NOaNU>e-5h|M-$u4spv;^W3Ut0d-JHiRz)!b7VJ%l!TQF(XTi;tImdIgqH-0mCV}oF9 zlE67fTPHL_5(Gq)zzdizmT?h%`QnQ3u`R6nDzwf~Bpt<3A<(?^f$QI^)wuL`?W4Pd)XI=eri#HKXr9=EiJRSea#o?l2?CzRM8O| z8|%Mgd6b-Ac9;yQ(r1%lu>9@F$jIBbS1-EOdW8<^4QPb$2SLU-DNFk3!RODP?~AXv zx2mwY?~*JTw4IpO{O;Xe&Hg?RB0B(vX5tI^I_T#4(@)Rdp48i`x964&QXR<$ zDcjQ0vS`tw(6F$3t5j4~`SzHYm^4$>{j821JLc(GGmdNvTL%XRmz9;3pDI_F!I9vMNSc`Wcci+_gws$~v!#X_a9!LyvKbtK(wS{@ zlu=VcLIM$cx#%UWl>Y*$? zha-TX+%R|$f`B6s4;2(7H&D^SuDbvwgG9v9Gj9Kd)rl8DVUMB!S%An4 zLLgVq(CMv#k_Qz{B2vH9iQig0%%|Cg6LcE&4M8w?oz3fHK4Va`JDx(gVBR=ub%l}N zA;M9p%b_~YwHL3aa0YzaHc&D92W)zFfgnP}&K@`+NZ~jYVZb*!Re&l8_U>N7C!6m* zm5@>Qe%&PZg>z;a1z^euD`qhTypA+nYH+)z9L<+}T003{!GjQCA63cigJbROr|!MI z`p>ITJ3sGi@j@EFIZdGG#h!n$T!7_hVa=ZLcCWd^PiK<+3}P0FEc%rYW<@S ze6zDNv5x*R-IJx0Dbrm$*_u>@fgXL=9i5~yM=f+@x&vyJ+y=Wxt1>-0GxWPt8}g(A zA~n6bmk;Nqc8?8^y~cCQhHEyvd2IT=X*X-+!SG;;1~wMqPsu)=@R2~5Nhjv}vLy4G z8TmT`!qwed+L&dfArZc;t1d3T+#PO9x0q<~W+`2et)0kAIWy3oGU9JKu#zGl#vq>oUnm1zbCua*h=E#;5h%94VOa_GG1cRd)0S=huxMi5IK;T<$&D zo~k|3+bXRy5f9Sx-cbC4 z-6lb4!vzb11NCV*MCL#bg@6JSo-)raf)ciY?rI1} z1QmE6Cc*&@{GG^pS+FepgwkWN8j4*r`v-6kc|h-REh3T;y+vZ4o#j!1sx44MbWwRn zAK)jL4I@`*=OhPt+mrzNzJ^gU_ z@Zm>$*E}$`@$eXDC|z~TvuS+o(5IF;+W^UdXkVnON!r|5X|Pd8M@L!NL~&86n2*{C zx=P7Xa$vvvbwWlYuCLFHV;`!j4yBt1cBUO(XCXPup+xO8U7aG`I?P*MX5Xu^kG5Ij zel|${*`ZG6?=lUikX8B=iw?!@yZ1JElL(XUP^IQb*DUGY8pO5#yni(-y!g$s^;cdvf|R z4k{rm=7cuU9B_d)B4q2{RT3iqU_*AOe3MW_h}Nz$CuogqZEfA%Q>#1g$l&Ca+@)wx zAZ#>e;uqfje4GuDRtDQBuErlL7z{>sc6MJMRU)^g<>BZJqFv1i0=wS-@_gn64HRlj z669scnyUwFt*qqytel;lZERvb*nP=$iIfzLPNzpiJl+=@A;Bk6gam=eq4K*IL`WFT zA<%Gl&TlBq;N0?wPFuI&>0;1+ChW(NrBMQejG0=R3L0@6L!OLJdC<*$i zWbJybB4MO(+D2F7p8Me3>dl3|u`;nYZgfynI%GTt9?DF7?4q-SoXQGH^vykbBWllRI4~<`h|$NpT)L}jy$)mpVaTHScL`{X1jGa`DT1dM%o z!Q*fb?K6XIChNlb3sLtTF_Tt(e!NXDZN)1ei;?WO-W#P}LwmcIpGoWW&6mj>9B7l; zp&w{Ap?nv*=8Lw!@E>pkYH}xX*5;b!`gb<4q*meDX=m?V;_I5~)F$40L(F?*nB~kg zi7XtPbT;wOtUaA1Q@d6ck@5_AXcHvg-`}sStek7rtC@Q3v#+UNs-M&~cV$gntnKV8 zHj-;y8^h_zjR9|KQ&sCcJkwG$yq*tt#l~wdgQQ=2PO%8i-|d5LcCVVLqk_}YP$YW| zr`EBKMX&cBeO{}tJ^G5)SQ9zk?nt#6 zZ`z!!luHsHm7m(Y#DDjvbzNlp)m>}s6}yUiF!nr2>9nATRMEgAJ**Uu?O`>YjG*&u zHo*vA;Yivf1Z5Q;HnCYDa^UEVAQ5(<((z2&3|?b@|#H+MbQ zyT(5-Fj9&Oq^MR9djItwUyM_>(v>MvCjx@=M^VE~h&+3FQ6ikp-O{D2&!!yCP~w8< zdayiU5e-QqCW?rdxH2uBdu44oJ0hk=DATTK>swfJSo2V%s z=cm9gH~l+lcEfh=-ecUue|EqrWraFSR0@s{h{-4a*!tTS0-#Mp5lS1|^ixx3VzT|ebrvclQ{#E|d>d8Gue|!_ zdRjU*DmCkAM`qghG*Q)omPb!B&DQeh(VP}%Cr7zWwBp>{++7ch*WAp@&o33dtY$zb zQx=}MCO+0TgG9BmgnKVT3WZ{1v_JOm>JCj*Im0q}gO=9T6UUGH8V1M?zWZ6K5sv7+Gn4QpGYp!@q` z&6bKd1gRO(*OV+G55O%9R7}hHj!}Ti8B@4COP4NRA|CDcG*{SjQ~&|2WDfl!v|cVs zpXm9ErBQ!zVQVS6^Lkl3vNZu;8z5l)hASkvZp4^@m;RXS7^S-5zj#FjBOKHmqBaf~ zqV{2EG+`i@eLV-Tjhqjk(PbX>f&f+t4|_P*co-xx4bC+lXh~eC;R_!09eV=5Mi@Xk zClFHFx^}E$%epY=5(P>1^WpHt+lN_u=hWnh=4<7j&JUN;R8NRC_ZfpbV_jK3Z=W5~ zXPjpI$Z%vhGg4k+N@^lg&Tb0@XMYMzu1;=9ZcXk?cJzVim=NhlNy69phS=9f`8f%| zcc<^eEA5{u+CDqvzoT)%=jB~0_G(?~_8MJ`dbkNa_giD%LbIX#5>Y7?Fdd`-T-kCS z{`9^W1ORJ)xdYMQ+xiFh#XZ{SIu@_4c+V2FIc@u3#q2^!AW1UVPSJb3&GN#(0pQFL AMF0Q* literal 0 HcmV?d00001 diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java index 79aeddb..f87385e 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java @@ -46,7 +46,7 @@ import ve.ucv.ciens.cicore.icaro.ryabi.utils.QuitButtonListener; @SuppressWarnings("deprecation") public class RyABI { private static final float WHEEL_DIAMETER = 56.0f; - private static final float TRACK_WIDTH = 155.0f; + private static final float TRACK_WIDTH = 144.0f; private static ArcRotateMoveController pilot; private static UltrasonicSensor sonar; diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java index 57997d7..7f4b2f0 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java @@ -53,14 +53,15 @@ public class CatchBallBehavior extends BaseBehavior { @Override public void action() { - pilot.travel(65); + pilot.travel(60); Motor.B.backward(); try { Thread.sleep(2000); } catch(InterruptedException e) { }; Motor.B.stop(true); + Motor.B.suspendRegulation(); /* Turn towards the start line and start moving. */ Rotations.rotate90(compass, pilot); Rotations.rotate90(compass, pilot); - pilot.travel(200); + pilot.travel(350); state.setState(States.WANDER); } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java index 34a2d0b..516cb5d 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java @@ -63,18 +63,19 @@ public class SearchBallBehavior extends BaseBehavior { @Override public boolean takeControl() { /* If the ball has already been found then this behavior should not take control again. */ - if(ballFound) + if(ballFound) { return false; + } /* If the current state is SEARCH_BALL then set the detectors and take control. */ - if(state.getState() == States.SEARCH_BALL) { + if(state.getState() == States.SEARCH_BALL && !ballFound) { setDetectors(); return true; } /* If the state is not SEARCH_BALL but there is at least one light feature detected * indicating that the finish line has been reached then set the detectors and take control. */ - if(queue.hasNextLightSensorEvent()) { + if(queue.hasNextLightSensorEvent() && !ballFound) { state.setState(States.SEARCH_BALL); setDetectors(); @@ -91,6 +92,7 @@ public class SearchBallBehavior extends BaseBehavior { @Override public void action() { + boolean _ballFound = false; boolean obstacleFound = false; /* Discard unneeded touch events. */ @@ -106,9 +108,11 @@ public class SearchBallBehavior extends BaseBehavior { pilot.stop(); if(!obstacleFound) { - ballFound = searchBall(); - if(ballFound) + _ballFound = searchBall(); + if(_ballFound) { + ballFound = true; state.setState(States.BALL_FOUND); + } } else { ballFound = true; Rotations.rotate90(compass, pilot); @@ -127,7 +131,7 @@ public class SearchBallBehavior extends BaseBehavior { private void setDetectors() { detectorHandler.enableTouchDetector(); detectorHandler.disableLightDetector(); - detectorHandler.disableRangeDetector(); + detectorHandler.enableRangeDetector(); } private void moveLeft() { @@ -157,17 +161,17 @@ public class SearchBallBehavior extends BaseBehavior { private boolean searchBall() { boolean found = false; - detectorHandler.enableRangeDetector(); - activeWait(1000); + while(queue.hasNextRangeSensorEvent()) + queue.getNextRangeSensorEvent(); + + try { Thread.sleep(2000); } catch(InterruptedException e) { }; if(queue.hasNextRangeSensorEvent()) { - /* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */ - state.setState(States.BALL_FOUND); + found = true; /* Discard unneeded range features. */ while(queue.hasNextRangeSensorEvent()) queue.getNextRangeSensorEvent(); } - detectorHandler.disableRangeDetector(); return found; } @@ -188,12 +192,4 @@ public class SearchBallBehavior extends BaseBehavior { return obstacleFound; } - - private void activeWait(long milliseconds) { - long then = System.currentTimeMillis(), now; - System.out.println("Waiting."); - do { - now = System.currentTimeMillis(); - } while(now - then < 1000); - } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java index a0d4623..4f818af 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java @@ -41,7 +41,7 @@ public final class FeatureDetectorsHandler { /** * Gets the singleton instance of this class. * - * @return the instacne. + * @return the instance. */ public static FeatureDetectorsHandler getInstance() { return SingletonHolder.INSTANCE;