1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2% 3% FILE : Env/legorcx.pl 4% Time-stamp: <02/11/30 01:16:39 ssardina> 5% 6% Author : First, by Maurice Pagnucco and Hector J. Levesque 7% Then, by Sebastian Sardina 8% DESCRIPTION : Prolog code to establish communication with 9% LEGO Mindstorms RCX 10% contains sockets, strings, OS tools, others 11% email : ssardina@cs.toronto.edu 12% WWW : www.cs.toronto.edu/~ssardina 13% TYPE CODE : system independent predicates 14% TESTED : ECLIPSE 5.4 http://www.icparc.ic.ac.uk/eclipse/ 15% SWI Prolog 5.0.10 under RedHat Linux 6.2-8.0 16% 17%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18% 19% June 15, 2000 20% 21% This software was developed by the Cognitive Robotics Group under the 22% direction of Hector Levesque and Ray Reiter. 23% 24% Do not distribute without permission. 25% Include this notice in any copy made. 26% 27% 28% Copyright (c) 2000 by The University of Toronto, 29% Toronto, Ontario, Canada. 30% 31% All Rights Reserved 32% 33% Permission to use, copy, and modify, this software and its 34% documentation for non-commercial research purpose is hereby granted 35% without fee, provided that the above copyright notice appears in all 36% copies and that both the copyright notice and this permission notice 37% appear in supporting documentation, and that the name of The University 38% of Toronto not be used in advertising or publicity pertaining to 39% distribution of the software without specific, written prior 40% permission. The University of Toronto makes no representations about 41% the suitability of this software for any purpose. It is provided "as 42% is" without express or implied warranty. 43% 44% THE UNIVERSITY OF TORONTO DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS 45% SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND 46% FITNESS, IN NO EVENT SHALL THE UNIVERSITY OF TORONTO BE LIABLE FOR ANY 47% SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER 48% RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF 49% CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN 50% CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 51% 52%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 53% 54% NOTE: All communication between Prolog and the RCX is initiated by Prolog 55% 56% This file defines the following top level predicates 57% 58% -- initializeRcx 59% prepare the RCX for reading 60% -- finalizeRcx 61% finished with RCX for reading and writing 62% -- sendRcxActionNumber(+Num, -Result) 63% send the action number Num to the RCX, and return 64% the value Result from the RCX 65% -- receiveRcxActionNumber(-Actions) 66% receive 0 or 1 action numbers in list Actions from RCX. 67% Fail if no reply from RCX 68% 69% The following *system dependent* predicates are assumed to be predefined: 70% 71% -- initRcx : initialize serial port, baud, parity etc. for RCX 72% -- openRcxRead : open RCX serial port for reading 73% -- openRcxWrite : open RCX serial port for writing 74% -- closeRcx : close RCX serial port 75% -- getRcxByte(-Ascii): get a character from RCX 76% -- eofRcx : succeed if there is no character from RCX to read 77% -- putRcxByte(+Ascii): write a character to RCX 78% -- currentTime(-Time): current system time in 100ths of seconds 79% -- waitUntilRcx(+End): optionally wait (busy or suspend) until End 80% system time or until RCX input arrives. 81% Fail if End is passed 82%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 84%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 85% CONSTANTS 86%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 87 88% Msg numbers in RCX for send/receive 89% rcxMessageRange(+MesgType, -MesgLow, -MesgHigh, -MesgBase): Send messages 90% using numbered RCX messages in the range MesgLow--MesgHigh in 91% base MesgBase (= 2^n where n is the number of bits per message) 92 93rcxMessageRange(action, 32, 63, 16). % Primitive action message range 94rcxMessageRange(value, 64, 79, 8). % Sensing value message range 95 96% Special message numbers 97rcxPanicMessage(80). % Panic message - RCX is in a panicked state 98rcxAbortMessage(81). % Abort message - reset RCX 99rcxContinueMessage(82). % Used for synchronisation in multi-part messages 100rcxExogReqMessage(83). % Request whether RCX has exogenous action to report 101rcxNoExogMessage(84). % Response from RCX when no detected exogenous actions 102rcxDelayMessage(85). % Response from RCX to obtain more time for the 103 % execution of a primitive action 104 105% Delays in hundredths of seconds 106rcxResponseDelay(200). % Normal delay for RCX that is ready to answer 107%rcxResponseDelay(30). % Normal delay for RCX that is ready to answer 108rcxPyramidDelay(600). % Window of time while infrared Pyramid is active 109rcxLongDelay(1500). % Time for RCX to complete longest behaviour 110rcxGetDelay(50). % Minimum time between subsequent rcxGetMess 111 112% We assume that we are open for read except during putRcxList below 113 114% initializeRcx: Perform any necessary initialization of RCX 115initializeRcx :- 116 initRcx, % Initialise serial port settings 117 openRcxRead. % Have RCX ready to read 118 119% finalizeRcx: Shut down RCX 120finalizeRcx :- 121 closeRcx. % Close serial port 122 123% rcxMess(?MesgNo, ?CharList): Verifies that CharList is the correct 124% sequence of 9 bytes for user message number MesgNo 125rcxMess(MesgNo, CharList) :- 126 CharList = [85, 255, 0, 247, 8, MesgNo, N1, CheckSum, C1], 127 N1 is 255 - MesgNo, 128 CheckSum is mod(247 + MesgNo, 256), 129 C1 is 255 - CheckSum. 130 131%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 132% LOW LEVEL SENDING TO RCX - put lists of ascii bytes 133%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 135% putRcxMess(+MesgNo): Send message number MesgNo to RCX 136putRcxMess(MesgNo) :- 137 rcxMess(MesgNo,CharList), % Generate a list of 9 characters 138 putRcxList(CharList). % Send them out to the RCX 139 140% putRcxList(+CharList): Open serial port for writing and transmit CharList 141putRcxList(CharList) :- 142 closeRcx, 143 openRcxWrite, 144 putRcxL(CharList), 145 closeRcx, 146 openRcxRead. 147 148% putRcxL(+CharList): Send characters in CharList one at a time 149putRcxL([]). 150putRcxL([FirstChar|CharList]) :- 151 putRcxByte(FirstChar), 152 putRcxL(CharList). 153 154%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 155% LOW LEVEL RECEIVING FROM RCX - get lists of ascii bytes 156%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 157 158% getRcxMess(+MesgNo, +End): Get message number MesgNo from RCX by End time. 159% Succeeds if message of correct format received within End time 160getRcxMess(MesgNo, End) :- 161 getRcxList(9, CharList, End), % Read a list of 9 bytes 162 rcxMess(MesgNo, CharList). % Check the format of Mess 163 164% getRcxList(+NumBytes, -CharList, +End): Returns NumBytes of data from 165% RCX before End time has passed. On backtracking, it uses maybeSkip to 166% obtain the next byte from the RCX, while discarding the first byte 167getRcxList(NumBytes, CharList, End) :- 168 getRcxCount(NumBytes, Z, E, End), 169 maybeSkip(Z, E, CharList, End). 170 171% getRcxCount(+NumBytes, -CharList, -ListEnd, +End): Reads NumBytes of 172% input from RCX and returns it in CharList before End. The end of the 173% list ListEnd is left open so that new bytes can be added as needed. 174getRcxCount(0, L, L, _). 175getRcxCount(NumBytes, [X|L], E, End) :- 176 NumBytes > 0, 177 waitRcxByte(X, End), 178 M1 is NumBytes - 1, 179 getRcxCount(M1, L, E, End). 180 181% maybeSkip(+CurList, -NewEnd, -NewList, +End): Either return CurList 182% of bytes, or if time remains, drop a byte and read a new one at the end 183maybeSkip(Z, [], Z, _). 184maybeSkip([_|Z], [X|E], L, End) :- 185 waitRcxByte(X, End), 186 maybeSkip(Z, E, L, End). 187 188% waitRcxByte(-Ascii, +End): Get a byte from RCX before End time or fail 189waitRcxByte(Ascii, End) :- 190 eofRcx -> 191 (waitUntilRcx(End), waitRcxByte(Ascii, End)) ; 192 getRcxByte(Ascii). 193 194%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 195% HIGH LEVEL COMMUNICATION 196% The high level primitives will try repeatedly to send a message or to 197% receive a message in small chunks of time, up until a given End time. 198%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 199 200% sendRcxMessage(+MesgNo, +End, -Reply): Repeatedly send message MesgNo 201% to RCX until reply received from RCX. Fails if there is no reply 202% before End. 203 204sendRcxMessage(Msg, End, Reply) :- 205 singleGetTime(End, End2), 206 putRcxMess(Msg), 207 (getRcxMess(Reply, End2) -> true ; sendRcxMessageAgain(Msg, End, Reply)). 208 209sendRcxMessageAgain(Msg, End, Reply) :- % Here if getRcxMess above failed 210 currentTime(Now), 211 rcxResponseDelay(Delay), 212 End > Now+Delay, % Continue if there is still time 213 sendRcxMessage(Msg, End, Reply). 214 215% singleGetTime(+End, -End2): End2 is maximum time between Now and End 216% for a single getRcxMess. It must not exceed the time the infrared 217% pyramid is active 218singleGetTime(End, End2) :- 219 currentTime(Now), 220 rcxPyramidDelay(Delay), 221 (Delay < End-Now -> 222 End2 is Now + Delay; 223 End2 is End). 224 225%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 226% In the actual communication to/from the RCX, to send or receive action N, 227% we need to break N down into a sequence of messages N1, N2, ..., Nk. 228% Similarly, to receive a sensing value N, we receive a sequence of messages. 229% Action and sensing values are sent using different ranges. 230% The predicate rcxMessageRange is used to control what the Ni should be. 231% In between each message Ni, a message of rcxContinueMessage synchronizes. 232%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 233 234% decodeNumber(+N, +NumberType, -Nums): Decodes number N of type NumberType 235% into a list for sending to RCX 236decodeNumber(N, NumberType, [First|Rest]) :- 237 rcxMessageRange(NumberType, Low, _, Base), 238 (N < Base -> (First is N+Low, Rest = []) ; 239 (First is mod(N,Base) + Low + Base, 240 N1 is N // Base, 241 decodeNumber(N1, NumberType, Rest))). 242 243% getEncodedNumber(+First, +NumberType, +End, -N): Interprets First as the 244% first part of an encoding of NumberType and goes on to receive 245% messages from RCX as necessary to encode number N (a sensing value). 246% Sends rcxContinueMessages as needed. Fails if End time has passed 247getEncodedNumber(First, NumberType, End, N) :- 248 rcxDelayMessage(First), !, % Received a request from RCX for more 249 % time to execute primitive action 250 rcxContinueMessage(Num), % Grant this by returning rcxContinueMessage 251 sendRcxMessage(Num, End, Ans), 252 getEncodedNumber(Ans, NumberType, End, N). 253 254 255getEncodedNumber(First, NumberType, End, N) :- 256 rcxMessageRange(NumberType, Low, High, Base), 257 rcxContinueMessage(Ack), 258 First >= Low, First =< High, % Fail if not in Low-High range 259 M is First - Low, % Assign M in 0--(Base-1) range 260 (M < Base -> N is M ; % Return M if only one message needed 261 (sendRcxMessage(Ack, End, Next), % Else get next number 262 getEncodedNumber(Next, NumberType, End, Temp), 263 N is (M - Base) + Base * Temp)). % Decode final number 264 265% sendRcxNumbers(+Nums, +ResultType, +End, -Result): Sends the numbers in 266% the list Nums to the RCX as a sequence of messages, ensuring it gets 267% continue messages between each one. Fails if End time has passed. 268% After the last number, the message received is interpreted as the 269% number Result of ResultType (action or value), using getEncodedNumber. 270sendRcxNumbers([First|Rest], ResultType, End, Result) :- 271 sendRcxMessage(First, End, Answer), 272 (Rest == [] -> 273 getEncodedNumber(Answer, ResultType, End, Result) ; 274 (rcxContinueMessage(Answer), 275 sendRcxNumbers(Rest, ResultType, End, Result) )). 276 277%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 278% This handles communication between RCX and Prolog required by IndiGolog. 279%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 280 281% sendRcxActionNumber(+Num, -Result): Send action number N and obtain 282% sensing value Result. Fails if RCX does not return Result 283sendRcxActionNumber(Num, Result) :- 284 rcxLongDelay(C), 285 currentTime(Now), 286 End is Now + C, 287 decodeNumber(Num, action, Nums), % Translate action number N 288 sendRcxNumbers(Nums, value, End, Result). % Send Nums and obtain sensing 289 % value Result 290 291receiveRcxActionNumber(Actions) :- 292 currentTime(Now), 293 rcxResponseDelay(Delay1), 294 End1 is Now + Delay1, 295 rcxExogReqMessage(Msg), 296 rcxLongDelay(Delay2), 297 End2 is Now+Delay2, 298 sendRcxMessage(Msg, End1, Ans), % Query RCX for exogenous actions 299 ((rcxNoExogMessage(Ans), Actions = []) ; % RCX reports none 300 (getEncodedNumber(Ans, action, End2, N), 301 Actions = [N])). % RCX returns action 302 303%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 304% EOF: Env/legorcx.pl 305%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%