From mcermak at common-lisp.net Wed Aug 15 21:40:33 2007 From: mcermak at common-lisp.net (mcermak at common-lisp.net) Date: Wed, 15 Aug 2007 17:40:33 -0400 (EDT) Subject: [nxtlisp-cvs] r1 - libbtnxtlisp nxtlisp Message-ID: <20070815214033.407FA48152@common-lisp.net> Author: mcermak Date: Wed Aug 15 17:40:32 2007 New Revision: 1 Added: LICENSE TODO libbtnxtlisp/ libbtnxtlisp/Makefile libbtnxtlisp/bluetooth.c libbtnxtlisp/bluetooth.h nxtlisp/ nxtlisp/communication.lisp nxtlisp/nxt.lisp nxtlisp/nxtlisp.asd nxtlisp/protocol-commands.lisp nxtlisp/remote-commands.lisp Log: NXTLisp 0.0.1 The first commit to the repository. NXTLisp is an Open Source project distributed under the Apache License 2.0 (see LICENSE file for more info) written in Common Lisp. Its aim is to provide a Common Lisp library for the Lego Mindstorms NXT robotic platform. It is heavily inspired by RCXLisp by Frank Klassner. libbtnxtlisp contains the C library developed from scratch and used to provide low-level access to OS BT stack. It currently supports Linux (tested) and Win XP with SP2 (not tested). OS X support may come in the future (help with porting the library is welcomed). nxtlisp directory contains the CL source files (nxt.lisp, remote-commands.lisp, protocol-commands.lisp, communication.lisp and nxtlisp.asd). There's not much functionality available right now, but stay tuned. Also, there is no documentation yet. However if you will play around and run into any bugs, please write to the mailing list: http://common-lisp.net/cgi-bin/mailman/listinfo/nxtlisp-devel Added: LICENSE ============================================================================== --- (empty file) +++ LICENSE Wed Aug 15 17:40:32 2007 @@ -0,0 +1,166 @@ +NXTLisp is free software licensed under the Apache License 2.0: + +Apache License, Version 2.0 + +Apache License +Version 2.0, January 2004 +http://www.apache.org/licenses/ + +TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + +1. Definitions. + +"License" shall mean the terms and conditions for use, reproduction, +and distribution as defined by Sections 1 through 9 of this document. + +"Licensor" shall mean the copyright owner or entity authorized by the +copyright owner that is granting the License. + +"Legal Entity" shall mean the union of the acting entity and all other +entities that control, are controlled by, or are under common control +with that entity. For the purposes of this definition, "control" means +(i) the power, direct or indirect, to cause the direction or management +of such entity, whether by contract or otherwise, or +(ii) ownership of fifty percent (50%) or more of the outstanding shares, or +(iii) beneficial ownership of such entity. + +"You" (or "Your") shall mean an individual or Legal Entity exercising +permissions granted by this License. + +"Source" form shall mean the preferred form for making modifications, +including but not limited to software source code, documentation source, +and configuration files. + +"Object" form shall mean any form resulting from mechanical transformation +or translation of a Source form, including but not limited to compiled +object code, generated documentation, and conversions to other media types. + +"Work" shall mean the work of authorship, whether in Source or Object form, +made available under the License, as indicated by a copyright notice that +is included in or attached to the work (an example is provided in the Appendix +below). + +"Derivative Works" shall mean any work, whether in Source or Object form, +that is based on (or derived from) the Work and for which the editorial +revisions, annotations, elaborations, or other modifications represent, +as a whole, an original work of authorship. For the purposes of this License, +Derivative Works shall not include works that remain separable from, +or merely link (or bind by name) to the interfaces of, the Work and +Derivative Works thereof. + +"Contribution" shall mean any work of authorship, including the original +version of the Work and any modifications or additions to that Work or +Derivative Works thereof, that is intentionally submitted to Licensor +for inclusion in the Work by the copyright owner or by an individual or +Legal Entity authorized to submit on behalf of the copyright owner. +For the purposes of this definition, "submitted" means any form of electronic, +verbal, or written communication sent to the Licensor or its representatives, +including but not limited to communication on electronic mailing lists, +source code control systems, and issue tracking systems that are managed by, +or on behalf of, the Licensor for the purpose of discussing and improving +the Work, but excluding communication that is conspicuously marked or otherwise +designated in writing by the copyright owner as "Not a Contribution." + +"Contributor" shall mean Licensor and any individual or Legal Entity on behalf +of whom a Contribution has been received by Licensor and subsequently +incorporated within the Work. + +2. Grant of Copyright License. Subject to the terms and conditions of +this License, each Contributor hereby grants to You a perpetual, worldwide, +non-exclusive, no-charge, royalty-free, irrevocable copyright license to +reproduce, prepare Derivative Works of, publicly display, publicly perform, +sublicense, and distribute the Work and such Derivative Works in Source +or Object form. + +3. Grant of Patent License. Subject to the terms and conditions of this +License, each Contributor hereby grants to You a perpetual, worldwide, +non-exclusive, no-charge, royalty-free, irrevocable (except as stated +in this section) patent license to make, have made, use, offer to sell, sell, +import, and otherwise transfer the Work, where such license applies only to +those patent claims licensable by such Contributor that are necessarily +infringed by their Contribution(s) alone or by combination of their +Contribution(s) with the Work to which such Contribution(s) was submitted. +If You institute patent litigation against any entity (including a cross-claim +or counterclaim in a lawsuit) alleging that the Work or a Contribution +incorporated within the Work constitutes direct or contributory patent +infringement, then any patent licenses granted to You under this License +for that Work shall terminate as of the date such litigation is filed. + +4. Redistribution. You may reproduce and distribute copies of the Work +or Derivative Works thereof in any medium, with or without modifications, +and in Source or Object form, provided that You meet the following conditions: + + 1. You must give any other recipients of the Work or Derivative Works +a copy of this License; and + + 2. You must cause any modified files to carry prominent notices stating +that You changed the files; and + + 3. You must retain, in the Source form of any Derivative Works that You +distribute, all copyright, patent, trademark, and attribution notices from +the Source form of the Work, excluding those notices that do not pertain to +any part of the Derivative Works; and + + 4. If the Work includes a "NOTICE" text file as part of its distribution, +then any Derivative Works that You distribute must include a readable copy +of the attribution notices contained within such NOTICE file, excluding those +notices that do not pertain to any part of the Derivative Works, in at least +one of the following places: within a NOTICE text file distributed as part of +the Derivative Works; within the Source form or documentation, if provided +along with the Derivative Works; or, within a display generated by the +Derivative Works, if and wherever such third-party notices normally appear. +The contents of the NOTICE file are for informational purposes only and do not +modify the License. You may add Your own attribution notices within Derivative +Works that You distribute, alongside or as an addendum to the NOTICE text from +the Work, provided that such additional attribution notices cannot be construed +as modifying the License. + +You may add Your own copyright statement to Your modifications and may provide +additional or different license terms and conditions for use, reproduction, or +distribution of Your modifications, or for any such Derivative Works as a whole, +provided Your use, reproduction, and distribution of the Work otherwise complies +with the conditions stated in this License. + +5. Submission of Contributions. Unless You explicitly state otherwise, any +Contribution intentionally submitted for inclusion in the Work by You to +the Licensor shall be under the terms and conditions of this License, without +any additional terms or conditions. Notwithstanding the above, nothing herein +shall supersede or modify the terms of any separate license agreement you may +have executed with Licensor regarding such Contributions. + +6. Trademarks. This License does not grant permission to use the trade names, +trademarks, service marks, or product names of the Licensor, except as required +for reasonable and customary use in describing the origin of the Work and +reproducing the content of the NOTICE file. + +7. Disclaimer of Warranty. Unless required by applicable law or agreed to in +writing, Licensor provides the Work (and each Contributor provides its +Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY +KIND, either express or implied, including, without limitation, any warranties +or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A +PARTICULAR PURPOSE. You are solely responsible for determining the +appropriateness of using or redistributing the Work and assume any risks +associated with Your exercise of permissions under this License. + +8. Limitation of Liability. In no event and under no legal theory, whether in +tort (including negligence), contract, or otherwise, unless required by +applicable law (such as deliberate and grossly negligent acts) or agreed to in +writing, shall any Contributor be liable to You for damages, including any +direct, indirect, special, incidental, or consequential damages of any +character arising as a result of this License or out of the use or inability to +use the Work (including but not limited to damages for loss of goodwill, work +stoppage, computer failure or malfunction, or any and all other commercial +damages or losses), even if such Contributor has been advised of the possibility +of such damages. + +9. Accepting Warranty or Additional Liability. While redistributing the Work +or Derivative Works thereof, You may choose to offer, and charge a fee for, +acceptance of support, warranty, indemnity, or other liability obligations +and/or rights consistent with this License. However, in accepting such +obligations, You may act only on Your own behalf and on Your sole +responsibility, not on behalf of any other Contributor, and only if You agree +to indemnify, defend, and hold each Contributor harmless for any liability +incurred by, or claims asserted against, such Contributor by reason of your +accepting any such warranty or additional liability. + +END OF TERMS AND CONDITIONS Added: TODO ============================================================================== --- (empty file) +++ TODO Wed Aug 15 17:40:32 2007 @@ -0,0 +1,19 @@ +NXTLisp TODO file + +* near future: + - unite documentation (in .lisp) + - add a find_all_BT_devices function (with apropriate CL bindings) + - update web + +* far future: + - check if every numerical calulation respects little-endianess + - make it asdf-installable (halfway there, probably) + - asdf can compile and load C functions, try it + - test on multiple CL implementations + - write a manual + - create example algorithms/programs + - check all files for TODO (constant task) + - add README/BUGS/... files (?) + + + Added: libbtnxtlisp/Makefile ============================================================================== --- (empty file) +++ libbtnxtlisp/Makefile Wed Aug 15 17:40:32 2007 @@ -0,0 +1,46 @@ +# +# Contributors: Milan Cermak, milan.cermak at gmail.com +# +# Description: Makefile for libbtnxtlisp (Linux) +# + +# variables + +RM = rm -f +MV = mv +CC = gcc +CFLAGS = -std=c99 -Wall -pedantic + +LIBS = -lbluetooth +LIBDIR = /usr/lib/ + +SOURCES = bluetooth.c bluetooth.h +OBJ = bluetooth.o +TARGETS = libbtnxtlisp.so libbtnxtlisp.a +ARCH = libbtnxtlisp.tgz + +# targets + +all: $(TARGETS) win + +libbtnxtlisp.a: $(OBJ) + ar rc $@ $? + +libbtnxtlisp.so: $(OBJ) + $(CC) $(LIBS) -shared -o $@ $? + +$(OBJ): $(SOURCES) + $(CC) $(CFLAGS) -c -fPIC $< + + + +.PHONY: clean install pack + +clean: + $(RM) $(TARGETS) $(OBJ) + +install: + $(MV) $(TARGETS) $(LIBDIR) + +pack: + tar czf $(ARCH) $(SOURCES) Makefile \ No newline at end of file Added: libbtnxtlisp/bluetooth.c ============================================================================== --- (empty file) +++ libbtnxtlisp/bluetooth.c Wed Aug 15 17:40:32 2007 @@ -0,0 +1,545 @@ +/* + File: bluetooth.win.c + + Contributors: Milan Cermak; milan.cermak at gmail.com + + Description: libbtnxlisp source file for Linux and Windows XP SP2 + All functions are called from Common Lisp + + */ + +/* + TODO: there's probably no need for the channel argument in some + related functions - when talking to the NXT through BT, it + has to be in slave mode -> we have to be connected on the + first channel (slot 0 on NXT's display) + + +/* * * * * * * * * * * * * + + for Win compilation, #define NXT_WIN_XP + Win XP with Servise Pack 2 is a prerequisite + + * * * * * * * * * * * * */ + +#ifdef NXT_WIN_XP + +#include +#include +#include +#include + +#else + +#include +#include +#include +#include +#include +//#include + +#endif + +#include +#include "bluetooth.h" + + +#ifdef NXT_WIN_XP +// a Windows specific function to initialise the BT stack (or something) +int win_init(void) { + WORD version; + WSADATA wsa; + int error; + + version = MAKEWORD(2,2); + error = WSAStartup(version, &wsa); + + if (error != 0) + return -1; + if ((LOBYTE(wsa.wVersion) != 2) || + (HIBYTE(wsa.wVersion) != 2)) { + WSACleanup(); + return -1; + } + + return 0; + +} // win_init() +#endif + +/* + Functions connects to an NXT brick specified by the nxt_address + argument on the requested channel (port in BT terminology). If + channel is out of range (valid values are 1-30 inclusive), it uses + the first available channel. + Function returns a socket descriptor on success or -1 otherwise. + + */ +int open_nxt_stream(char *nxt_address, int *channel) { +#ifdef NXT_WIN_XP + + int nxt_socket, chan = *channel; + struct SOCKADDR_BTH nxt; + + int err = win_init(); + if (err < 0) + return -1; + + nxt_socket = socket(AF_BTH, SOCK_STREAM, BTHPROTO_RFCOMM); + if (nxt_socket < 0) + return -1; + + nxt.addressFamily = AF_BTH; + nxt.btAddr = 0; + +#else + + int nxt_socket, chan = *channel; + struct sockaddr_rc nxt = { 0 }; + + nxt_socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); + + if (nxt_socket < 0) + return -1; + + nxt.rc_family = AF_BLUETOOTH; + str2ba(nxt_address, &nxt.rc_bdaddr); + +#endif + /* is this really necessary? I have a feeling the only possible way + to connect to the NXT is through channel 1; investigate! + */ + if ((chan < 1) || (chan > 30)) { + // use first available channel if illegal requested + int i, status; + for (i = 1; i <= 30; i++) { + nxt.port = i; + status = connect(nxt_socket, (struct sockaddr*) &nxt, sizeof(nxt)); + if ((status < 0) && (i < 30)) + continue; // try the next one + else if ((status < 0) && (i == 30)) + return -1; // no available channel + else { + *channel = i; // connected + break; + } + } + } + else { + // user specified a legal channel + nxt.port = chan; + int status; + status = connect(nxt_socket, (struct sockaddr*) &nxt, sizeof(nxt)); + if (status < 0) + return -1; + } + + return nxt_socket; + +} // open_nxt_stream() + +/* + If nxt_socket == 0, we're debugging packets in CL. This functions + serves as a wrapper and to keep things simpler and consistent higher. +*/ +int close_nxt_stream(int nxt_socket) { + + return (nxt_socket == 0) ? 0 : close(nxt_socket); + +} // close_nxt_stream() + +/* + Function for sending data to an NXT. It tries to send() the + the whole package. Package has to be correctly constructed in CL. + It takes three argument - nxt_socket is the descriptor of the + socket, msg is a pointer to the packet buffer and length is the + length of the packet (msg). + It returns 0 on success and -1 on failure, in which case length + carries the total bytes sent. + TODO: update comment +*/ +int send_msg(int nxt_socket, unsigned char *msg, int length, + int block, int timeout, + int retries, int retries_interval) { +#ifdef NXT_WIN_XP + int sent = 0; // already sent bytes + int remaining = length; // bytes not sent yet + int b; // bytes sent in one send() + + /* + handle additional connection-specifying data + semantics is described in the user's manual + */ + + int retries_done, send_flag; + int do_retries = 0, do_timeout = 0; + + /* this handles all 4 possible settings: + - both retries and timeout are zero + - retries is set, timeout is zero + - retries is zero, timeout is set + - both retries and timeout are set (handled by 2nd if) + */ + if ((retries == 0) && (timeout == 0)) + // in linux, it works this way: send_flag = (block == 0) ? 0 : + // MSG_DONTWAIT; + send flag = 0; + else if (retries != 0) { + // priority: retries > timeout + send_flag = 0; + do_retries = 1; + retries_done = 0; + } + else if (timeout != 0) { + send_flag = 0; + do_timeout = 1; + } + + // act accordingly + if (!do_retries && !do_timeout) { + // no retries, no timeout + b = (int) send(nxt_socket, msg, remaining, send_flag); + if (b != -1) + sent = b; + } + else if (do_retries) { + // send with retries + while ((retries_done <= retries) || + (sent < length)) { + if (retries_done != 0) + sleep(retries_interval); + b = (int) send(nxt_socket, msg+sent, remaining, send_flag); + if (b != -1) { + sent += b; + remaining -= b; + } + retries_done++; + } + } + else if (do_timeout) { + // send with timeout (implemented as a pseudo-timeout) + b = (int) send(nxt_socket, msg, remaining, send_flag); + if (b != -1) { + sent += b; + remaining -= b; + } + if (b < length) { + // didn't send the whole package + sleep(timeout); + b = (int) send(nxt_socket, msg+sent, remaining, send_flag); + if (b != -1) { + sent += b; + remaining -= b; + } + } + } + +#else + + int sent = 0; // already sent bytes + int remaining = length; // bytes not sent yet + int b; // bytes sent in one send() + + /* + handle additional connection-specifying data + semantics is described in the user's manual + */ + + int retries_done, send_flag = MSG_DONTWAIT; + int do_retries = 0, do_timeout = 0; + + /* this handles all 4 possible settings: + - both retries and timeout are zero + - retries is set, timeout is zero + - retries is zero, timeout is set + - both retries and timeout are set (handled by 2nd if) + */ + if ((retries == 0) && (timeout == 0)) + send_flag = (block == 0) ? 0 : MSG_DONTWAIT; + else if (retries != 0) { + // priority: retries > timeout + send_flag = MSG_DONTWAIT; + do_retries = 1; + retries_done = 0; + } + else if (timeout != 0) { + send_flag = MSG_DONTWAIT; + do_timeout = 1; + } + + // act accordingly + if (!do_retries && !do_timeout) { + // no retries, no timeout + b = (int) send(nxt_socket, msg, remaining, send_flag); + if (b != -1) + sent = b; + } + else if (do_retries) { + // send with retries + while ((retries_done <= retries) || + (sent < length)) { + if (retries_done != 0) + sleep(retries_interval); + b = (int) send(nxt_socket, msg+sent, remaining, send_flag); + if (b != -1) { + sent += b; + remaining -= b; + } + retries_done++; + } + } + else if (do_timeout) { + // send with timeout (implemented as a pseudo-timeout) + b = (int) send(nxt_socket, msg, remaining, send_flag); + if (b != -1) { + sent += b; + remaining -= b; + } + if (b < length) { + // didn't send the whole package + sleep(timeout); + b = (int) send(nxt_socket, msg+sent, remaining, send_flag); + if (b != -1) { + sent += b; + remaining -= b; + } + } + } + +#endif + return sent; +} // send_msg() + + +/* + Function for receiving data from an NXT. It tries to recv() + the whole packet. + It takes three arguments - nxt_socket is the descriptor of the + socket, msg is a pointer to a buffer in which the packet will + be stored and length represents how many bytes to receive (should + be the length of msg). + The operation is non-blocking - if the user demands receiving more + bytes than there are available, the function ends, returning number + of bytes successfully read until that point. + Function returns number of bytes received on success and -1 on failure, + in which case length holds total received bytes. + TODO: update comment +*/ +int recv_msg(int nxt_socket, unsigned char *msg, int length, + int block, int timeout, + int retries, int retries_interval) { +#ifdef NXT_WIN_XP + int received = 0; // already received bytes + int remaining = length; // bytes yet to be received + int b; + + /* + handle additional connection-specifying data + semantics is described in the user's manual + */ + + int retries_done, recv_flag; + int do_retries = 0, do_timeout = 0; + + /* this handles all 4 possible settings: + - both retries and timeout are zero + - retries is set, timeout is zero + - retries is zero, timeout is set + - both retries and timeout are set (handled by 2nd if) + */ + if ((retries == 0) && (timeout == 0)) + // recv_flag = (block == 0) ? 0 : MSG_DONTWAIT; + recv_flag = 0; + else if (retries != 0) { + // priority: retries > timeout + recv_flag = 0; + do_retries = 1; + retries_done = 0; + } + else if (timeout != 0) { + recv_flag = 0; + do_timeout = 1; + } + + + // act accordingly + if (!do_retries && !do_timeout) { + // no retries, no timeout + b = (int) recv(nxt_socket, msg, remaining, recv_flag); + if (b != -1) + received = b; + } + else if (do_retries) { + // receive with retries + while ((retries_done <= retries) || + (received < length)) { + if (retries_done != 0) + sleep(retries_interval); + b = (int) recv(nxt_socket, msg+received, remaining, recv_flag); + if (b == 0) + break; + else if (b != -1) { + received += b; + remaining -= b; + } + retries_done++; + } + } + else if (do_timeout) { + // receive with timeout (implemented as a pseudo-timeout) + b = (int) recv(nxt_socket, msg, remaining, recv_flag); + if (b != -1) { + received += b; + remaining -= b; + } + if (b < length) { + // didn't receive the whole package + sleep(timeout); + b = (int) recv(nxt_socket, msg+received, remaining, recv_flag); + if (b != -1) { + received += b; + remaining -= b; + } + } + } + +#else + + int received = 0; // already received bytes + int remaining = length; // bytes yet to be received + int b; + + /* + handle additional connection-specifying data + semantics is described in the user's manual + */ + + int retries_done, recv_flag = MSG_DONTWAIT; + int do_retries = 0, do_timeout = 0; + + /* this handles all 4 possible settings: + - both retries and timeout are zero + - retries is set, timeout is zero + - retries is zero, timeout is set + - both retries and timeout are set (handled by 2nd if) + */ + if ((retries == 0) && (timeout == 0)) + recv_flag = (block == 0) ? 0 : MSG_DONTWAIT; + else if (retries != 0) { + // priority: retries > timeout + recv_flag = MSG_DONTWAIT; + do_retries = 1; + retries_done = 0; + } + else if (timeout != 0) { + recv_flag = MSG_DONTWAIT; + do_timeout = 1; + } + + + // act accordingly + if (!do_retries && !do_timeout) { + // no retries, no timeout + b = (int) recv(nxt_socket, msg, remaining, recv_flag); + if (b != -1) + received = b; + } + else if (do_retries) { + // receive with retries + while ((retries_done <= retries) || + (received < length)) { + if (retries_done != 0) + sleep(retries_interval); + b = (int) recv(nxt_socket, msg+received, remaining, recv_flag); + if (b == 0) + break; + else if (b != -1) { + received += b; + remaining -= b; + } + retries_done++; + } + } + else if (do_timeout) { + // receive with timeout (implemented as a pseudo-timeout) + b = (int) recv(nxt_socket, msg, remaining, recv_flag); + if (b != -1) { + received += b; + remaining -= b; + } + if (b < length) { + // didn't receive the whole package + sleep(timeout); + b = (int) recv(nxt_socket, msg+received, remaining, recv_flag); + if (b != -1) { + received += b; + remaining -= b; + } + } + } + +#endif + return received; + +} // recv_msg() + + +/* + A helper function; plays a tone (who would have guessed it?) +*/ +void greeting_tone(int sock) { + + unsigned char comm[8]; + int len = 6; + int hz = 1200, duration = 800; + + // construct the packet + memcpy(&comm[0], &len, 2); + comm[2] = 0x00; + comm[3] = 0x03; + memcpy(&comm[4], &hz, 2); + memcpy(&comm[6], &duration, 2); + + int i; + printf("message: \n"); + for (i = 0; i < 8; i++) + printf("0x%02X ", comm[i]); + printf("\n"); + + write(sock, &comm, sizeof(comm)); + return; + +} // greeting_tone() + + + + +//***************************************************************************** + +// for testing purposes +// TODO: delete main() and greeting_tone() when stable + +int main(void) { + + //TODO: if channel != 1, the NXT doesn't play a sound + int socket, channel = 1; + char *nxt_address = "00:16:53:01:5F:28"; + unsigned char buf[20] = { 0 }; + int ret, len = sizeof(buf); + + socket = open_nxt_stream(nxt_address, &channel); + greeting_tone(socket); + sleep(1); + ret = recv_msg(socket, buf, len, 1, 0, 0, 0); + + printf("received: %d\n", ret); + int i; + for (i = 0; i < ret; i++) + printf("0x%02X ", buf[i]); + printf("\n"); + close_nxt_stream(socket); + + + return 0; + +} Added: libbtnxtlisp/bluetooth.h ============================================================================== --- (empty file) +++ libbtnxtlisp/bluetooth.h Wed Aug 15 17:40:32 2007 @@ -0,0 +1,28 @@ +/* + File: bluetooth.h + + Contributors: Milan Cermak; milan.cermak at gmail.com + + Description: Header file for bluetooth.h + + */ + + +/* + Function prototypes; for detailed description, see the .c file +*/ + +int open_nxt_stream(char *nxt_address, int *channel); + +int close_nxt_stream(int nxt_socket); + +int send_msg(int nxt_socket, unsigned char *msg, int length, + int block, int timeout, + int retries, int retries_interval); + +int recv_msg(int nxt_socket, unsigned char *msg, int length, + int block, int timeout, + int retries, int retries_interval); + +// for testing +void greeting_tone(int tmp_socket); Added: nxtlisp/communication.lisp ============================================================================== --- (empty file) +++ nxtlisp/communication.lisp Wed Aug 15 17:40:32 2007 @@ -0,0 +1,194 @@ +;;;; +;;;; +;;;; File: communication.lisp +;;;; +;;;; License: Apache License 2.0 +;;;; +;;;; Contributors: Milan Cermak, milan.cermak at gmail.com +;;;; +;;;; Description: This file includes stuff relevant to communication +;;;; in the NXTLisp project. +;;;; +;;;; + + +#| + + Copyright 2007 Milan Cermak + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +|# + + +(in-package :nxt) + +;; define libbtnxtlisp CFFI wrappers + +(define-foreign-library libbtnxtlisp + (t (:default "libbtnxtlisp"))) + +;; ok, so what's the difference between load-foreign-library and use-foreign-library? +;; (load-foreign-library libbtnxtlisp) +(use-foreign-library libbtnxtlisp) + +(defcfun "open_nxt_stream" :int + (nxt-address :string) (channel :pointer)) + +(defcfun "close_nxt_stream" :int + (socket :int)) + +(defcfun "send_msg" :int + (socket :int) + (msg :string) + (length :int) + (blocking :int) + (timeout :int) + (retries :int) + (retries-interval :int)) + +(defcfun "recv_msg" :int + (socket :int) + (msg :string) + (length :int) + (blocking :int) + (timeout :int) + (retries :int) + (retries-interval :int)) + + + +(defun decode-reply (reply type) + "Decodes the received packet. Reply has to be an array." + (let ((decoded)) + (case type + (:simple (if (eql #X00 (aref reply 4)) ; check the status byte + (setf decoded t) + (setf decoded nil))) + (:raw (if (eql #X00 (aref reply 4)) + (progn + (loop for i from 5 to (1- fill-pointer reply) doing + (push (aref reply i) decoded)) + (setf decoded (reverse decoded))) + (setf decoded nil))) + (:ubyte (if (eql #X00 (aref reply 4)) + (setf decoded (aref reply 5)) + (setf decoded nil))) + (:uword (if (eql #X00 (aref reply 4)) + (setf decoded (+ (aref reply 5) (* (aref reply 6) 256))) ; little endian + (setf decoded nil))) + (:ulong (if (eql #X00 (aref reply 4)) + (setf decoded (+ (aref reply 5) (* (aref reply 6) 256) ; little endian + (* (aref reply 7) 65536) (* (aref reply 8) 16777216))) + (setf decoded nil))) + (:filename (if (eql #X00 (aref reply 4)) + (progn + (setf decoded (make-string 20 :initial-element #\0)) + (loop for ri from 5 to 24 + and si from 0 doing + (setf (aref decoded si) (code-char (aref reply ri))))) + (setf decoded nil))) + (otherwise (warn "decode-reply: unknown decode request"))) + decoded)) + +(defun send-packet (socket packet blocking timeout retries retries-interval) + "Function handles packet sending through calling a C function. +Socket must be a file descriptor of an opened socket. Packet must +be a well-formed BT packet in the form of an array with :fill-pointer. +Returns t on successful sending or nil otherwise." + (let ((length (fill-pointer packet))) + (if (eql socket nil) ; if debugging packets + packet ; show what would be sent + (with-foreign-string (msg packet) + (with-foreign-objects ((sd :int) (len :int) (blk :int) + (tout :int) (retr :int) (retr-i :int)) + ;; set the foreign variables to correct values + (setf (mem-ref sd :int) socket + (mem-ref len :int) length + (mem-ref blk :int) (if blocking 0 1) ; as implemented in libbtnxtlisp + (mem-ref tout :int) timeout + (mem-ref retr :int) retries + (mem-ref retr-i :int) retries-interval) + ;; send packet (by calling foreign function) and store return value + (send-msg (mem-ref sd :int) msg (mem-ref len :int) (mem-ref blk :int) + (mem-ref tout :int) (mem-ref retr :int) (mem-ref retr-i :int))))))) + +(defun recv-packet (socket length blocking timeout retries retries-interval) + "Function receives packet from the NXT, usually a reply to a command. It +returns an array of (unsigned-byte 8) representing the packet." + (let ((received 0) + (retval (make-array length :element-type '(unsigned-byte 8) :fill-pointer 0))) + (with-foreign-objects ((sd :int) (len :int) (blk :int) (tout :int) + (retr :int) (retr-i :int)) + ;; set foreign variables to correct values for the transmission + (setf (mem-ref sd :int) socket + (mem-ref len :int) length + (mem-ref blk :int) (if blocking 0 1) + (mem-ref tout :int) timeout + (mem-ref retr :int) retries + (mem-ref retr-i :int) retries-interval) + ;; msg serves as a buffer for the packet received from the NXT + (with-foreign-object (msg :uchar length) + (setf received (recv-msg (mem-ref sd :int) msg (mem-ref len :int) + (mem-ref blk :int) (mem-ref tout :int) + (mem-ref retr :int) (mem-ref retr-i :int))) + (if (eql received -1) + (error "recv-packet: packet couldn't be received")) + (dotimes (i received) + (vector-push (mem-aref msg :uchar i) retval)))) + retval)) + +(defmacro with-open-nxt-stream ((nxt-address port &rest keys) &body body) + `(call-with-open-nxt-stream ,nxt-address ,port (lambda () , at body) , at keys)) + +(defun call-with-open-nxt-stream (nxt-address port function &key + (blocking *blocking-default*) + (reply *reply-default*) + (timeout *timeout-default*) + (retries *retries-default*) + (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + (let ((*blocking-default* blocking) + (*reply-default* reply) + (*timeout-default* timeout) + (*retries-default* retries) + (*retries-interval-default* retries-interval) + (*default-answer-default* default-answer)) + (let ((*standard-nxt-io* (if (eql 0 nxt-address) + 0 + (call-open-nxt-stream nxt-address port)))) + (if (< *standard-nxt-io* 0) ; couldn't open socket + *default-answer-default* + (progn + (unwind-protect + ;; give the NXT time to recover from the fact, that we have + ;; just established a connection (0.5 is an empirical value) + (sleep 0.5) + (funcall function) + (call-close-nxt-stream *standard-nxt-io*))))))) + + +(defun call-open-nxt-stream (nxt-address channel) + "Calls foreign function open-nxt-stream." + (with-foreign-string (adr nxt-address) + (with-foreign-pointer (chan 1) + (setf (mem-ref chan :int) channel) + (open-nxt-stream adr chan)))) + +(defun call-close-nxt-stream (socket) + "Calls foreign function close-nxt-stream." + (with-foreign-object (sd :int) + (setf (mem-ref sd :int) socket) + (close-nxt-stream (mem-ref sd :int)))) + +;; EOF Added: nxtlisp/nxt.lisp ============================================================================== --- (empty file) +++ nxtlisp/nxt.lisp Wed Aug 15 17:40:32 2007 @@ -0,0 +1,69 @@ +;;;; +;;;; +;;;; File: nxt.lisp +;;;; +;;;; License: Apache License 2.0 +;;;; +;;;; Contributors: Milan Cermak, milan.cermak at gmail.com +;;;; +;;;; Description: A general purpose file. Includes global +;;;; variables, macros, .... +;;;; +;;;; + + + +#| + + Copyright 2007 Milan Cermak + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +|# + + +(in-package :nxt) + +(defvar *standard-nxt-io* 0 + "A file designator of a BT socket opened for communication with +a NXT brick. It is obtained by calling a function defined in the +libbtnxtlisp.") + +(defvar *blocking-default* nil + "Flag specifying, if sending/receiving should be blocking. +nil - don't block +t - block") + +(defvar *reply-default* nil + "Flag representing, if a command reply is requested. +nil - no reply requested +t - reply requested") + +(defvar *timeout-default* 0 + "How long will the program wait for the reply packet (in seconds).") + +(defvar *retries-default* 0 + "How many times should the program try to recieve the whole packet.") + +(defvar *retries-interval-default* 0 + "How long to pause between each retry (in seconds).") + +(defvar *default-answer-default* nil + "The default answer in case of an error.") + +(defconstant +zzz+ 0.5 + "Pause time between each command or sending and receving packets") + +(defmacro with-gensyms (vars &body body) + `(let ,(mapcar #'(lambda (v) `(,v (gensym))) vars) + , at body)) Added: nxtlisp/nxtlisp.asd ============================================================================== --- (empty file) +++ nxtlisp/nxtlisp.asd Wed Aug 15 17:40:32 2007 @@ -0,0 +1,61 @@ +;;;; -*- Mode: Lisp; Syntax: ANSI-Common-Lisp; Base: 10 -*- +;;;; +;;;; File: nxtlisp.asd +;;;; +;;;; License: Apache License 2.0 +;;;; +;;;; Contributors: Milan Cermak, milan.cermak at gmail.com +;;;; +;;;; Description: NXTLisp's ASDF definition file. +;;;; +;;;; + + +#| + + Copyright 2007 Milan Cermak + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +|# + + +(in-package :cl-user) + +(defpackage :nxtlisp + (:use :cl :asdf :cffi) + (:nicknames :nxt)) + +(in-package :nxt) + +;; file order: +;; nxt.lisp +;; communication.lisp +;; protocol-commands.lisp +;; remote-commands.lisp + + +(defsystem nxtlisp + :name "NXTLisp" + :author "Milan Cermak " + :version "0.0.1" + :license "Apache License 2.0" + :long-description "Library for programming Lego Mindstorms NXT" + :depends-on (:cffi) + :serial t + :components ((:file "nxt") + (:file "communication") + (:file "protocol-commands") + (:file "remote-commands"))) + +;; EOF Added: nxtlisp/protocol-commands.lisp ============================================================================== --- (empty file) +++ nxtlisp/protocol-commands.lisp Wed Aug 15 17:40:32 2007 @@ -0,0 +1,1069 @@ +;;;; +;;;; +;;;; File: protocol-commands.lisp +;;;; +;;;; License: Apache License 2.0 +;;;; +;;;; Contributors: Milan Cermak, milan.cermak at gmail.com +;;;; +;;;; Description: All commands specified in the NXT Communications Protocol, +;;;; NXT Direct Commands and related code. +;;;; +;;;; + + +#| + + Copyright 2007 Milan Cermak + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +|# + + + + +(in-package :nxt) + + +;;; +;;; Communications protocol (normal commands) +;;; + +(defun open-read-command (filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command OPEN READ COMMANND. Argument description: +filename - an ASCIIZ [15.3 chars] + null termination string + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "open-read-command: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-system-packet 24 reply #X80))) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 10 blocking timeout retries retries-interval) + :raw))))))) + +(defun open-write-command (filename filesize &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command OPEN WRITE COMMAND. Argument description: +filename - an ASCIIZ [15.3 chars] + null termination string +filesize - file size; max 2^32-1 bytes + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "open-write-command: filename is not an array/string: ~A" filename) + (assert (and (numberp filesize) (<= 0 filesize 4294967295)) (filesize) + "open-write-command: filesize is not a number or has an invalid value [0 - 4294967295]: ~A" filesize) + (let ((msg (make-initial-system-packet 28 reply #X81))) + (push-string filename 20 msg) + (push-4bytes filesize msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval) + :ubyte))))))) + +(defun read-command (handle bytes-to-read &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command READ COMMAND. Argumets description: +handle - the handle number +bytes-to-read - number of data to read (in bytes); max 65535 + +Reply should not be disabled for this command." + (assert (numberp handle) (handle) + "read-command: handle is not a number: ~A" handle) + (assert (and (numberp bytes-to-read) (<= 0 bytes-to-read 65535)) (bytes-to-read) + "read-command: bytes-to-read is not a number or has an invalid value [0 - 65535]: ~A" bytes-to-read) + (let ((msg (make-initial-system-packet 7 reply #X82))) + (vector-push handle msg) + (push-2bytes bytes-to-read msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream (+ bytes-to-read 8) blocking timeout + retries retries-interval) + :raw))))))) + +(defun write-command (handle data &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command WRITE COMMAND. Arguments description: +handle - the handle number +data - array of data to be written into FLASH" + (assert (numberp handle) (handle) + "write-command: handle is not a number: ~A" handle) + (assert (arrayp data) (data) + "write-command: data is not an array: ~A" data) + (let ((msg (make-initial-system-packet (+ (length data) 5) reply #X83))) + (vector-push handle msg) + (dotimes (c (length data)) + (vector-push (aref data c) msg)) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 8 blocking timeout retries retries-interval) + :raw))))))) + +(defun close-command (handle &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command CLOSE COMMAND. Argument description: +handle - the handle number." + (assert (numberp handle) (handle) + "close-command: handle is not a number: ~A" handle) + (let ((msg (make-initial-system-packet 5 reply #X84))) + (vector-push handle msg) + (create-packet) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval) + :ubyte))))))) + +(defun delete-command (filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command DELETE COMMAND. Argument description: +filename - an ASCIIZ [15.3 chars] null terminated string" + (assert (arrayp filename) (filename) + "delete-command: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-system-packet 24 reply #X85))) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 25 blocking timeout retries retries-interval) + :filename))))))) + +(defun find-first (filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command FIND FIRST. Argument description: +filename - an ASCIIZ [15.3 chars] null terminated string with extensions (see Wildcard in +Lego Communication protocol) + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "find-first: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-system-packet 24 reply #X86))) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 30 blocking timeout retries retries-interval) + :raw))))))) + +(defun find-next (handle &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command FIND NEXT. Argument description: +handle - the handle number from a previous find file or from find-first command + +Reply should not be disabled for this command." + (assert (numberp handle) (handle) + "find-next: handle is not a number: ~A" handle) + (let ((msg (make-initial-system-packet 5 reply #X87))) + (vector-push handle msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 30 blocking timeout retries retries-interval) + :raw))))))) + +(defun get-firmware-version (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command GET FIRMWARE VERSION. + +Reply should not be disabled for this command." + (let ((msg (make-initial-system-packet 4 reply #X88))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 9 blocking timeout retries retries-interval) + :raw))))))) + +(defun open-write-linear-command (filename filesize &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command OPEN WRITE LINEAR COMMAND. Argument description: +filename - an ASCIIZ [15.3 chars] null terminated string +filesize - file size; max 2^32-1 bytes + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "open-write-linear-command: filename is not an array/string: ~A" filename) + (assert (and (numberp filesize) (<= 0 filesize 4294967295)) (filesize) + "open-write-linear-command: filesize is not a number or has an invalid value [0 - 4294967295]: ~A" filesize) + (let ((msg (make-initial-system-packet 28 reply #X89))) + (push-string filename 20 msg) + (push-4bytes filesize msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval) + :ubyte))))))) + +(defun open-read-linear-command (filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command OPEN READ LINEAR COMMAND (internal command). Argument description: +filename - an ASCIIZ [15.3 chars] null terminated string + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "open-read-linear-command: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-system-packet 24 reply #X8A))) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 9 blocking timeout retries retries-interval) + :ulong))))))) + +(defun open-write-data-command (filename filesize &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command OPEN WRITE DATA COMMAND. Argument description: +filename - an ASCIIZ [15.3 chars] null terminated string +filesize - file size; max 2^32-1 bytes + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "open-write-data-command: filename is not an array/string: ~A" filename) + (assert (and (numberp filesize) (<= 0 filesize 4294967295)) (filesize) + "open-write-data-command: filesize is not a number or has an invalid value [0 - 4294967295]: ~A" filesize) + (let ((msg (make-initial-system-packet 28 reply #X8B))) + (push-string filename 20 msg) + (push-4bytes filesize msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval) + :ubyte))))))) + +(defun open-append-data-command (filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command OPEN APPEND DATA COMMAND. Argument description: +filename - an ASCIIZ [15.3 chars] null terminated string + +Reply should not be disabled for this command." + (assert (arrayp filename) (filename) + "open-append-data-command: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-system-packet 24 reply #X8C))) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 10 blocking timeout retries retries-interval) + :raw))))))) + +(defun set-brick-name-command (name &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command SET BRICK NAME COMMAND. Argument description: +name - max 15 character string" + (assert (and (arrayp name) (<= (length name) 15)) (name) + "set-brick-name-command: name is not an array/string or too long [max 15]: ~A" name) + (let ((msg (make-initial-system-packet 20 reply #X98))) + (push-string name 15 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun get-device-info (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command GET DEVICE INFO. + +Reply should not be disabled for this command." + (let ((msg (make-initial-system-packet 4 reply #X9B))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 35 blocking timeout retries retries-interval) + :raw))))))) + +(defun delete-user-flash (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command DELETE USER FLASH. + +Reply should not be disabled for this command." + (let ((msg (make-initial-system-packet 4 reply #XA0))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun poll-command-length (buffer &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command POLL COMMAND LENGTH. Argument description: +buffer - can take two discreet values: + #X00 - poll buffer + #X01 - high speed buffer + +Reply should not be disabled for this command." + (assert (and (numberp buffer) (or (= buffer #X00) (= buffer #X01))) (buffer) + "poll-command-length: buffer is not a number or has an invalid value [#X00, #X01]: ~A" buffer) + (let ((msg (make-initial-system-packet 5 reply #XA1))) + (vector-push buffer msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + ;; no decode-reply here, third byte in the reply packet doesn't + ;; represent a status/error byte, complain at Lego + (t (recv-packet stream 7 blocking timeout retries retries-interval))))))) + +(defun poll-command (buffer length &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "System command POLL COMMAND. Argument description: +buffer - can take two discreet values: + #X00 - poll buffer + #X01 - high speed buffer + +length - command length + +Reply should not be disabled for this command." + (assert (and (numberp buffer) (or (= buffer #X00) (= buffer #X01))) (buffer) + "poll-command: buffer is not a number or has an invalid value [#X00, #X01]: ~A" buffer) + (assert (numberp length) (length) + "poll-command: length is not a number: ~A" length) + (let ((msg (make-initial-system-packet 6 reply #XA2))) + (vector-push buffer msg) + (vector-push length msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + ;; no decode-reply here, third byte in the reply packet doesn't + ;; represent a status/error byte, complain at Lego + (t (recv-packet stream 67 blocking timeout retries retries-interval))))))) + + +;;; +;;; Direct commands +;;; + +(defun start-program (filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command STARTPROGRAM; filename has to be an ASCIIZ string +with maximum size [15.3] char + null terminator." + (assert (arrayp filename) (filename) + "start-program: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-direct-packet 24 reply #X00))) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg ; debugging packets + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) ; sending failed + ((null reply) answer) ; no reply requested + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun stop-program (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command STOPPROGRAM." + (let ((msg (make-initial-direct-packet 4 reply #X01))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun direct-play-soundfile (loop? filename &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command PLAYSOUNDFILE; loop? has to be a boolean; filename has to be an +ASCIIZ string with maximum size [15.3] char + null terminator." + (assert (arrayp filename) (filename) + "play-soundfile: filename is not an array/string: ~A" filename) + (let ((msg (make-initial-direct-packet 25 reply #X02))) + (vector-push (if loop? #X01 #X00) msg) + (push-string filename 20 msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun direct-play-tone (frequency duration &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command PLAYTONE. Frequency (hz) +has to be in range 200 - 14000; duration (ms) isn't specified (probably max +65535)." + (assert (and (numberp frequency) (<= 200 frequency 14000)) (frequency) + "play-tone: frequency is not a number or out of range [200-14000]: ~A" frequency) + (assert (and (numberp duration) (<= 0 duration 65535)) (duration) + "play-tone: duration is not a number or out of range [200-14000]: ~A" duration) + (let ((msg (make-initial-direct-packet 8 reply #X03))) + (push-2bytes frequency msg) + (push-2bytes duration msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun set-output-state (output-port power mode regulation turn-ratio run-state tacho-limit + &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command SETOUTPUTSTATE. +Argument description: + +output-port has to be in range 0 to 2 inclusive or #XFF meaning 'all' + +power has to be in range -100 to 100 inclusive + +mode byte recognises 3 discreet values: + #X01 - turn on the specified motor + #X02 - use run/break instead of run/float in PWM + #X04 - turn on the regulation + +regulation byte recognises 3 discreet values: + #X00 - no regulation will be enabled + #X01 - power control will be enabled on specified output + #X02 - synchronisation will be enabled (needs enabled on two outputs) + +turn-ration has to be in range -100 to 100 inclusive + +run-state byte recognises 4 discreet values: + #X00 - output will be idle + #X10 - output will ramp-up + #X20 - output will be running + #X40 - output will ramp-down + +tacho-limit has to be in range 0 - 2^32-1 inclusive; 0 is a special value - run forever" + + (assert (and (numberp output-port) (or (<= 0 output-port 2) (= output-port #XFF))) + (output-port) + "set-output-state: output-port is not a number or has an invalid value [0-2; #XFF]: ~A" output-port) + (assert (and (numberp power) (<= -100 power 100)) + (power) + "set-output-state: power is not a number or has an invalid value [-100 - 100]: ~A" power) + (assert (and (numberp mode) (or (= mode #X01) (= mode #X02) (= mode #X04))) + (mode) + "set-output-state: mode is not a number or has an invalid value [#X01, #X02, #X04]: ~A" mode) + (assert (and (numberp regulation) (or (= regulation #X00) (= regulation #X01) (= regulation #X02))) + (regulation) + "set-output-state: regulation is not a number or has an invalid value [#X00, #X01, #X02]: ~A" regulation) + (assert (and (numberp turn-ratio) (<= -100 turn-ratio 100)) + (turn-ratio) + "set-output-state: turn-ration is not a number or has an invalid value [-100 - 100]: ~A" turn-ratio) + (assert (and (numberp run-state) (or (= run-state #X00) (= run-state #X10) (= run-state #X20) (= run-state #X40))) + (run-state) + "set-output-state: run-state is not a number or has an invalid value [#X00, #X10, #X20, #X40]: ~A" run-state) + (assert (and (numberp tacho-limit) (<= 0 tacho-limit 4294967295)) + (tacho-limit) + "set-output-state: tacho-limit is not a number or has an invalid value [0 - 4294967295]: ~A" tacho-limit) + (let ((msg (make-initial-direct-packet 15 reply #X04))) + (vector-push output-port msg) + (vector-push power msg) + (vector-push mode msg) + (vector-push regulation msg) + (vector-push turn-ratio msg) + (push-4bytes tacho-limit msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun set-input-mode (input-port sensor-type sensor-mode &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command SETINPUTMODE. +Argument description: + +input-port - has to be in range 0 to 3 inclusive + +sensor-type recognises following values: + #X00 - no sensor + #X01 - switch + #X02 - temperature + #X03 - reflection + #X04 - angle + #X05 - light active + #X06 - light inactive + #X07 - sound dB + #X08 - sound dBA + #X09 - custom + #X0A - lowspeed + #X0B - lowspeed 9V + #X0C - no of sensor types + +sensor-mode recognises following values: + #X00 - raw mode + #X20 - boolean mode + #X40 - transition cnt mode + #X60 - period counter mode + #X80 - pct full scale mode + #XA0 - celsius mode + #XC0 - fahrenheit mode + #XE0 - angle step mode + #X1F - slope mask + #XE0 - mode mask" + (assert (and (numberp input-port) (<= 0 input-port 3)) (input-port) + "set-input-mode: input-port is not a number or has an invalid value [0 - 3]: ~A" input-port) + (assert (and (numberp sensor-type) (or (= sensor-type #X00) (= sensor-type #X01) + (= sensor-type #X02) (= sensor-type #X03) + (= sensor-type #X04) (= sensor-type #X05) + (= sensor-type #X06) (= sensor-type #X07) + (= sensor-type #X08) (= sensor-type #X09) + (= sensor-type #X0A) (= sensor-type #X0B) + (= sensor-type #X0C))) + (sensor-type) + "set-input-mode: sensor-type is not a number or has an invalid value [#X00 - #X0C]: ~A" sensor-type) + (assert (and (numberp sensor-mode) (or (= sensor-mode #X00) (= sensor-mode #X20) + (= sensor-mode #X40) (= sensor-mode #X60) + (= sensor-mode #X80) (= sensor-mode #XA0) + (= sensor-mode #XC0) (= sensor-mode #XE0) + (= sensor-mode #X1F) (= sensor-mode #XE0))) + (sensor-mode) + "set-input-mode: sensor-mode is not a number or has an invalid value: ~A" sensor-mode) + (let ((msg (make-initial-direct-packet 7 reply #X05))) + (vector-push input-port msg) + (vector-push sensor-type msg) + (vector-push sensor-mode msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun get-output-state (output-port &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command GETOUTPUTSTATE. Argument description: +output-port - has to be in range 0 to 2 inclusive + +Function returns a list of bytes. First element of the list is the output port byte (Byte 3 in +Lego Direct commands protocol.) It is up to the user to handle the values correctly. For +additional information about byte values, see Lego Direct commands protocol." + (assert (and (numberp output-port) (<= 0 output-port 2)) (output-port) + "get-output-state: output-port is not a number or has an invalid value [0 - 2]: ~A" output-port) + (let ((msg (make-initial-direct-packet 5 reply #X06))) + (vector-push output-port msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 27 blocking timeout retries retries-interval) + :raw))))))) + +(defun get-input-values (input-port &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command GETINPUTVALUES. Argument description: +input-port - haf to be in range 0 - 3 inclusive + +Function returns a list of bytes. First element of the list is the input port byte (Byte 3 in +Lego Direct commands protocol.) It is up to the user to handle the values correctly. For +additional information about byte values, see Lego Direct commands protocol." + (assert (and (numberp input-port) (<= 0 input-port 3)) (input-port) + "get-input-values: input-port is not a number or has an invalid value [0 - 3]: ~A" input-port) + (let ((msg (make-initial-direct-packet 5 reply #X07))) + (vector-push input-port msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 18 blocking timeout retries retries-interval) + :raw))))))) + +(defun reset-input-scaled-value (input-port &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command RESETINPUTSCALEDVALUE. Argument description: +input-port - has to be in range 0 to 3 inclusive" + (let ((msg (make-initial-direct-packet 5 reply #X08))) + (vector-push input-port msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun message-write (inbox msg-len msg-data &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command MESSAGEWRITE. Arguments description: +inbox - the inbox number; has to be in range 0 to 9 +msg-len - message length including the terminating null; message can't be longer than 59 bytes +msg-data - message (of length msg-len); treated as a string, must be null terminated" + (assert (and (numberp inbox) (<= 0 inbox 9)) (inbox) + "message-write: inbox is not a nubmer or has an invalid value [0 - 9]: ~A" inbox) + (assert (and (numberp msg-len) (<= 0 msg-len 59)) (msg-len) + "message-write: msg-len is not a number or has an invalid value [0 - 59]: ~A" msg-len) + (assert (arrayp msg-data) (msg-data) + "message-write: msg-data is not a array/string") + (let ((msg (make-initial-direct-packet (+ msg-len 6) reply #X09))) + (vector-push inbox msg) + (vector-push msg-len msg) + (loop for i from 0 to (1- msg-len) doing (vector-push (aref msg-data i) msg)) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun reset-motor-position (output-port relative? &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command RESETMOTORPOSITION. Arguments description: +output-port - has to be in range 0 to 2 inclusive +relative? - boolean; TRUE: position relative to last movement, FALSE: absolute position" + (assert (and (numberp output-port) (<= 0 output-port 2)) (output-port) + "reset-motor-position: output-port is not a number or has an invalid value [0 - 2]: ~A" output-port) + (let ((msg (make-initial-direct-packet 6 reply #X0A))) + (vector-push output-port msg) + (vector-push (if relative? #X01 #X00) msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun get-battery-level (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command GETBATTERYLEVEL." + (let ((msg (make-initial-direct-packet 4 reply #X0B))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 7 blocking timeout retries retries-interval) + :uword))))))) + + +(defun stop-sound-playback (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command STOPSOUNDPLAYBACK." + (let ((msg (make-initial-direct-packet 4 reply #X0C))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun keep-alive (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command KEEPALIVE." + (let ((msg (make-initial-direct-packet 4 reply #X0D))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 9 blocking timeout retries retries-interval) + :ulong))))))) + +(defun ls-get-status (port &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command LSGETSTATUS. Argument description: +port - has to be in range 0 to 3 inclusive" + (assert (and (numberp port) (<= 0 port 3)) (port) + "ls-get-status: port is not a number or out of range [0 - 3]: ~A" port) + (let ((msg (make-initial-direct-packet 5 reply #X0E))) + (vector-push port msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval) + :ubyte))))))) + +(defun ls-write (port tx-len rx-len data &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command LSWRITE. Arguments description: +port - has to be in range 0 to 3 inclusive +tx-len - tx data length (in bytes) +rx-len - rx data length (in bytes) +data - data; length is limited to 16 bytes per command" + (assert (and (numberp port) (<= 0 port 3)) (port) + "ls-write: port is not a number or has an invalid value [0 - 2]: ~A" port) + (assert (numberp tx-len) (tx-len) ; not sure about the limit so I don't test it + "ls-write: tx-len is not a number: ~A" tx-len) + (assert (numberp rx-len) (rx-len) + "ls-write: rx-len is not a number: ~A" rx-len) + (assert (arrayp data) (data) + "ls-write: data has to be an array") + (let ((msg (make-initial-direct-packet (+ tx-len 7) reply #X0F))) + (vector-push port msg) + (vector-push tx-len msg) + (vector-push rx-len msg) + (dotimes (i tx-len) + (vector-push (aref data i) msg)) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval) + :simple))))))) + +(defun ls-read (port &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command LSREAD. Argument description: +port - has to be in range 0 to 3 inclusive" + (assert (and (numberp port) (<= 0 port 3)) (port) + "ls-read: port is not a number or has an invalid value [0 - 3]: ~A" port) + (let ((msg (make-initial-direct-packet 5 reply #X0F))) + (vector-push port msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 22 blocking timeout retries retries-interval) + :raw))))))) + +(defun get-current-program-name (&key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command GETCURRENTPROGRAMNAME." + (let ((msg (make-initial-direct-packet 4 reply #X11))) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 25 blocking timeout retries retries-interval) + :filename))))))) + +(defun message-read (remote-inbox local-inbox remove? &key + (stream *standard-nxt-io*) (blocking *blocking-default*) + (reply *reply-default*) (timeout *timeout-default*) + (retries *retries-default*) (retries-interval *retries-interval-default*) + (default-answer *default-answer-default*)) + "Direct command MESSAGEREAD. Arguments description: +remote-inbox - has to be in range 0 to 19 inclusive +local-inbox - has to be in range 0 to 9 inclusive +remove - boolean; TRUE clears message from remote-inbox + +For additional information and semantics see the Lego Direct Commands protocol." + (assert (and (numberp remote-inbox) (<= 0 remote-inbox 19)) (remote-inbox) + "message-read: remote-inbox is not a number or has an invalid value [0 - 19]: ~A" remote-inbox) + (assert (and (numberp local-inbox) (<= 0 local-inbox 9)) (local-inbox) + "message-read: local-inbox is not a number or has an invalid value [0 - 9]: ~A" local-inbox) + (let ((msg (make-initial-direct-packet 7 reply #X13))) + (vector-push remote-inbox msg) + (vector-push local-inbox msg) + (vector-push (if remove? #X01 #X00) msg) + (create-packet msg) + (if (eql stream 0) + msg + (let ((answer (send-packet stream msg blocking timeout retries retries-interval))) + (cond + ((< answer 0) default-answer) + ((null reply) answer) + (t (decode-reply (recv-packet stream 66 blocking timeout retries retries-interval) + :raw))))))) + +;;; +;;; various helper functions +;;; + + +(defun push-string (string maxlength vector) + "Appends string to the tail of the vector. Lenght has to be the maximal possible +length of the string without null terminator. Empty slots are filled with #\0." + (dotimes (c (length string)) + (vector-push (char-code (aref string c)) vector)) + (dotimes (foo (- maxlength (length string))) ; zero padding + (vector-push #X00 vector)) + (vector-push #X00 vector)) ; null termination + +(defun push-2bytes (number vector) + (multiple-value-bind (lsb msb) (dec-to-2bytes number) + (vector-push lsb vector) + (vector-push msb vector))) + +(defun push-4bytes (number vector) + (multiple-value-bind (one two three four) (dec-to-4bytes number) + (vector-push one vector) + (vector-push two vector) + (vector-push three vector) + (vector-push four vector))) + +(defun dec-to-2bytes (number) + "Converts a decimal number (max 65535) to its representation in 2 bytes. +Returns multiple values - least significant byte and most significant byte." + (cond + ((< number 256) (values number 0)) + ((eql 0 (rem number 256)) (values 0 (truncate (/ number 256)))) + (t (values (rem number 256) (truncate (/ number 256)))))) + +(defun dec-to-4bytes (number) + "Converts a decimal number (max 4294967295) to its little-endian representation +in 4 bytes. Returns four multiple values (each byte as one)." + (cond + ((< number 256) (values number 0 0 0)) + ((eql 0 (rem number 16777216)) (values 0 0 0 (truncate (/ number 16777216)))) + ((eql 0 (rem number 65536)) (values 0 0 (truncate (/ number 65536)) 0)) + ((eql 0 (rem number 256)) (values 0 (truncate (/ number 256)) 0 0)) + ;; crazy! maybe there's a better way but I'm not in a thinking mood today + (t (let* ((one (rem number 256)) + (two (rem (/ (- number one) 256) 256)) + (three (rem (/ (- (/ (- number one) 256) two) 256) 256)) + (four (truncate (/ (- (/ (- (/ (- number one) 256) two) 256) three) 256)))) + (values one two three four))))) + +(defun make-initial-system-packet (size reply command) + "Creates a system command packet with correct initial contents (reply and command bytes)." + (let ((packet (make-array size :element-type '(unsigned-byte 8) + :initial-element 0 :fill-pointer 2))) + (vector-push (if reply #X01 #X81) packet) + (vector-push command packet) + packet)) + +(defun make-initial-direct-packet (size reply command) + "Creates a direct command packet of correct length and fills in reply and command bytes." + (let ((packet (make-array size :element-type '(unsigned-byte 8) + :initial-element 0 :fill-pointer 2))) + (vector-push (if reply #X00 #X80) packet) + (vector-push command packet) + packet)) + +(defun create-packet (vector) + "Forms a legal BT packet by inserting the length of the message +to the first two bytes of the packet. For additional information, +consult the Lego Communication protocol." + ;; len holds the length of the whole packet which will be transmited + ;; by Bluetooth, that is with 2 header bytes representing packet length + (let ((len (fill-pointer vector))) + (setf (fill-pointer vector) 0) + (push-2bytes (- len 2) vector) ; push header bytes + (setf (fill-pointer vector) len))) \ No newline at end of file Added: nxtlisp/remote-commands.lisp ============================================================================== --- (empty file) +++ nxtlisp/remote-commands.lisp Wed Aug 15 17:40:32 2007 @@ -0,0 +1,165 @@ +;;;; +;;;; +;;;; File: remote-commands.lisp +;;;; +;;;; License: Apache License 2.0 +;;;; +;;;; Contributors: Milan Cermak, milan.cermak at gmail.com +;;;; +;;;; Description: Commands for the "remote" NXTLisp. Inspired by the RCXLisp; +;;;; I tried to perserve backward compatibility. +;;;; +;;;; + + +#| + + Copyright 2007 Milan Cermak + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +|# + +;; TODO: add asserts to set-sensor-state (and possibly other functions); asserts are +;; now in protocol-commands.lisp, but functions here call them, so correct input +;; is provided, but users will (mostly) call remote functions -> asserts needed! +;; +;; check how are strings (filenames, IDs, ...) handled, if an argument has to be +;; an array of unsigned-bytes or a simple string will suffice +;; +;; what values should sensor return? according to the Lego specs, GETINPUTVALUES +;; returns a lot. I think, there should be a way for the user to select what return +;; value he wants. + +(in-package :nxt) + +;; notes: +;; +;; skipped: all clock related functions, *-rcx-thread, *current-program, *timer +;; +;; done: play-tone (a direct command) +;; play-system-sound-file (play-system-file in rcxlisp; a direct command wrapper) +;; battery-power (simplified get-battery-level direct command) +;; start-nxt-program (a wrapper around start-program direct command) +;; shutdown (a wrapper around stop-program direct command) +;; alivep (internaly uses get-battery-level direct command) +;; firmware (returns firmware and protocol versions) +;; change-nxt-id (a wrapper around set-brick-name-command) +;; set-sensor-state (uses set-input-mode direct command and +;; reset-input-scaled-value if clear flag is set) +;; sensor - done just partly, consult next steps with Dr.K + + + +;; differences between RCX and NXT: +;; play-system-sound-file plays a sound file in NXT's memory, possibly in a loop +;; start-nxt-program takes and string as an program name; rcx requires a number +;; shutdown does not take :infinity as a valid delay value +;; alivep uses a direct command (get-battery-level) as a helper to ping the NXT +;; firmware has no keystring argument; returns firmware and protocol major and minor versions +;; change-nxt-id takes a string as argument (not a number) +;; set-sensor-state has no slope argument + + +(EXPORT '(var set-var message send-message sensor effector set-effector-state + read-rcx-executable)) + +(defun play-tone (frequency duration &optional (stream *standard-nxt-io*)) + "Plays a tone on the NXT. Frequency is in Hz (200-14000), duration in ms." + (direct-play-tone frequency duration :stream stream)) + +(defun play-system-sound-file (loop? filename &optional stream) + "Plays a soundfile on the NXT. If loop? if t, soundfile is played in a loop." + (direct-play-soundfile (loop? filename :stream stream))) + +(defun battery-power (&optional (stream *standard-nxt-io*)) + "Return NXT's current battery voltage in mV." + (get-battery-level :stream stream)) + +(defun start-nxt-program (program &optional (stream *standard-nxt-io*)) + "Starts a program on the NXT brick. Argument must be a [15.3] filename with null +termination." + (start-program (program :stream stream))) + +(defun shutdown (&optional (delay 0) (stream *standard-nxt-io*)) + "Tells the NXT to stop execution of the current program after minutes." + (sleep (* 60 delay)) + (stop-program :stream stream)) + +(defun alivep (&optional (stream *standard-nxt-io*)) + "Pings the NXT to see if a reply comes back." + ;; get-battery-level is just a random function, in fact any could be used (altough + ;; this one makes the best sense); it is sent to the brick to see if a reply comes back + (get-battery-level :stream stream :reply t)) + +(defun firmware (&optional (stream *standard-nxt-io*)) + "Returns four distinct values: major and minor firmware number and +major and minor protocol version." + (let ((raw-reply (get-firmware-version :stream stream))) + (values (aref 8 raw-reply) ; major firmware version + (aref 7 raw-reply) ; minor firmware version + (aref 6 raw-reply) ; major protocol version + (aref 5 raw-reply)))); minor protocol version + +(defun change-nxt-id (id &optional (stream *standard-nxt-io*)) + "Changes the ID (name) of the NXT brick. The change is immediately visible +on the brick. ID has to be a string of max 15 characters." + (set-brick-name-command (id :stream stream))) + +(defun set-sensor-state (port &key (type :no-sensor)(mode :raw) + (clear nil) (stream *standard-nxt-io*)) + "Configures a sensor attached to port . For description of valid values +for and consult the NXTLisp manual or SETINPUTMODE direct command +specified in Lego Direct Commands communication protocol." + (let ((sensor-type (case type + (:no-sensor #x00) + (:switch #x01) + (:temperature #x02) + (:reflection #x03) + (:angle #x04) + (:light-active #x05) + (:light-inactive #x06) + (:sound-db #x07) + (:sound-dba #x08) + (:custom #x09) + (:lowspeed #x0A) + (:lowspeed-9V 0x0B) + (:no-of-sensor-types #x0C))) + (sensor-mode (case mode + (:raw #x00) + (:boolean #x20) + (:transition-cnt #x40) + (:period-counter #x60) + (:pct-full-scale #x80) + (:celsius #xA0) + (:fahrenheit #xC0) + (:angle-steps #xE0) + (:slope-mask #x1F) + (:mode-mask #xE0)))) ; has the same value as :angle-steps in docs (?) + (set-input-mode port sensor-type sensor-mode :stream stream) + (if clear (reset-input-scaled-value port)))) ; if the clear flag is set, reset the counter + +(defun sensor (port &optional (stream *standard-nxt-io*)) + "Retrieves values from the sensor plugged in port." + ;; TODO: handle the return value (consult with Dr. K) + (get-input-values port)) + +(defun effector (effector feature &optional (stream *standard-nxt-io*)) + "Returns the current value of for ." + ;; there probably will be a problem with backward compatibility + ;; I'm not sure what can be done with get-output-state direct command + ) + +(defun set-effector-state (effector feature value &optional (stream *standard-nxt-io*)) + "TODO: document") + From mcermak at common-lisp.net Mon Aug 20 20:05:17 2007 From: mcermak at common-lisp.net (mcermak at common-lisp.net) Date: Mon, 20 Aug 2007 16:05:17 -0400 (EDT) Subject: [nxtlisp-cvs] r2 - nxtlisp Message-ID: <20070820200517.6A249601B0@common-lisp.net> Author: mcermak Date: Mon Aug 20 16:05:17 2007 New Revision: 2 Modified: nxtlisp/remote-commands.lisp Log: set-sensor-state and sensor remote commands done but not tested yet Modified: nxtlisp/remote-commands.lisp ============================================================================== --- nxtlisp/remote-commands.lisp (original) +++ nxtlisp/remote-commands.lisp Mon Aug 20 16:05:17 2007 @@ -57,10 +57,10 @@ ;; change-nxt-id (a wrapper around set-brick-name-command) ;; set-sensor-state (uses set-input-mode direct command and ;; reset-input-scaled-value if clear flag is set) -;; sensor - done just partly, consult next steps with Dr.K +;; sensor (type and mode are possible &optional arguments, maybe not the +;; best solution) - ;; differences between RCX and NXT: ;; play-system-sound-file plays a sound file in NXT's memory, possibly in a loop ;; start-nxt-program takes and string as an program name; rcx requires a number @@ -74,6 +74,53 @@ (EXPORT '(var set-var message send-message sensor effector set-effector-state read-rcx-executable)) +(defvar *last-sensor-type* nil + "If sensor is called with no modifier arguments, *last-sensor-type* is checked +to obtain sensor type. It also maintains backward compatibility with RCXLisp.") + +(defvar *last-sensor-mode* nil + "If sensor is called with no modifier arguments, *last-sensor-mode* is checked +to obtain sensor mode. It also maintains backward compatibility with RCXLisp.") + +(defun update-last-sensor-type (port sensor-type) + "Updates the *last-sensor-type* variable according to last call of set-sensor-state." + (if (or (eql *last-sensor-type* nil) ; port has not been used yet (not in list) + (not (let ((used nil)) ; there's gotta be a better way... + (dolist (i *last-sensor-type*) + (if (eql (car i) port) + (setf used t))) + used))) + (push (list port sensor-type) *last-sensor-type*) + (loop for element in *last-sensor-type* ; port has been used, update + when (eql (car element) port) + return (setf (cdr element) sensor-type)))) + +(defun update-last-sensor-mode (port sensor-mode) + "Updates the *last-sensor-mode* variable according to last call of set-sensor-state." + (if (or (eql *last-sensor-mode* nil) ; port has not been used yet (not in list) + (not (let ((used nil)) + (dolist (i *last-sensor-mode*) + (if (eql (car i) port) + (setf used t))) + used))) + (push (list port sensor-mode) *last-sensor-mode*) + (loop for element in *last-sensor-mode* ; port has been used, update + when (eql (car element) port) + return (setf (cdr element) sensor-mode)))) + +(defun extract-type (port) + "Returns the last set type of sensor on port." + (loop for element in *last-sensor-type* + when (eql (car element) port) + return (cdr element))) + +(defun extract-mode (port) + "Returns the last set mode of sensor on port." + (loop for element in *last-sensor-mode* + when (eql (car element) port) + return (cdr element))) + + (defun play-tone (frequency duration &optional (stream *standard-nxt-io*)) "Plays a tone on the NXT. Frequency is in Hz (200-14000), duration in ms." (direct-play-tone frequency duration :stream stream)) @@ -147,12 +194,45 @@ (:slope-mask #x1F) (:mode-mask #xE0)))) ; has the same value as :angle-steps in docs (?) (set-input-mode port sensor-type sensor-mode :stream stream) - (if clear (reset-input-scaled-value port)))) ; if the clear flag is set, reset the counter + (if clear (reset-input-scaled-value port)) ; if the clear flag is set, reset the counter + ;; see the manual or documentation in the source code for description + (update-last-sensor-type port sensor-type) + (update-last-sensor-mode port sensor-mode))) + -(defun sensor (port &optional (stream *standard-nxt-io*)) +;; TODO: should the type/mode handling be this way? rethink +(defun sensor (port &optional (type :raw type-supplied-p) + (mode :raw mode-supplied-p) (stream *standard-nxt-io*)) "Retrieves values from the sensor plugged in port." - ;; TODO: handle the return value (consult with Dr. K) - (get-input-values port)) + (let ((raw-packet (get-input-values port)) + ;; if type or mode was not supplied, use values set + ;; in last call to set-sensor-state + (sensor-type (if type-supplied-p + type + (extract-type port))) + (sensor-mode (if mode-supplied-p + mode + (extract-mode port)))) + ;; let's decode the packet according to type/mode + (if (not (aref raw-packet 1)) + (warn "sensor: Packet marked as not valid.")) + ;; sensor-type has higher priority to sensor-mode + (case sensor-type + (:angle (extract-number (aref raw-packet 9) + (aref raw-packet 10) + :sword)) + ((or :light-active + :light-inactive) (extract-number (aref raw-packet 7) + (aref raw-packet 8) + :uword)) + (t (case sensor-mode + ((or :boolean :transition-cnt :period-counter + :celsius :fahrenheit) (extract-number (aref raw-packet 9) + (aref raw-packet 10) + :sword)) + (t (extract-number (aref raw-packet 5) + (aref raw-packet 6) + :uword))))))) (defun effector (effector feature &optional (stream *standard-nxt-io*)) "Returns the current value of for ." @@ -163,3 +243,10 @@ (defun set-effector-state (effector feature value &optional (stream *standard-nxt-io*)) "TODO: document") +(defun extract-number (byte-1 byte-2 mode) + "Return a number constructed from 2 bytes, either a signed word or unsigned word." + (case mode + (:uword (+ byte-1 (* byte-2 256))) + (:sword (if (< byte-2 127) ; possitive? + (+ byte-1 (* byte-2 256)) + (1+ (lognot (+ byte-1 (* byte-2 256)))))))) \ No newline at end of file From mcermak at common-lisp.net Mon Aug 20 20:44:41 2007 From: mcermak at common-lisp.net (mcermak at common-lisp.net) Date: Mon, 20 Aug 2007 16:44:41 -0400 (EDT) Subject: [nxtlisp-cvs] r3 - nxtlisp Message-ID: <20070820204441.A759B72C2@common-lisp.net> Author: mcermak Date: Mon Aug 20 16:44:41 2007 New Revision: 3 Modified: nxtlisp/remote-commands.lisp Log: sensor function bugfixing and minor update Modified: nxtlisp/remote-commands.lisp ============================================================================== --- nxtlisp/remote-commands.lisp (original) +++ nxtlisp/remote-commands.lisp Mon Aug 20 16:44:41 2007 @@ -201,6 +201,8 @@ ;; TODO: should the type/mode handling be this way? rethink +;; document and add asserts to type and mode arguments +;; if type is temperature (defun sensor (port &optional (type :raw type-supplied-p) (mode :raw mode-supplied-p) (stream *standard-nxt-io*)) "Retrieves values from the sensor plugged in port." @@ -209,10 +211,14 @@ ;; in last call to set-sensor-state (sensor-type (if type-supplied-p type - (extract-type port))) + (if (eql (extract-type port) nil) + :raw + (extract-type port)))) (sensor-mode (if mode-supplied-p mode - (extract-mode port)))) + (if (eql (extract-mode port) nil) + :raw + (extract-type port))))) ;; let's decode the packet according to type/mode (if (not (aref raw-packet 1)) (warn "sensor: Packet marked as not valid.")) @@ -225,6 +231,9 @@ :light-inactive) (extract-number (aref raw-packet 7) (aref raw-packet 8) :uword)) + (:temperature (extract-number (aref raw-packet 9) ; the same as :celsius/:fahrenheit + (aref raw-packet 10) + :sword)) (t (case sensor-mode ((or :boolean :transition-cnt :period-counter :celsius :fahrenheit) (extract-number (aref raw-packet 9)