packages feed

NXT (empty) → 0.1

raw patch · 21 files changed

+2928/−0 lines, 21 filesdep +NXTdep +basedep +bytestringsetup-changedbinary-added

Dependencies added: NXT, base, bytestring, filepath, mtl, time, unix

Files

+ LICENSE view
@@ -0,0 +1,165 @@+                  GNU LESSER GENERAL PUBLIC LICENSE+                       Version 3, 29 June 2007++ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>+ Everyone is permitted to copy and distribute verbatim copies+ of this license document, but changing it is not allowed.+++  This version of the GNU Lesser General Public License incorporates+the terms and conditions of version 3 of the GNU General Public+License, supplemented by the additional permissions listed below.++  0. Additional Definitions. ++  As used herein, "this License" refers to version 3 of the GNU Lesser+General Public License, and the "GNU GPL" refers to version 3 of the GNU+General Public License.++  "The Library" refers to a covered work governed by this License,+other than an Application or a Combined Work as defined below.++  An "Application" is any work that makes use of an interface provided+by the Library, but which is not otherwise based on the Library.+Defining a subclass of a class defined by the Library is deemed a mode+of using an interface provided by the Library.++  A "Combined Work" is a work produced by combining or linking an+Application with the Library.  The particular version of the Library+with which the Combined Work was made is also called the "Linked+Version".++  The "Minimal Corresponding Source" for a Combined Work means the+Corresponding Source for the Combined Work, excluding any source code+for portions of the Combined Work that, considered in isolation, are+based on the Application, and not on the Linked Version.++  The "Corresponding Application Code" for a Combined Work means the+object code and/or source code for the Application, including any data+and utility programs needed for reproducing the Combined Work from the+Application, but excluding the System Libraries of the Combined Work.++  1. Exception to Section 3 of the GNU GPL.++  You may convey a covered work under sections 3 and 4 of this License+without being bound by section 3 of the GNU GPL.++  2. Conveying Modified Versions.++  If you modify a copy of the Library, and, in your modifications, a+facility refers to a function or data to be supplied by an Application+that uses the facility (other than as an argument passed when the+facility is invoked), then you may convey a copy of the modified+version:++   a) under this License, provided that you make a good faith effort to+   ensure that, in the event an Application does not supply the+   function or data, the facility still operates, and performs+   whatever part of its purpose remains meaningful, or++   b) under the GNU GPL, with none of the additional permissions of+   this License applicable to that copy.++  3. Object Code Incorporating Material from Library Header Files.++  The object code form of an Application may incorporate material from+a header file that is part of the Library.  You may convey such object+code under terms of your choice, provided that, if the incorporated+material is not limited to numerical parameters, data structure+layouts and accessors, or small macros, inline functions and templates+(ten or fewer lines in length), you do both of the following:++   a) Give prominent notice with each copy of the object code that the+   Library is used in it and that the Library and its use are+   covered by this License.++   b) Accompany the object code with a copy of the GNU GPL and this license+   document.++  4. Combined Works.++  You may convey a Combined Work under terms of your choice that,+taken together, effectively do not restrict modification of the+portions of the Library contained in the Combined Work and reverse+engineering for debugging such modifications, if you also do each of+the following:++   a) Give prominent notice with each copy of the Combined Work that+   the Library is used in it and that the Library and its use are+   covered by this License.++   b) Accompany the Combined Work with a copy of the GNU GPL and this license+   document.++   c) For a Combined Work that displays copyright notices during+   execution, include the copyright notice for the Library among+   these notices, as well as a reference directing the user to the+   copies of the GNU GPL and this license document.++   d) Do one of the following:++       0) Convey the Minimal Corresponding Source under the terms of this+       License, and the Corresponding Application Code in a form+       suitable for, and under terms that permit, the user to+       recombine or relink the Application with a modified version of+       the Linked Version to produce a modified Combined Work, in the+       manner specified by section 6 of the GNU GPL for conveying+       Corresponding Source.++       1) Use a suitable shared library mechanism for linking with the+       Library.  A suitable mechanism is one that (a) uses at run time+       a copy of the Library already present on the user's computer+       system, and (b) will operate properly with a modified version+       of the Library that is interface-compatible with the Linked+       Version. ++   e) Provide Installation Information, but only if you would otherwise+   be required to provide such information under section 6 of the+   GNU GPL, and only to the extent that such information is+   necessary to install and execute a modified version of the+   Combined Work produced by recombining or relinking the+   Application with a modified version of the Linked Version. (If+   you use option 4d0, the Installation Information must accompany+   the Minimal Corresponding Source and Corresponding Application+   Code. If you use option 4d1, you must provide the Installation+   Information in the manner specified by section 6 of the GNU GPL+   for conveying Corresponding Source.)++  5. Combined Libraries.++  You may place library facilities that are a work based on the+Library side by side in a single library together with other library+facilities that are not Applications and are not covered by this+License, and convey such a combined library under terms of your+choice, if you do both of the following:++   a) Accompany the combined library with a copy of the same work based+   on the Library, uncombined with any other library facilities,+   conveyed under the terms of this License.++   b) Give prominent notice with the combined library that part of it+   is a work based on the Library, and explaining where to find the+   accompanying uncombined form of the same work.++  6. Revised Versions of the GNU Lesser General Public License.++  The Free Software Foundation may publish revised and/or new versions+of the GNU Lesser General Public License from time to time. Such new+versions will be similar in spirit to the present version, but may+differ in detail to address new problems or concerns.++  Each version is given a distinguishing version number. If the+Library as you received it specifies that a certain numbered version+of the GNU Lesser General Public License "or any later version"+applies to it, you have the option of following the terms and+conditions either of that published version or of any later version+published by the Free Software Foundation. If the Library as you+received it does not specify a version number of the GNU Lesser+General Public License, you may choose any version of the GNU Lesser+General Public License ever published by the Free Software Foundation.++  If the Library as you received it specifies that a proxy can decide+whether future versions of the GNU Lesser General Public License shall+apply, that proxy's public statement of acceptance of any version is+permanent authorization for you to choose that version for the+Library.
+ NXT.cabal view
@@ -0,0 +1,73 @@+Name:                NXT+Version:             0.1+Synopsis:            A Haskell interface to Lego Mindstorms NXT+Description:         A Haskell interface to Lego Mindstorms NXT over Bluetoooth. It supports direct commands, messages and+                     many sensors (also unofficial). It has also support for a simple message-based control of a NXT brick+                     via remotely executed program (basic NXC code included).+                     .+                     It contains two simple programs: @nxt-upload@ for uploading files to a NXT brick and @nxt-shutdown@ for+                     remote shutdown of a NXT brick.+                     .+                     It works best on Linux and it also works on Mac OS X.+                     .+                     Feel free to contribute additional features, interfaces for more sensors and propose or write other+                     (example) programs.+License:             LGPL-3+License-file:        LICENSE+Author:              Mitar Milutinovic+Maintainer:          mitar.haskell@tnode.com+Copyright:           (c) 2010 Mitar Milutinovic+Category:            Robotics+Build-type:          Simple+Cabal-version:       >= 1.8+Stability:           experimental+Homepage:            http://mitar.tnode.com+Extra-source-files:  remote/remote.rxe,+                     remote/remote.nxc++Library+  Exposed-modules:     Robotics.NXT,+                       Robotics.NXT.MotorControl,+                       Robotics.NXT.Remote,+                       Robotics.NXT.Sensor.Compass,+                       Robotics.NXT.Sensor.Ultrasonic+  Build-depends:       base >= 4 && < 5,+                       mtl >= 1.1 && < 2,+                       bytestring >= 0.9 && < 1,+                       unix >= 2.4 && < 3,+                       time >= 1.1 && < 2+  Other-modules:       Robotics.NXT.BluetoothUtils,+                       Robotics.NXT.Data,+                       Robotics.NXT.Errors,+                       Robotics.NXT.Protocol,+                       Robotics.NXT.Types,+                       Robotics.NXT.Internals,+                       Robotics.NXT.Externals+  HS-source-dirs:      lib+  C-sources:           ffi/blue.c,+                       ffi/initserial.c+  Includes:            ffi/blue.h,+                       ffi/initserial.h+  GHC-options:         -Wall+  if os(linux)+    Extra-libraries:   bluetooth++Executable nxt-shutdown+  Main-is:             Shutdown.hs+  HS-source-dirs:      src+  Build-depends:       base >= 4 && < 5,+                       mtl >= 1.1 && < 2,+                       NXT == 0.1+  GHC-options:         -Wall++Executable nxt-upload+  Main-is:             UploadFiles.hs+  HS-source-dirs:      src+  Build-depends:       base >= 4 && < 5,+                       mtl >= 1.1 && < 2,+                       bytestring >= 0.9 && < 1,+                       filepath >= 1.1 && < 2,+                       NXT == 0.1+  GHC-options:         -Wall+  GHC-prof-options:    -Wall+  GHC-shared-options:  -Wall
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ ffi/blue.c view
@@ -0,0 +1,165 @@+#include <stdio.h>+#include <stdlib.h>+#include <stdbool.h>+#include <sys/ioctl.h>+#include <sys/socket.h>++// TODO: This works currently only on Linux++#if defined(LINUX) || defined(__linux__)+#include <bluetooth/bluetooth.h>+#include <bluetooth/hci.h>+#include <bluetooth/hci_lib.h>+#endif++#include "blue.h"++// This is not really thread-safe+bool error;++#if defined(LINUX) || defined(__linux__)+int findConn(int s, int dev_id, long arg) {+	struct hci_conn_list_req *cl;+	struct hci_conn_info *ci;+	+	if ((cl = malloc(HCI_MAX_DEV * sizeof(*ci) + sizeof(*cl))) == NULL) {+		perror("malloc");+		error = true;+		return 0;+	}+	cl->dev_id = dev_id;+	cl->conn_num = HCI_MAX_DEV;+	ci = cl->conn_info;++	if (ioctl(s, HCIGETCONNLIST, (void *)cl)) {+		perror("Could not get connection list");+		error = true;+		free(cl);+		return 0;+	}++	int i;+	for (i = 0; i < cl->conn_num; i++, ci++) {+		if (!bacmp((bdaddr_t *)arg, &ci->bdaddr)) {+			free(cl);+			return 1;+		}+	}++	free(cl);+	return 0;+}+#endif++int rssi(char *address) {+#if defined(LINUX) || defined(__linux__)+	struct hci_conn_info_req *cr;+	int8_t rssi;+	int dd, dev_id;+	+	bdaddr_t bdaddr;+	str2ba(address, &bdaddr);++	error = false;+	dev_id = hci_for_each_dev(HCI_UP, findConn, (long)&bdaddr);+	if (dev_id < 0) {+		if (error) {+			return BLUE_ERROR;+		}+		else {+			return BLUE_NOT_CONNECTED;+		}+	}++	dd = hci_open_dev(dev_id);+	if (dd < 0) {+		perror("Could not open HCI device");+		return BLUE_ERROR;+	}++	if ((cr = malloc(sizeof(struct hci_conn_info_req) + sizeof(struct hci_conn_info))) == NULL) {+		perror("malloc");+		hci_close_dev(dd);+		return BLUE_ERROR;+	}++	bacpy(&cr->bdaddr, &bdaddr);+	cr->type = ACL_LINK;+	if (ioctl(dd, HCIGETCONNINFO, (unsigned long)cr) < 0) {+		perror("Could not get connection info");+		free(cr);+		hci_close_dev(dd);+		return BLUE_ERROR;+	}++	if (hci_read_rssi(dd, htobs(cr->conn_info->handle), &rssi, 1000) < 0) {+		perror("Could not read RSSI");+		free(cr);+		hci_close_dev(dd);+		return BLUE_ERROR;+	}++	free(cr);+	hci_close_dev(dd);+	+	return rssi;+#else+    return BLUE_NOT_SUPPORTED;+#endif+}++int lq(char *address) {+#if defined(LINUX) || defined(__linux__)+	struct hci_conn_info_req *cr;+	uint8_t lq;+	int dd, dev_id;++	bdaddr_t bdaddr;+	str2ba(address, &bdaddr);++	dev_id = hci_for_each_dev(HCI_UP, findConn, (long)&bdaddr);+	if (dev_id < 0) {+		if (error) {+			return BLUE_ERROR;+		}+		else {+			return BLUE_NOT_CONNECTED;+		}+	}++	dd = hci_open_dev(dev_id);+	if (dd < 0) {+		perror("Could not open HCI device");+		return BLUE_ERROR;+	}++	if ((cr = malloc(sizeof(struct hci_conn_info_req) + sizeof(struct hci_conn_info))) == NULL) {+		perror("malloc");+		hci_close_dev(dd);+		return BLUE_ERROR;+	}++	bacpy(&cr->bdaddr, &bdaddr);+	cr->type = ACL_LINK;+	if (ioctl(dd, HCIGETCONNINFO, (unsigned long)cr) < 0) {+		perror("Could not get connection info");+		free(cr);+		hci_close_dev(dd);+		return BLUE_ERROR;+	}+	+	if (hci_read_link_quality(dd, htobs(cr->conn_info->handle), &lq, 1000) < 0) {+		perror("Could not read link quality");+		free(cr);+		hci_close_dev(dd);+		return BLUE_ERROR;+	}++	free(cr);+	hci_close_dev(dd);+	+	return lq;+#else+	return BLUE_NOT_SUPPORTED;+#endif+}
+ ffi/initserial.c view
@@ -0,0 +1,29 @@+#include <termios.h>+#include <unistd.h>++#import "initserial.h"++// A simple function which initializes serial port device: 8 bit data, one stop bit, RTS/CTS flow control++int initSerialPort(int fd) {+	struct termios params;+	+	tcflush(fd, TCIOFLUSH);+	+	if (tcgetattr(fd, &params) == -1) return -1;+	+	cfmakeraw(&params);+#ifdef __MAX_BAUD+	cfsetspeed(&params, __MAX_BAUD);+#elif defined B230400+	cfsetspeed(&params, B230400);+#endif+	params.c_cflag = CLOCAL | CREAD | CS8 | HUPCL | CRTSCTS;+	+	//params.c_cc[VTIME] = (5000 + 50) / 100;+    //params.c_cc[VMIN] = 0;+	+	if (tcsetattr(fd, TCSANOW, &params) == -1) return -1;+	+	return 0;+}
+ lib/Robotics/NXT.hs view
@@ -0,0 +1,17 @@+{-|+This module defines an interface over Bluetooth to a NXT brick as defined in Lego Mindstorms NXT Bluetooth Developer Kit,+Appendix 1 - Communication protocol and Appendix 2 - Direct commands. It also defines some additional functions not available+directly otherwise.+-}++module Robotics.NXT (+  module Robotics.NXT.Protocol,+  module Robotics.NXT.BluetoothUtils,+  module Robotics.NXT.Types,+  module Robotics.NXT.Errors+) where++import Robotics.NXT.BluetoothUtils+import Robotics.NXT.Errors+import Robotics.NXT.Protocol+import Robotics.NXT.Types
+ lib/Robotics/NXT/BluetoothUtils.hs view
@@ -0,0 +1,84 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# CFILES ffi/blue.c #-}++module Robotics.NXT.BluetoothUtils (+  -- * Bluetooth utils+  -- | `getDeviceInfo` returns zero for Bluetooth signal strength as this is not implemented in current NXT firmware versions. +  -- Here are functions which retrieve that from a host (computer) Bluetooth stack.+  bluetoothRSSI,+  bluetoothLinkQuality+) where++import Control.Exception+import Control.Monad.State+import Foreign.C.String+import Foreign.C.Types++import Robotics.NXT.Errors+import Robotics.NXT.Protocol+import Robotics.NXT.Types+import Robotics.NXT.Internals++-- Foreign function call for C function which returns RSSI Bluetooth value of a connection to a given Bluetooth address+foreign import ccall unsafe "rssi" rssi :: CString -> IO CInt++-- Foreign function call for C function which returns link quality Bluetooth value of a connection to a given Bluetooth address+foreign import ccall unsafe "lq" lq :: CString -> IO CInt++-- As defined in blue.h+blueError :: Int+blueError = 1000+blueNotConnected :: Int+blueNotConnected = 1001+blueNotSupported :: Int+blueNotSupported = 1002++{-|+Gets received signal strength indicator (RSSI) of the Bluetooth connection to the NXT brick.++Currently supported only on Linux. It throws a 'NXTException' otherwise.+-}+bluetoothRSSI :: NXT Int+bluetoothRSSI = do+  addr <- bluetoothAddress+  bluetoothRSSIAddr addr++bluetoothRSSIAddr :: BTAddress -> NXT Int+bluetoothRSSIAddr addr = do+  ret <- liftIO $ withCString addr rssi+  let ret' = fromIntegral ret+  case ret' of+    _ | ret' == blueError        -> liftIO $ throwIO $ NXTException "Could not get connection's RSSI"+      | ret' == blueNotConnected -> liftIO $ throwIO $ NXTException "Connection not established"+      | ret' == blueNotSupported -> liftIO $ throwIO $ NXTException "Not supported on this system"+      | otherwise                -> return ret'++{-|+Gets link quality of the Bluetooth connection to the NXT brick.++Currently supported only on Linux. It throws a 'NXTException' otherwise.+-}+bluetoothLinkQuality :: NXT Int+bluetoothLinkQuality = do+  addr <- bluetoothAddress+  bluetoothLinkQualityAddr addr++bluetoothLinkQualityAddr :: BTAddress -> NXT Int+bluetoothLinkQualityAddr addr = do+  ret <- liftIO $ withCString addr lq+  let ret' = fromIntegral ret+  case ret' of+    _ | ret' == blueError        -> liftIO $ throwIO $ NXTException "Could not get connection's link quality"+      | ret' == blueNotConnected -> liftIO $ throwIO $ NXTException "Connection not established"+      | ret' == blueNotSupported -> liftIO $ throwIO $ NXTException "Not supported on this system"+      | otherwise                -> return ret'++bluetoothAddress :: NXT BTAddress+bluetoothAddress = do+  addr <- getsNXT address+  case addr of+    Just a  -> return a+    Nothing -> do+      _ <- getDeviceInfo+      (Just a) <- getsNXT address+      return a
+ lib/Robotics/NXT/Data.hs view
@@ -0,0 +1,119 @@+module Robotics.NXT.Data (+  fromUByte,+  fromUWord,+  fromULong,+  fromSByte,+  fromSWord,+  fromSLong,+  dataToString,+  dataToString0,+  toUByte,+  toUWord,+  toULong,+  toSByte,+  toSWord,+  toSLong,+  stringToData,+  stringToData0,+  nameToData,+  messageToData+) where++import qualified Data.ByteString.Lazy as B+import qualified Data.ByteString.Lazy.Char8 as C+import Data.List+import Data.Word+import Control.Exception++-- Converts a list of bytes to an unsigned numeric value+dataToInt :: Integral a => [Word8] -> a -- least significant byte first+dataToInt = foldr addByte 0x00+  where addByte x y = y' * 0x100 + x'+          where x' = fromIntegral x+                y' = fromIntegral y++fromUByte :: Integral a => [Word8] -> a -- one byte, unsigned+fromUByte ws@[_] = dataToInt ws+fromUByte _      = throw $ PatternMatchFail "fromUByte"++fromUWord :: Integral a => [Word8] -> a -- two bytes, unsigned, least significant byte first+fromUWord ws@[_, _] = dataToInt ws+fromUWord _         = throw $ PatternMatchFail "fromUWord"++fromULong :: Integral a => [Word8] -> a -- four bytes, unsigned, least significant byte first+fromULong ws@[_, _, _, _] = dataToInt ws+fromULong _               = throw $ PatternMatchFail "fromULong"++fromSByte :: Integral a => [Word8] -> a -- one byte, signed+fromSByte ws@[b] | b <= 0x7F = dataToInt ws+                 | otherwise = negate . (-) 0x100 . dataToInt $ ws+fromSByte _                  = throw $ PatternMatchFail "fromSByte"++fromSWord :: Integral a => [Word8] -> a -- two bytes, signed, least significant byte first+fromSWord ws@[_, b] | b <= 0x7F = dataToInt ws+                    | otherwise = negate . (-) 0x10000 . dataToInt $ ws+fromSWord _                     = throw $ PatternMatchFail "fromSWord"++fromSLong :: Integral a => [Word8] -> a -- four bytes, signed, least significant byte first+fromSLong ws@[_, _, _, b] | b <= 0x7F = dataToInt ws+                          | otherwise = negate . (-) 0x100000000 . dataToInt $ ws+fromSLong _                           = throw $ PatternMatchFail "fromSLong"++-- Converts a null-terminated list of bytes to a string+dataToString0 :: [Word8] -> String+dataToString0 = dataToString . takeWhile (/= 0x00)++dataToString :: [Word8] -> String+dataToString = C.unpack . B.pack++-- Converts a numeric value to list of bytes+-- In a case of a negative number it produces an infinite list+intToData :: Integral a => a -> [Word8] -- least significant byte first+intToData 0x00 = [0x00]+intToData x    = unfoldr getByte x+  where getByte 0x00 = Nothing+        getByte y    = Just (fromIntegral $ y `mod` 0x100, y `div` 0x100)++toUByte :: Integral a => a -> [Word8] -- one byte, unsigned+toUByte x | x >= 0x00 && x <= 0xFF = intToData x+          | otherwise              = throw . PatternMatchFail $ "toUByte: " ++ show x++toUWord :: Integral a => a -> [Word8] -- two bytes, unsigned, least significant byte first+toUWord x | x >= 0x00 && x <= 0xFFFF = take 2 . flip (++) (repeat 0x00) . intToData $ x+          | otherwise                = throw . PatternMatchFail $ "toUWord: " ++ show x++toULong :: Integral a => a -> [Word8] -- four bytes, unsigned, least significant byte first+toULong x | x' >= 0x00 && x' <= 0xFFFFFFFF = take 4 . flip (++) (repeat 0x00) . intToData $ x'+          | otherwise                      = throw . PatternMatchFail $ "toULong: " ++ show x+  where x' = fromIntegral x :: Integer++toSByte :: Integral a => a -> [Word8] -- one byte, signed+toSByte x | x >= (-0x80) && x < 0x00 = intToData $ 0x100 + x+          | x >= 0x00 && x <= 0x7F   = intToData x+          | otherwise                = throw . PatternMatchFail $ "toSByte: " ++ show x++toSWord :: Integral a => a -> [Word8] -- two bytes, signed, least significant byte first+toSWord x | x >= (-0x8000) && x < 0x00 = take 2 . flip (++) (repeat 0x00) . intToData $ 0x10000 + x+          | x >= 0x00 && x <= 0x7FFF   = take 2 . flip (++) (repeat 0x00) . intToData $ x+          | otherwise                  = throw . PatternMatchFail $ "toSWord: " ++ show x++toSLong :: Integral a => a -> [Word8] -- four bytes, signed, least significant byte first+toSLong x | x' >= (-0x80000000) && x' < 0x00 = take 4 . flip (++) (repeat 0x00) . intToData $ 0x100000000 + x'+          | x' >= 0x00 && x' <= 0x7FFFFFFF   = take 4 . flip (++) (repeat 0x00) . intToData $ x'+          | otherwise                        = throw . PatternMatchFail $ "toSLong: " ++ show x+  where x' = fromIntegral x :: Integer++-- Converts a string to a null-terminated list of bytes+stringToData0 :: String -> [Word8]+stringToData0 = stringToData . flip (++) "\0"++stringToData :: String -> [Word8]+stringToData = B.unpack . C.pack++-- Converts a name to a null-terminated list of bytes+nameToData :: String -> [Word8]+nameToData = stringToData0 . take 19 . flip (++) (repeat '\0')++-- Converts a message to a null-terminated list of bytes+messageToData :: String -> [Word8]+messageToData = stringToData0 . take 58
+ lib/Robotics/NXT/Errors.hs view
@@ -0,0 +1,65 @@+{-# LANGUAGE DeriveDataTypeable #-}+{-# OPTIONS_HADDOCK prune #-}++module Robotics.NXT.Errors (+  -- * Errors+  -- | Possible error codes and their descriptions are described in Lego Mindstorms NXT Bluetooth Developer Kit, Appendix 1 - Communication+  -- protocol and Appendix 2 - Direct commands.+  failNXT,+  failNXT',+  NXTException(..)+) where++import Control.Exception+import Data.Typeable+import Data.Word++failNXT :: String -> Word8 -> IO a+failNXT msg 0x20 = throwIO . NXTException $ msg ++ ": Pending communication transaction in progress"+failNXT msg 0x40 = throwIO . NXTException $ msg ++ ": Specified mailbox queue is empty"+failNXT msg 0x81 = throwIO . NXTException $ msg ++ ": No more handles"+failNXT msg 0x82 = throwIO . NXTException $ msg ++ ": No space"+failNXT msg 0x83 = throwIO . NXTException $ msg ++ ": No more files"+failNXT msg 0x84 = throwIO . NXTException $ msg ++ ": End of file expected"+failNXT msg 0x85 = throwIO . NXTException $ msg ++ ": End of file"+failNXT msg 0x86 = throwIO . NXTException $ msg ++ ": Not a linear file"+failNXT msg 0x87 = throwIO . NXTException $ msg ++ ": File not found"+failNXT msg 0x88 = throwIO . NXTException $ msg ++ ": Handle all ready closed"+failNXT msg 0x89 = throwIO . NXTException $ msg ++ ": No linear space"+failNXT msg 0x8A = throwIO . NXTException $ msg ++ ": Undefined error"+failNXT msg 0x8B = throwIO . NXTException $ msg ++ ": File is busy"+failNXT msg 0x8C = throwIO . NXTException $ msg ++ ": No write buffers"+failNXT msg 0x8D = throwIO . NXTException $ msg ++ ": Append not possible"+failNXT msg 0x8E = throwIO . NXTException $ msg ++ ": File is full"+failNXT msg 0x8F = throwIO . NXTException $ msg ++ ": File exists"+failNXT msg 0x90 = throwIO . NXTException $ msg ++ ": Module not found"+failNXT msg 0x91 = throwIO . NXTException $ msg ++ ": Out of boundary"+failNXT msg 0x92 = throwIO . NXTException $ msg ++ ": Illegal file name"+failNXT msg 0x93 = throwIO . NXTException $ msg ++ ": Illegal handle"+failNXT msg 0xBD = throwIO . NXTException $ msg ++ ": Request failed (i.e. specified file not found)"+failNXT msg 0xBE = throwIO . NXTException $ msg ++ ": Unknown command opcode"+failNXT msg 0xBF = throwIO . NXTException $ msg ++ ": Insane packet"+failNXT msg 0xC0 = throwIO . NXTException $ msg ++ ": Data contains out-of-range values"+failNXT msg 0xDD = throwIO . NXTException $ msg ++ ": Communication bus error"+failNXT msg 0xDE = throwIO . NXTException $ msg ++ ": No free memory in communication buffer"+failNXT msg 0xDF = throwIO . NXTException $ msg ++ ": Specified channel/connection is not valid"+failNXT msg 0xE0 = throwIO . NXTException $ msg ++ ": Specified channel/connection not configured or busy"+failNXT msg 0xEC = throwIO . NXTException $ msg ++ ": No active program"+failNXT msg 0xED = throwIO . NXTException $ msg ++ ": Illegal size specified"+failNXT msg 0xEE = throwIO . NXTException $ msg ++ ": Illegal mailbox queue ID specified"+failNXT msg 0xEF = throwIO . NXTException $ msg ++ ": Attempted to access invalid field of a structure"+failNXT msg 0xF0 = throwIO . NXTException $ msg ++ ": Bad input or output specified"+failNXT msg 0xFB = throwIO . NXTException $ msg ++ ": Insufficient memory available"+failNXT msg 0xFF = throwIO . NXTException $ msg ++ ": Bad arguments"+failNXT msg 0x00 = throwIO . NXTException $ msg -- some guard (restriction) failed?+failNXT msg _    = throwIO . NXTException $ msg -- invalid error code?++failNXT' :: String -> IO a+failNXT' msg = throwIO . NXTException $ msg++{-|+Exception for NXT interface errors. Currently only one exception is defined which takes textual description as an argument.+-}+data (Show a, Typeable a) => NXTException a = NXTException a deriving (Show, Typeable)++instance (Show a, Typeable a) => Exception (NXTException a)
+ lib/Robotics/NXT/Externals.hs view
@@ -0,0 +1,173 @@+module Robotics.NXT.Externals where++import Data.Int+import Data.Ratio+import Data.Time.Clock+import Data.Word++-- Described in Lego Mindstorms NXT Bluetooth Developer Kit:+--  Appendix 1 - Communication protocol+--  Appendix 2 - Direct commands++-- I2C communication with ultrasonics sensor is described in Lego Mindstorms NXT Hardware Developer Kit:+--  Appendix 7 - Ultrasonic sensor I2C communication protocol++-- | The format of version is \"major.minor\". To format it use @'printf' \"%d.%02d\" major minor@.+data Version = Version FirmwareVersion ProtocolVersion deriving (Bounded, Eq, Ord, Read, Show)+data FirmwareVersion = FirmwareVersion Major Minor deriving (Bounded, Eq, Ord, Read, Show)+data ProtocolVersion = ProtocolVersion Major Minor deriving (Bounded, Eq, Ord, Read, Show)+type Major = Int+type Minor = Int++data DeviceInfo = DeviceInfo Name BTAddress BTStrength FlashFree deriving (Eq, Ord, Read, Show)+-- | Name of the NXT brick.+type Name = String+-- | Bluetooth address of the NXT brick in the string format.+type BTAddress = String+-- | Strength of the Bluetooth signal. Not implemented in current NXT firmware versions. Use 'bluetoothRSSI' or 'bluetoothLinkQuality' as an alternative.+type BTStrength = Int64+-- | Free flash space on the NXT brick (in bytes).+type FlashFree = Int64++data OutputState = OutputState OutputPort OutputPower [OutputMode] RegulationMode TurnRatio RunState TachoLimit TachoCount BlockTachoCount RotationCount deriving (Eq, Ord, Read, Show)++data OutputPort =+    A -- ^ Output port (motor) A.+  | B -- ^ Output port (motor) B.+  | C -- ^ Output port (motor) C.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)+-- | Power and direction. In [-100, 100] range.+type OutputPower = Int+data OutputMode =+    MotorOn -- ^ Enables PWM power according to speed.+  | Brake -- ^ Voltage is not allowed to float between PWM pulses, improves accuracy, uses more power.+  | Regulated -- ^ Required in conjunction with output regulation mode setting.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)+data RegulationMode =+    RegulationModeIdle -- ^ Disables regulation.+  | RegulationModeMotorSpeed -- ^ Auto adjust PWM duty cycle if motor is affected by physical load. Really works only if there is+                             -- room for that (not that motor is already running at the maximum power).+  | RegulationModeMotorSync -- ^ Attempts to keep rotation in sync with another motor that has this set. Also involves turn ratio.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)+-- | In regulated synced mode the difference between two motors. In [-100, 100] range.+type TurnRatio = Int+data RunState =+    MotorRunStateIdle -- ^ Disables power to motor.+  | MotorRunStateRampUp -- ^ Ramping to a new speed set-point that is greater than the current speed set-point.+  | MotorRunStateRunning -- ^ Enables power to motor.+  | MotorRunStateRampDown -- ^ Ramping to a new speed set-point that is less than the current speed set-point.+  | MotorRunStateHold -- ^ Hold at the current position.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)++-- | Target tacho limit for a motor movement. 0 means no limit. It is an unsigned value (you select direction of motor movement with+-- sign of 'OutputPower' value).+type TachoLimit = Int64 -- unsigned long+-- | Internal (absolute) tacho counter. Number since the last reset of the motor tacho counter.+type TachoCount = Int64 -- signed long+-- | Block-relative position counter. Current position relative to the last programmed movement.+type BlockTachoCount = Int64 -- signed long+-- | Program-relative position counter. Current position relative to the last reset of the rotation sensor for this motor.+type RotationCount = Int64 -- signed long++data MotorReset =+    AbsolutePosition -- ^ Resets program-relative position counter ('RotationCount').+  | RelativePosition -- ^ Resets block-relative position counter ('BlockTachoCount')+  | InternalPosition -- ^ Resets internal movement counters (also 'TachoCount'), cancels current goal and resets internal error-correction system.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)++data InputPort =+    One -- ^ Input port (sensor) 1.+  | Two -- ^ Input port (sensor) 2.+  | Three -- ^ Input port (sensor) 3.+  | Four -- ^ Input port (sensor) 4.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)++data InputValue = InputValue InputPort Valid Calibrated SensorType SensorMode RawADValue NormalizedADValue ScaledValue CalibratedValue deriving (Bounded, Eq, Ord, Read, Show)++-- | 'True' if new data value should be seen as valid data.+type Valid = Bool+-- | 'True' if calibration file found and used for 'CalibratedValue'.+type Calibrated = Bool+-- | Type of the sensor currently attached to 'InputPort'. 'NoSensor' turns off the sensor.+data SensorType = NoSensor | Switch | Temperature | Reflection | Angle | LightActive | LightInactive | SoundDB | SoundDBA | Custom | Lowspeed | Lowspeed9V | NoOfSensorTypes deriving (Bounded, Enum, Eq, Ord, Read, Show)+data SensorMode =+    RawMode -- ^ Reports scaled value equal to the raw value.+  | BooleanMode -- ^ Reports scaled value as 1 true or 0 false, false if raw value > 55% of total range, true if < 45%.+  | TransitionCntMode -- ^ Reports scaled value as number of transitions between true and false.+  | PeriodCounterMode -- ^ Reports scaled value as number of transitions from false to true, then back to false.+  | PctFullScaleMode -- ^ Reports scaled value as % of full scale reading for a configured sensor type.+  | CelsiusMode -- ^ For reporting temperature in celsius.+  | FahrenheitMode -- ^ For reporting temperature in fahrenheit.+  | AngleStepsMode -- ^ Reports scaled value as count of ticks on RCX-style rotation sensor.+  +  -- two modes unnecessary?+  -- SlopeMask+  -- ModeMask+  deriving (Bounded, Enum, Eq, Ord, Read, Show)+-- | Raw A/D value. Device dependent.+type RawADValue = Int+-- | Normalized A/D value. Type dependent. In [0, 1023] range.+type NormalizedADValue = Int+-- | Scaled value. Mode dependent. In percent.+type ScaledValue = Int+-- | Value scaled according to calibration. Unused in current NXT firmware versions.+type CalibratedValue = Int++-- | Voltage value (in volts).+type Voltage = Ratio Int+-- | Time duration (in seconds).+type Duration = NominalDiffTime++-- | Inbox on the NXT brick into which the host (computer) queues messages for the program running there.+data Inbox = Inbox0 | Inbox1 | Inbox2 | Inbox3 | Inbox4 | Inbox5 | Inbox6 | Inbox7 | Inbox8 | Inbox9 deriving (Bounded, Enum, Eq, Ord, Read, Show)+-- | Outbox on the NXT brick where the program running there queues messages for the host (computer).+-- There is a convention that only 'RemoteInbox10' - 'RemoteInbox19' outboxes are used for this purpose so that lower ones can+-- be used for inter-brick communication. But this convention is not really obeyed in practice.+data RemoteInbox = RemoteInbox0 | RemoteInbox1 | RemoteInbox2 | RemoteInbox3 | RemoteInbox4 | RemoteInbox5 | RemoteInbox6 | RemoteInbox7 | RemoteInbox8 | RemoteInbox9 | RemoteInbox10 | RemoteInbox11 | RemoteInbox12 | RemoteInbox13 | RemoteInbox14 | RemoteInbox15 | RemoteInbox16 | RemoteInbox17 | RemoteInbox18 | RemoteInbox19 deriving (Bounded, Enum, Eq, Ord, Read, Show)++-- | Should the message be remove from the queue once received?+type RemoveMessage = Bool++-- | Loop playback of the sound file?+type LoopPlayback = Bool+-- | Frequency of the played tone (in hertz).+type Frequency = Int++-- | At most 16 data bytes can be read at a time.+type RxDataLength = Int++type TxData = [Word8]+type RxData = [Word8]++-- | Address of the device (sensor) on the I2C bus.+type DeviceAddress = Word8+-- | I2C device command.+type Command = Word8++-- | I2C device measurement value.+type Measurement = Int -- specification is vague whether measurement value is signed or unsigned++-- | Filename of the file on the NXT brick filesystem. In 15.3 format.+type FileName = String+-- | Size of the file on the NXT brick filesystem.+type FileSize = Int -- unsigned long+-- | Handle of the opened file on the NXT brick filesystem.+type FileHandle = Int -- unsigned byte++type FileData = [Word8]++-- | Type of the IO map module information.+data ModuleInfo = ModuleInfo ModuleName ModuleID ModuleSize ModuleIOMapSize deriving (Eq, Ord, Read, Show)+-- | Module name extension is @.mod@. For some functions this can be also a wild card.+type ModuleName = String+type ModuleID = Int64 -- unsigned long+type ModuleSize = Int64 -- unsigned long+type ModuleIOMapSize = Int -- unsigned word++-- | Handle for traversing of modules. Only one module handle can be opened at a time so be careful to close them when not+-- needed anymore.+type ModuleHandle = Int -- unsigned byte++type IOMapOffset = Int -- unsigned word+type IOMapLength = Int -- unsigned word+type IOMapData = [Word8]
+ lib/Robotics/NXT/Internals.hs view
@@ -0,0 +1,43 @@+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++module Robotics.NXT.Internals where++import Control.Monad.State+import Data.Time.Clock.POSIX+import System.IO++import Robotics.NXT.Externals++{-|+Monad which encompasses interface to the NXT brick.+-}+newtype NXT a = NXT (StateT NXTInternals IO a) deriving (Monad, MonadIO, Functor, MonadFix) -- NXT monad++{-|+A token used for exposed internal functions.+-}+data NXTInternals = NXTInternals {+    nxthandle :: Handle, -- a handle of the opened serial port+    address :: Maybe BTAddress,+    modules :: [(ModuleName, ModuleInfo)], -- modules info+    sleeptime :: Maybe Duration, -- sleep time limit in seconds+    lastkeepalive :: Maybe POSIXTime -- last time keep alive has been sent+  }++{-|+Runs a computation in a context of a given 'NXTInternals' token, returning a value and a new token.+-}+runNXT :: NXT a -> NXTInternals -> IO (a, NXTInternals)+runNXT (NXT action) internals = runStateT action internals ++{-|+Runs a computation in a context of a given 'NXTInternals' token, returning just a new token.+-}+execNXT :: NXT a -> NXTInternals -> IO NXTInternals+execNXT (NXT action) internals = execStateT action internals ++modifyNXT :: (NXTInternals -> NXTInternals) -> NXT ()+modifyNXT f = NXT (modify f)++getsNXT :: (NXTInternals -> a) -> NXT a+getsNXT f = NXT (gets f)
+ lib/Robotics/NXT/MotorControl.hs view
@@ -0,0 +1,145 @@+{-|+A message-based control of a NXT brick via remotely executed @MotorControl@ program. The main goal of this approach is to achieve+better precision of motor movements as program monitors and adapts motor commands on the NXT brick itself. For example, common+motor's overshoot is thus removed in most cases.++You should download\/compile @MotorControl@ program from <http://www.mindstorms.rwth-aachen.de/trac/wiki/MotorControl> and upload+it to the NXT brick with @nxt-upload@ program. This module will then run that program and sends it messages to control the NXT brick.+Because commands will be executed and controlled directly on the NXT brick more powerful and precise control is possible.++Please refer to original documentation of @MotorControl@ program for more information, description of commands and explanations+how to use them. This interface also implements static pauses between commands mentioned there and required by @MotorControl@. This+means functions calls will block for this time so consider using special Haskell thread for communication with the NXT brick.+Dynamic\/minimal pauses are not implemented and have to be taken care of by user of this interface.++Check "Robotics.NXT.Remote" for another (simpler) approach to message-based control. It would be great to one day combine+@MotorControl@'s precision with API (especially interruptability) of @remote.nxc@.+-}++module Robotics.NXT.MotorControl (+  -- * Initialization+  startMotorControlProgram,+  stopMotorControlProgram,+  -- * Control+  controlledMotorCmd,+  MotorControlMode(..),+  resetErrorCorrection,+  isMotorReady,+  classicMotorCmd,+  -- * Types+  SpeedRegulation+) where++import Control.Concurrent+import Control.Exception+import Control.Monad.State+import Data.List+import Data.Maybe+import Text.Printf++import Robotics.NXT++-- TODO: Implement dynamic/minimal pauses internally.++programFilename :: FileName+programFilename = "MotorControl22.rxe"++{-|+Interface to @CONTROLLED_MOTORCMD@ command which takes care of precise motor movements.++Requires dynamic\/minimal pauses.+-}+controlledMotorCmd :: [OutputPort] -> OutputPower -> TachoLimit -> [MotorControlMode] -> NXT ()+controlledMotorCmd ports power limit modes = motorControlSend message+  where message = "1" ++ (fromPorts ports) ++ (fromPower power) ++ (fromLimit limit) ++ modes'+        modes' = show . sum . zipWith (*) [1, 2, 4] . map (fromEnum . flip elem modes) $ [HoldBrake ..]++{-|+Interface to @RESET_ERROR_CORRECTION@ command which can be used to reset the NXT brick's internal error correction mechanism (and+motor position information at the same time).++The same thing can be achieved by using 'resetMotorPosition' with 'InternalPosition' argument.+-}+resetErrorCorrection :: [OutputPort] -> NXT ()+resetErrorCorrection ports = motorControlSend message+  where message = "2" ++ ports'+        ports' = fromPorts ports++{-|+Interface to @ISMOTORREADY@ command which determine the state of a motor: is it currently executing a command (for example, moving)+or is it ready to accept new commands?++Implements static pauses.+-}+isMotorReady :: [OutputPort] -> NXT [Bool]+isMotorReady ports = do+  mapM_ (\port -> motorControlSend $ "3" ++ port) ports''+  liftIO $ threadDelay (10 * 1000) -- 10 ms+  replies <- mapM (\_ -> motorControlReceive) ports''+  liftIO $ threadDelay (10 * 1000) -- 10 ms+  let replies' = map (\[p, r] -> ([p], r == '1')) replies+  return $ map (fromJust . (`lookup` replies')) ports'+    where ports' = map (show . fromEnum) ports+          ports'' = nub ports'++{-|+Interface to @CLASSIC_MOTORCMD@ command which is very similar to 'setOutputState' but better interacts with @MotorControl@.++Requires dynamic\/minimal pauses.+-}+classicMotorCmd :: [OutputPort] -> OutputPower -> TachoLimit -> SpeedRegulation -> NXT ()+classicMotorCmd ports power limit regulation = motorControlSend message+  where message = "4" ++ (fromPorts ports) ++ (fromPower power) ++ (fromLimit limit) ++ regulation'+        regulation' | regulation = "1"+                    | otherwise  = "0"++data MotorControlMode =+    HoldBrake -- ^ Keeps active brake on after the end of the movement.+  | SpeedRegulation -- ^ In a case of a load on the motor adapt motor power to retain the same speed. Really works only if there is+                    -- room for that (not that motor is already running at the maximum power).+  | SmoothStart -- ^ Smoothly starts the movement.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)++{-|+Starts @MotorControl22.rxe@ program on the NXT brick. Program has to be uploaded/available on the NXT brick.+-}+startMotorControlProgram :: NXT ()+startMotorControlProgram = ensureStartProgram programFilename++{-|+Stops current running program on the NXT brick. Probably this means @MotorControl22.rxe@ program on the NXT brick.+-}+stopMotorControlProgram :: NXT ()+stopMotorControlProgram = stopProgram++-- Sends a message to MotorControl program on the NXT brick+motorControlSend :: String -> NXT ()+motorControlSend = messageWrite Inbox1++-- Reads a message from MotorControl program on the NXT brick+motorControlReceive :: NXT String+motorControlReceive = ensureMessageRead RemoteInbox0 True++fromPorts :: [OutputPort] -> String+fromPorts ports | length ports' == 1 = show . fromEnum . head $ ports'+                | otherwise          = case ports' of+                                         [A, B]    -> "3"+                                         [A, C]    -> "4"+                                         [B, C]    -> "5"+                                         [A, B, C] -> "6"+                                         _         -> throw $ PatternMatchFail "fromPorts"+  where ports' = sort . nub $ ports++fromPower :: OutputPower -> String+fromPower power | (-100) <= power && power <= 100 = printf "%03d" $ if power < 0 then 100 + (abs power) else power+                | otherwise                       = throw $ PatternMatchFail "fromPower"++fromLimit :: TachoLimit -> String+fromLimit limit | 0 <= limit && limit <= 999999 = printf "%06d" limit+                | otherwise                     = throw $ PatternMatchFail "fromLimit"++{-|+Should in a case of a load on the motor its power be adapted to retain the same speed? Really works only if there is room for that+(not that motor is already running at the maximum power).+-}+type SpeedRegulation = Bool
+ lib/Robotics/NXT/Protocol.hs view
@@ -0,0 +1,1147 @@+{-# LANGUAGE ForeignFunctionInterface #-}+{-# CFILES ffi/initserial.c #-}++module Robotics.NXT.Protocol (+  -- * Initialization+  withNXT,+  defaultDevice,++  -- * Motors+  setOutputState,+  setOutputStateConfirm,+  getOutputState,+  resetMotorPosition,++  -- * Sensors+  setInputMode,+  setInputModeConfirm,+  getInputValues,+  resetInputScaledValue,++  -- * Miscellaneous+  getVersion,+  getDeviceInfo,+  getBatteryLevel,+  isBatteryRechargeable,+  keepAlive,+  keepAliveConfirm,+  getSleepTimeout,+  getLastKeepAliveTime,+  stopEverything,+  shutdown,++  -- * Remote Programs+  -- | It is possible to remotely run and control (with messages) programs on the NXT brick. Those here are low-level functions+  -- but check also high-level "Robotics.NXT.Remote" and "Robotics.NXT.MotorControl" modules.+  startProgram,+  startProgramConfirm,+  stopProgram,+  stopProgramConfirm,+  stopProgramExisting,+  ensureStartProgram,+  getCurrentProgramName,++  -- * Messages+  -- | It is possible to control programs on the NXT brick with messages. Those here are low-level functions+  -- but check also high-level "Robotics.NXT.Remote" and "Robotics.NXT.MotorControl" modules.+  messageWrite,+  messageWriteConfirm,+  messageRead,+  maybeMessageRead,+  ensureMessageRead,++  -- * Sounds+  playSoundFile,+  playSoundFileConfirm,+  playTone,+  stopSoundPlayback,+  stopSoundPlaybackConfirm,++  -- * Low Speed (I2C)+  -- | With those low-level functions it is possible to communicate with digital sensors attached to the NXT brick. But check+  -- also high-level "Robotics.NXT.Sensor.Ultrasonic" and "Robotics.NXT.Sensor.Compass" modules.+  lowspeedGetStatus,+  lowspeedWrite,+  lowspeedWriteConfirm,+  lowspeedRead,++  -- * Filesystem+  openWrite,+  openWriteLinear,+  write,+  writeConfirm,+  close,+  closeConfirm,+  delete,+  deleteConfirm,+  deleteExisting,++  -- * IO Map+  -- | Interface to NXT firmware is based on internal IO map interface. All commands are in fact just pretty wrappers to this+  -- interface, but it is possible to use it directly and thus gain some additional possibilities which are not+  -- available otherwise (some of those are already wrapped in this interface's additional functions and feel free to suggest+  -- more if you need them).+  getModuleID,+  listModules,+  requestFirstModule,+  requestNextModule,+  closeModuleHandle,+  closeModuleHandleConfirm,+  readIOMap,+  writeIOMap,+  writeIOMapConfirm,+  +  -- * Internals+  -- | Be careful when using those functions as you have to assure your program is well-behaved: you should see 'NXTInternals' as a+  -- token you have to pass around in order, not reusing or copying values. They are exposed so that you can decouple initalization,+  -- execution and termination phase. If you do not need that use 'withNXT'.+  --+  -- For example, using 'bracket' is not the best way to combine them together as token returned from 'initialize' in \"acquire resource\"+  -- phase is reused in \"release resource\" phase even if it was otherwise used in-between. Really use 'withNXT' for that.+  initialize,+  terminate,+  runNXT,+  execNXT+) where++import qualified Data.ByteString as B+import Control.Exception+import Control.Monad.State+import Data.Bits+import Data.Char+import Data.List hiding (delete)+import Data.Maybe+import Data.Ratio+import Data.Time.Clock.POSIX+import Data.Word+import Foreign.C.Error+import Foreign.C.Types+import System.IO+import System.Posix.IO+import System.Posix.Signals+import System.Posix.Types+import Text.Printf++import Robotics.NXT.Data+import Robotics.NXT.Errors+import Robotics.NXT.Types+import Robotics.NXT.Internals++-- Described in Lego Mindstorms NXT Bluetooth Developer Kit:+--  Appendix 1 - Communication protocol+--  Appendix 2 - Direct commands++-- TODO: All functions which requests ModuleInfo could populate module ID cache along the way+-- TODO: Add an optional warning if direction of communication changes+-- TODO: Implement all missing "confirm" versions++-- Foreign function call for C function which initialize serial port device on POSIX systems+foreign import ccall unsafe "initSerialPort" initSerialPort' :: Fd -> IO CInt++initSerialPort :: Fd -> IO ()+initSerialPort fd = throwErrnoIfMinus1_ "initSerialPort" $ initSerialPort' fd++{-|+Default Bluetooth serial device filename for current operating system. Currently always @\/dev\/rfcomm0@.+-}+defaultDevice :: FilePath+defaultDevice = "/dev/rfcomm0"++debug :: Bool+debug = False++{-|+Opens and intializes a Bluetooth serial device communication.+-}+initialize :: FilePath -> IO NXTInternals+initialize device = do+  -- we have to block signals from interrupting openFd system call (fixed in GHC versions after 6.12.1)+  let signals = foldl (flip addSignal) emptySignalSet [virtualTimerExpired]+  blockSignals signals+  fd <- openFd device ReadWrite Nothing OpenFileFlags { append = False, noctty = True, exclusive = False, nonBlock = True, trunc = False }+  unblockSignals signals+  initSerialPort fd+  h <- fdToHandle fd+  hSetBuffering h NoBuffering+  when debug $ hPutStrLn stderr "initialized"+  return $ NXTInternals h Nothing [] Nothing Nothing++{-|+Stops all NXT activities (by calling 'stopEverything') and closes the Bluetooth serial device communication. 'NXTInternals' token must not+be used after that anymore.+-}+terminate :: NXTInternals -> IO ()+terminate i = do+  i' <- execNXT stopEverything i+  let h = nxthandle i'+  hClose h+  when debug $ hPutStrLn stderr "terminated"++-- TODO: Change to mask/restore in GHC 7.0+{-|+Function which initializes and terminates Bluetooth connection to the NXT brick (using 'initialize' and 'terminate') and in-between+runs given computation. It terminates Bluetooth connection on an exception, too, rethrowing it afterwards.+-}+withNXT :: FilePath -> NXT a -> IO a+withNXT device action = block $ do+  i <- initialize device+  (r, i') <- unblock (runNXT action i) `onException` terminate i+  terminate i'+  return r++-- Main function for sending data to NXT+-- It calculates the length and prepends it to the message+sendData :: [Word8] -> NXT ()+sendData message = do+  h <- getsNXT nxthandle+  let len = toUWord . length $ message+      packet = len ++ message+  liftIO . B.hPut h . B.pack $ packet+  when debug $ liftIO . hPutStrLn stderr $ "sent: " ++ show packet++-- Main function for receiving data from NXT+receiveData :: NXT [Word8]+receiveData = do+  h <- getsNXT nxthandle+  len <- liftIO $ B.hGet h 2+  let len' = fromUWord . B.unpack $ len+  packet <- liftIO $ B.hGet h len'+  let unpacket = B.unpack packet+  when debug $ liftIO . hPutStrLn stderr $ "received: " ++ show unpacket+  return unpacket++{-|+Gets firmware and protocol versions of the NXT brick.+-}+getVersion :: NXT Version+getVersion = do+  when debug $ liftIO . hPutStrLn stderr $ "getversion"+  let send = [0x01, 0x88]+  sendData send+  receive <- receiveData+  case receive of+    [0x02, 0x88, 0x00, pMinor, pMajor, fMinor, fMajor] ->+      return $ Version (FirmwareVersion fMajor' fMinor') (ProtocolVersion pMajor' pMinor')+        where fMajor' = fromIntegral fMajor+              fMinor' = fromIntegral fMinor+              pMajor' = fromIntegral pMajor+              pMinor' = fromIntegral pMinor+    _:_:e:_                                            -> liftIO $ failNXT "getVersion" e+    _                                                  -> liftIO $ failNXT' "getVersion"++{-|+Gets device (the NXT brick) information: name, Bluetooth 48 bit address in the string format, strength of Bluetooth signal (not implemented in+current NXT firmware versions, use 'bluetoothRSSI' or 'bluetoothLinkQuality' as an alternative), free space on flash.+-}+getDeviceInfo :: NXT DeviceInfo+getDeviceInfo = do+  when debug $ liftIO . hPutStrLn stderr $ "getdeviceinfo"+  let send = [0x01, 0x9B]+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x9B:0x00:info | length info == 30 -> do+      modifyNXT (\s -> s { address = Just btaddress }) -- we cache it+      return $ DeviceInfo name' btaddress btstrength flashfree+        where (name, info') = splitAt 15 info+              name' = dataToString0 name+              btaddress = map toUpper . intercalate ":" . map (printf "%02x") . take 6 $ info'+              -- 7th byte not used?+              btstrength = fromULong . take 4 . drop 7 $ info'+              flashfree = fromULong . take 4 . drop 11 $ info'+    _:_:e:_               -> liftIO $ failNXT "getDeviceInfo" e+    _                     -> liftIO $ failNXT' "getDeviceInfo"++{-|+Starts a given program on the NXT brick.+-}+startProgram :: FileName -> NXT ()+startProgram = startProgram' False++{-|+Same as 'startProgram' but also request a confirmation. Useful to assure the command was really accepted, but this does not assure+that the program has really started successfully (especially not that it is already running when the confirmation is received).+Use 'ensureStartProgram' for that. In a case of an error it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms.+-}+startProgramConfirm :: FileName -> NXT ()+startProgramConfirm = startProgram' True++startProgram' :: Bool -> FileName -> NXT ()+startProgram' confirm filename = do+  when debug $ liftIO . hPutStrLn stderr $ "startprogram"+  let send = [request confirm, 0x00] ++ nameToData filename+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x00, 0x00] -> return ()+      [_, _, e]          -> liftIO $ failNXT "startProgram" e+      _                  -> liftIO $ failNXT' "startProgram"++{-|+Stops a currently running program.+-}+stopProgram :: NXT ()+stopProgram = stopProgram' False False++{-|+Same as 'stopProgram' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it+throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+stopProgramConfirm :: NXT ()+stopProgramConfirm = stopProgram' True False++{-|+Same as 'stopProgramConfirm' but it also requires that the program was really running. It throws a 'NXTException' otherwise.+-}+stopProgramExisting :: NXT ()+stopProgramExisting = stopProgram' True True++stopProgram' :: Bool -> Bool -> NXT ()+stopProgram' confirm running = do+  when debug $ liftIO . hPutStrLn stderr $ "stopprogram"+  let send = [request confirm, 0x01]+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x01, 0x00] -> return ()+      [0x02, 0x01, 0xEC] -> when running $ liftIO $ failNXT "stopProgram" 0xEC+      [_, _, e]          -> liftIO $ failNXT "stopProgram" e+      _                  -> liftIO $ failNXT' "stopProgram"++-- TODO: Could probably loop infinitely in some strange situation? Some timeout could be useful?+{-|+Helper function which first ensures that no other program is running and then ensures that a given program is really running before+it returns.+-}+ensureStartProgram :: FileName -> NXT ()+ensureStartProgram filename = do+  stopAndWait+  startAndWait+    where stopAndWait = do+            stopProgramConfirm+            name <- getCurrentProgramName+            unless (isNothing name) stopAndWait+          startAndWait = do+            startProgramConfirm filename+            name <- getCurrentProgramName+            unless (isJust name) startAndWait++{-|+Plays a given sound file.+-}+playSoundFile :: LoopPlayback -> FileName -> NXT ()+playSoundFile = playSoundFile' False++{-|+Same as 'playSoundFile' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it+throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+playSoundFileConfirm :: LoopPlayback -> FileName -> NXT ()+playSoundFileConfirm = playSoundFile' True++playSoundFile' :: Bool -> LoopPlayback -> FileName -> NXT ()+playSoundFile' confirm loop filename = do+  when debug $ liftIO . hPutStrLn stderr $ "playsoundfile"+  let send = [request confirm, 0x02, fromIntegral . fromEnum $ loop] ++ nameToData filename+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x02, 0x00] -> return ()+      [_, _, e]          -> liftIO $ failNXT "playSoundFile" e+      _                  -> liftIO $ failNXT' "playSoundFile"++{-|+Plays a tone with a given frequency (in hertz) for a given duration (in seconds).+-}+playTone :: Frequency -> Duration -> NXT ()+playTone frequency duration = do+  when debug $ liftIO . hPutStrLn stderr $ "playtone"+  let send = [0x80, 0x03] ++ toUWord frequency ++ toUWord (toMilliseconds duration)+  sendData send+    where toMilliseconds :: Duration -> Integer -- duration is in seconds, but NXT requires milliseconds+          toMilliseconds d = floor (d * 1000)++{-|+Sets output port (motor) state. This is the main function for controlling a motor.+-}+setOutputState :: OutputPort -> OutputPower -> [OutputMode] -> RegulationMode -> TurnRatio -> RunState -> TachoLimit -> NXT ()+setOutputState = setOutputState' False++{-|+Same as 'setOutputState' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error+it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms.+-}+setOutputStateConfirm :: OutputPort -> OutputPower -> [OutputMode] -> RegulationMode -> TurnRatio -> RunState -> TachoLimit -> NXT ()+setOutputStateConfirm = setOutputState' True++setOutputState' :: Bool -> OutputPort -> OutputPower -> [OutputMode] -> RegulationMode -> TurnRatio -> RunState -> TachoLimit -> NXT ()+setOutputState' confirm output power mode regulation turn runstate tacholimit+  | power >= -100 && power <= 100 && turn >= -100 && turn <= 100 = do+      when debug $ liftIO . hPutStrLn stderr $ "setoutputstate"+      let send = [request confirm, 0x04, fromIntegral . fromEnum $ output] ++ toSByte power ++ [modebyte, regulation'] ++ toSByte turn ++ [runstate'] ++ toULong tacholimit+      sendData send+      when confirm $ do+        receive <- receiveData+        case receive of+          [0x02, 0x04, 0x00] -> return ()+          [_, _, e]          -> liftIO $ failNXT "setOutputState" e+          _                  -> liftIO $ failNXT' "setOutputState"+  | otherwise                                                    = liftIO . throwIO $ PatternMatchFail "setOutputState"+    where modebyte    = foldl (.|.) 0x00 . map convmode $ mode+            where convmode m = case m of+                                 MotorOn -> 0x01+                                 Brake -> 0x02+                                 Regulated -> 0x04+          regulation' = case regulation of+                          RegulationModeIdle -> 0x00+                          RegulationModeMotorSpeed -> 0x01+                          RegulationModeMotorSync -> 0x02+          runstate'   = case runstate of+                          MotorRunStateIdle -> 0x00+                          MotorRunStateRampUp -> 0x10+                          MotorRunStateRunning -> 0x20+                          MotorRunStateRampDown -> 0x40+                          MotorRunStateHold -> 0x60++{-|+Gets output port (motor) current state. In additional to values used with 'setOutputState' also 'TachoCount', 'BlockTachoCount'+and 'RotationCount' values are available which tell you current position of a motor.+-}+getOutputState :: OutputPort -> NXT OutputState+getOutputState output = do+  when debug $ liftIO . hPutStrLn stderr $ "getoutputstate"+  let send = [0x00, 0x06, fromIntegral . fromEnum $ output]+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x06:0x00:port:power:modebyte:regulation:turn:runstate:values+      | length values == 16 && fromEnum output == fromIntegral port ->+          return $ OutputState output (fromSByte [power]) mode regulation' (fromSByte [turn]) runstate' tacholimit tachocount blocktachocount rotationcount+            where mode = motoron ++ brake ++ regulated+                  motoron = [MotorOn | testBit modebyte 0]+                  brake = [Brake | testBit modebyte 1]+                  regulated = [Regulated | testBit modebyte 2]+                  regulation' = case regulation of+                                  0x00 -> RegulationModeIdle+                                  0x01 -> RegulationModeMotorSpeed+                                  0x02 -> RegulationModeMotorSync+                                  _    -> throw $ PatternMatchFail "getOutputState"+                  runstate' = case runstate of+                                0x00 -> MotorRunStateIdle+                                0x10 -> MotorRunStateRampUp+                                0x20 -> MotorRunStateRunning+                                0x40 -> MotorRunStateRampDown+                                0x60 -> MotorRunStateHold+                                _    -> throw $ PatternMatchFail "getOutputState"+                  tacholimit = fromULong . take 4 $ values+                  tachocount = fromSLong . take 4 . drop 4 $ values+                  blocktachocount = fromSLong . take 4 . drop 8 $ values+                  rotationcount = fromSLong . take 4 . drop 12 $ values+    _:_:e:_                                                         -> liftIO $ failNXT "getOutputState" e+    _                                                               -> liftIO $ failNXT' "getOutputState"++{-|+Sets input port (sensor) type and mode.+-}+setInputMode :: InputPort -> SensorType -> SensorMode -> NXT ()+setInputMode = setInputMode' False++{-|+Same as 'setInputMode' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+setInputModeConfirm :: InputPort -> SensorType -> SensorMode -> NXT ()+setInputModeConfirm = setInputMode' True++setInputMode' :: Bool -> InputPort -> SensorType -> SensorMode -> NXT ()+setInputMode' confirm input sensortype sensormode = do+  when debug $ liftIO . hPutStrLn stderr $ "setinputmode"+  let send = [request confirm, 0x05, fromIntegral . fromEnum $ input, sensortype', sensormode']+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x05, 0x00] -> return ()+      [_, _, e]          -> liftIO $ failNXT "setInputMode" e+      _                  -> liftIO $ failNXT' "setInputMode"+    where sensortype' = case sensortype of+                          NoSensor -> 0x00+                          Switch -> 0x01+                          Temperature -> 0x02+                          Reflection -> 0x03+                          Angle -> 0x04+                          LightActive -> 0x05+                          LightInactive -> 0x06+                          SoundDB -> 0x07+                          SoundDBA -> 0x08+                          Custom -> 0x09+                          Lowspeed -> 0x0A+                          Lowspeed9V -> 0x0B+                          NoOfSensorTypes -> 0x0C+          sensormode' = case sensormode of+                          RawMode -> 0x00+                          BooleanMode -> 0x20+                          TransitionCntMode -> 0x40+                          PeriodCounterMode -> 0x60+                          PctFullScaleMode -> 0x80+                          CelsiusMode -> 0xA0+                          FahrenheitMode -> 0xC0+                          AngleStepsMode -> 0xE0+                          -- two modes unnecessary?+                          -- SlopeMask -> 0x1F+                          -- ModeMask -> 0xE0++{-|+Gets input port (sensor) values. This is the main function for reading a sensor.+-}+getInputValues :: InputPort -> NXT InputValue+getInputValues input = do+  when debug $ liftIO . hPutStrLn stderr $ "getinputvalues"+  let send = [0x00, 0x07, fromIntegral . fromEnum $ input]+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x07:0x00:port:valid:calibrated:sensortype:sensormode:values+      | length values == 8 && fromEnum input == fromIntegral port ->+          return $ InputValue input valid' calibrated' sensortype' sensormode' raw normalized scaled calibratedv+            where valid'      = valid /= 0x00+                  calibrated' = calibrated /= 0x00+                  sensortype' = case sensortype of+                                  0x00 -> NoSensor+                                  0x01 -> Switch+                                  0x02 -> Temperature+                                  0x03 -> Reflection+                                  0x04 -> Angle+                                  0x05 -> LightActive+                                  0x06 -> LightInactive+                                  0x07 -> SoundDB+                                  0x08 -> SoundDBA+                                  0x09 -> Custom+                                  0x0A -> Lowspeed+                                  0x0B -> Lowspeed9V+                                  0x0C -> NoOfSensorTypes+                                  _    -> throw $ PatternMatchFail "getInputValues"+                  sensormode' = case sensormode of+                                  0x00 -> RawMode+                                  0x20 -> BooleanMode+                                  0x40 -> TransitionCntMode+                                  0x60 -> PeriodCounterMode+                                  0x80 -> PctFullScaleMode+                                  0xA0 -> CelsiusMode+                                  0xC0 -> FahrenheitMode+                                  0xE0 -> AngleStepsMode+                                  -- two modes unnecessary?+                                  -- 0x1F -> SlopeMask+                                  -- 0xE0 -> ModeMask+                                  _    -> throw $ PatternMatchFail "getInputValues"+                  raw         = fromUWord . take 2 $ values+                  normalized  = fromUWord . take 2 . drop 2 $ values+                  scaled      = fromSWord . take 2 . drop 4 $ values+                  calibratedv = fromSWord . take 2 . drop 6 $ values+    _:_:e:_                                                       -> liftIO $ failNXT "getInputValues" e+    _                                                             -> liftIO $ failNXT' "getInputValues"++{-|+Resets input port (sensor) scaled value.+-}+resetInputScaledValue :: InputPort -> NXT ()+resetInputScaledValue input = do+  when debug $ liftIO . hPutStrLn stderr $ "resetinputscaledvalue"+  let send = [0x80, 0x08, fromIntegral . fromEnum $ input]+  sendData send++{-|+Writes a message to the given inbox queue of the running remote program. A message length is limited to 58 characters/bytes. A queue+is limited to 5 messages.+-}+messageWrite :: Inbox -> String -> NXT ()+messageWrite = messageWrite' False++{-|+Same as 'messageWrite' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it+throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms.+-}+messageWriteConfirm :: Inbox -> String -> NXT ()+messageWriteConfirm = messageWrite' True++messageWrite' :: Bool -> Inbox -> String -> NXT ()+messageWrite' confirm inbox message+  | length message <= 58 = do+      when debug $ liftIO . hPutStrLn stderr $ "messagewrite"+      let message' = messageToData message+          send = [request confirm, 0x09, fromIntegral . fromEnum $ inbox] ++ (toUByte . length $ message') ++ message'+      sendData send+      when confirm $ do+        receive <- receiveData+        case receive of+          [0x02, 0x09, 0x00] -> return ()+          [_, _, e]          -> liftIO $ failNXT "messageWrite" e+          _                  -> liftIO $ failNXT' "messageWrite"+  | otherwise             = liftIO . throwIO $ PatternMatchFail "messageWrite"++{-|+Resets one of three position counters for a given output port.+-}+resetMotorPosition :: OutputPort -> MotorReset -> NXT ()+resetMotorPosition output reset = do+  when debug $ liftIO . hPutStrLn stderr $ "resetmotorposition"+  case reset of+    InternalPosition -> do+      mid <- getModuleID "Output.mod"+      writeIOMap (fromJust mid) (fromEnum output * 32 + 18) [0x08] -- flags field is at offset 18, output block is 32 bytes long, UPDATE_RESET_COUNT is 0x08+    _                -> do+      let send = [0x80, 0x0A, fromIntegral . fromEnum $ output, fromIntegral . fromEnum $ reset]+      sendData send++{-|+Gets current battery level (in volts).+-}+getBatteryLevel :: NXT Voltage+getBatteryLevel = do+  when debug $ liftIO . hPutStrLn stderr $ "getbatterylevel"+  let send = [0x00, 0x0B]+  sendData send+  receive <- receiveData+  case receive of+    [0x02, 0x0B, 0x00, v1, v2] -> return $ fromUWord [v1, v2] % 1000 -- voltage is in millivolts+    _:_:e:_                    -> liftIO $ failNXT "getBatteryLevel" e+    _                          -> liftIO $ failNXT' "getBatteryLevel"++{-|+Is battery used in the NXT brick rechargeable?+-}+isBatteryRechargeable :: NXT Bool+isBatteryRechargeable = do+  when debug $ liftIO . hPutStrLn stderr $ "isbatteryrechargeable"+  mid <- getModuleID "Ui.mod"+  r <- readIOMap (fromJust mid) 35 1+  return $ (/=) 0 (head r)++{-|+Stops current sound file playback.+-}+stopSoundPlayback :: NXT ()+stopSoundPlayback = stopSoundPlayback' False++{-|+Same as 'stopSoundPlayback' but also request a confirmation. Useful to assure the command was really accepted. In a case of an+error it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+stopSoundPlaybackConfirm :: NXT ()+stopSoundPlaybackConfirm = stopSoundPlayback' True++stopSoundPlayback' :: Bool -> NXT ()+stopSoundPlayback' confirm = do+  when debug $ liftIO . hPutStrLn stderr $ "stopsoundplayback"+  let send = [request confirm, 0x0C]+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x0C, 0x00] -> return ()+      [_, _, e]          -> liftIO $ failNXT "stopSoundPlayback" e+      _                  -> liftIO $ failNXT' "stopSoundPlayback"++{-|+Sends a keep alive (turned on) packet. It prevents the NXT brick from automatically powering off. Other commands do not prevent that+from hapenning so it is useful to send this packet from time to time if you want to prevent powering off.+-}+keepAlive :: NXT ()+keepAlive = keepAlive' False >> return ()++{-|+Same as 'keepAlive' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it+throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+keepAliveConfirm :: NXT ()+keepAliveConfirm = keepAlive' True >> return ()++keepAlive' :: Bool -> NXT Duration+keepAlive' confirm = do+  when debug $ liftIO . hPutStrLn stderr $ "keepalive"+  current <- liftIO getPOSIXTime+  modifyNXT (\s -> s { lastkeepalive = Just current })+  let send = [0x00, 0x0D]+  sendData send+  if confirm+    then do+      receive <- receiveData+      case receive of+        0x02:0x0D:0x00:limit -> do+          let l = fromRational $ fromULong limit % 1000 -- limit is in milliseconds+          modifyNXT (\s -> s { sleeptime = Just l })+          return l+        _:_:e:_              -> liftIO $ failNXT "keepAlive" e+        _                    -> liftIO $ failNXT' "keepAlive"+    else return 0++{-|+Gets current sleep timeout setting (in seconds) after which the NXT brick automatically powers off if+not prevented with a keep alive packet (use 'keepAlive' to send one). This setting is cached.+-}+getSleepTimeout :: NXT Duration+getSleepTimeout = do+  sleep <- getsNXT sleeptime+  case sleep of+    Just s  -> return s+    Nothing -> keepAlive' True++{-|+When was a last keep alive packet send?+-}+getLastKeepAliveTime :: NXT (Maybe POSIXTime)+getLastKeepAliveTime = getsNXT lastkeepalive++{-|+Gets number of bytes available to read.+-}+lowspeedGetStatus :: InputPort -> NXT Int+lowspeedGetStatus input = do+  when debug $ liftIO . hPutStrLn stderr $ "lowspeedgetstatus"+  let send = [0x00, 0x0E, fromIntegral . fromEnum $ input]+  sendData send+  receive <- receiveData+  case receive of+    [0x02, 0x0E, 0x00, bytes] -> return $ fromUByte [bytes]+    0x02:0x10:0x20:_          -> lowspeedGetStatus input -- pending communication transaction in progress, retrying+    _:_:e:_                   -> liftIO $ failNXT "lowSpeedGetStatus" e+    _                         -> liftIO $ failNXT' "lowSpeedGetStatus"++{-|+Writes data. At most 16 bytes can be written at a time.++Reply data length must be specified in the write command since reading from the device is done on a master-slave basis.+-}+lowspeedWrite :: InputPort -> RxDataLength -> TxData -> NXT ()+lowspeedWrite = lowspeedWrite' False++{-|+Same as 'lowspeedWrite' but also request a confirmation. Useful to assure the command was really accepted. In a case of an+error it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+lowspeedWriteConfirm :: InputPort -> RxDataLength -> TxData -> NXT ()+lowspeedWriteConfirm = lowspeedWrite' True++lowspeedWrite' :: Bool -> InputPort -> RxDataLength -> TxData -> NXT ()+lowspeedWrite' confirm input rx txdata+  | length txdata <= 16 && rx <= 16 = do+      when debug $ liftIO . hPutStrLn stderr $ "lowspeedwrite"+      let send = [request confirm, 0x0F, fromIntegral . fromEnum $ input] ++ (toUByte . length $ txdata) ++ toUByte rx ++ txdata+      sendData send+      when confirm $ do+        receive <- receiveData+        case receive of+          [0x02, 0x0F, 0x00] -> return ()+          [_, _, e]          -> liftIO $ failNXT "lowspeedWrite" e+          _                  -> liftIO $ failNXT' "lowspeedWrite"+  | otherwise                       = liftIO . throwIO $ PatternMatchFail "lowspeedWrite"++{-|+Reads data. The protocol does not support variable-length return packages so the response always contains 16 data bytes with invalid+data padded with zeros.+-}+lowspeedRead :: InputPort -> NXT RxData+lowspeedRead input = do+  when debug $ liftIO . hPutStrLn stderr $ "lowspeedread"+  let send = [0x00, 0x10, fromIntegral . fromEnum $ input]+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x10:0x00:rx:rxdata+      | length rxdata == 16 && rx <= 16 -> return $ take (fromUByte [rx]) rxdata+    0x02:0x10:0x20:_                    -> lowspeedRead input -- pending communication transaction in progress, retrying+    _:_:e:_                             -> liftIO $ failNXT "lowSpeedRead" e+    _                                   -> liftIO $ failNXT' "lowSpeedRead"++{-|+Gets the name of the currently running program, if any.+-}+getCurrentProgramName :: NXT (Maybe String)+getCurrentProgramName = do+  when debug $ liftIO . hPutStrLn stderr $ "getcurrentprogramname"+  let send = [0x00, 0x11]+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x11:0x00:filename | length filename == 20 -> return $ Just $ dataToString0 filename+    0x02:0x11:0xEC:_                                -> return Nothing+    _:_:e:_                                         -> liftIO $ failNXT "getCurrentProgramName" e+    _                                               -> liftIO $ failNXT' "getCurrentProgramName"++{-|+Reads a message from the currently running program from a given remote inbox queue. A queue is limited to 5 messages.+It throws a 'NXTException' if there is no message in a remote inbox queue.+-}+messageRead :: RemoteInbox -> RemoveMessage -> NXT String+messageRead inbox remove = do+  m <- maybeMessageRead inbox remove+  case m of+    Just m' -> return m'+    Nothing -> liftIO $ failNXT "messageRead" 0x40++-- TODO: Could probably loop infinitely? Some timeout could be useful?+{-|+Same as 'messageWrite' but if there is no message in a given remote inbox queue it retries until there is.+-}+ensureMessageRead :: RemoteInbox -> RemoveMessage -> NXT String+ensureMessageRead inbox remove = do+  m <- maybeMessageRead inbox remove+  case m of+    Just m' -> return m'+    Nothing -> ensureMessageRead inbox remove++{-|+Same as 'messageWrite' but returns 'Nothing' if there is no message in a given remote inbox queue.+-}+maybeMessageRead :: RemoteInbox -> RemoveMessage -> NXT (Maybe String)+maybeMessageRead inbox remove = do+  when debug $ liftIO . hPutStrLn stderr $ "messageRead"+  let inbox' = fromIntegral . fromEnum $ inbox+      send = [0x00, 0x13, inbox', fromIntegral . fromEnum $ Inbox0, fromIntegral . fromEnum $ remove] -- local inbox number does not matter for PC, it is used only when master NXT reads from slave NXT+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x13:0x00:inbox'':size:message+      | inbox'' == inbox' && length message == 59 && size <= 59 -> return $ Just $ dataToString0 message+    0x02:0x13:0x40:_                                            -> return Nothing+    _:_:e:_                                                     -> liftIO $ failNXT "messageRead" e+    _                                                           -> liftIO $ failNXT' "messageRead"++{-|+Helper function which stops all NXT brick activities: stops motors and disables sensors.+-}+stopEverything :: NXT ()+stopEverything = do+  when debug $ liftIO . hPutStrLn stderr $ "stopeverything"+  mapM_ stopMotor [A ..]+  mapM_ stopSensor [One ..]+    where stopMotor x = setOutputState x 0 [] RegulationModeIdle 0 MotorRunStateIdle 0+          stopSensor x = setInputMode x NoSensor RawMode++{-|+Shutdowns (powers off) the NXT brick. You have to manually turn it on again.+-}+shutdown :: NXT ()+shutdown = do+  when debug $ liftIO . hPutStrLn stderr $ "shutdown"+  mid <- getModuleID "IOCtrl.mod"+  writeIOMap (fromJust mid) 0 [0x00, 0x5A]++{-|+Opens a given file for writing as a linked list of flash sectors.+-}+openWrite :: FileName -> FileSize -> NXT FileHandle+openWrite filename filesize = do+  when debug $ liftIO . hPutStrLn stderr $ "openwrite"+  let send = [0x01, 0x81] ++ nameToData filename ++ toULong filesize+  sendData send+  receive <- receiveData+  case receive of+    [0x02, 0x81, 0x00, h] -> return $ fromUByte [h]+    _:_:e:_               -> liftIO $ failNXT "openWrite" e+    _                     -> liftIO $ failNXT' "openWrite"++{-|+Opens a given file for writing as a linear contiguous block of flash memory (required for user programs and certain data files).+-}+openWriteLinear :: FileName -> FileSize -> NXT FileHandle+openWriteLinear filename filesize = do+  when debug $ liftIO . hPutStrLn stderr $ "openwritelinear"+  let send = [0x01, 0x89] ++ nameToData filename ++ toULong filesize+  sendData send+  receive <- receiveData+  case receive of+    [0x02, 0x89, 0x00, h] -> return $ fromUByte [h]+    _:_:e:_               -> liftIO $ failNXT "openWriteLinear" e+    _                     -> liftIO $ failNXT' "openWriteLinear"++{-|+Writes data to a file. At most 61 bytes can be written at a time.+-}+write :: FileHandle -> FileData -> NXT ()+write = write' False++{-|+Same as 'write' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it+throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+writeConfirm :: FileHandle -> FileData -> NXT ()+writeConfirm = write' True++write' :: Bool -> FileHandle -> FileData -> NXT ()+write' confirm filehandle filedata+  | length filedata <= 61 = do+      when debug $ liftIO . hPutStrLn stderr $ "write"+      let send = [request' confirm, 0x83] ++ toUByte filehandle ++ filedata+      sendData send+      when confirm $ do+        receive <- receiveData+        case receive of+          [0x02, 0x83, 0x00, h, bw1, bw2]+            | fromUByte [h] == filehandle && length filedata == fromUWord [bw1, bw2] -> return ()+          _:_:e:_                                                                    -> liftIO $ failNXT "write" e+          _                                                                          -> liftIO $ failNXT' "write"+  | otherwise             = liftIO . throwIO $ PatternMatchFail "write"++{-|+Closes a file.+-}+close :: FileHandle -> NXT ()+close = close' False++{-|+Same as 'close' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it+throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+closeConfirm :: FileHandle -> NXT ()+closeConfirm = close' True++close' :: Bool -> FileHandle -> NXT ()+close' confirm filehandle = do+  when debug $ liftIO . hPutStrLn stderr $ "close"+  let send = [request' confirm, 0x84] ++ toUByte filehandle+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x84, 0x00, h]+        | fromUByte [h] == filehandle -> return ()+      _:_:e:_                         -> liftIO $ failNXT "close" e+      _                               -> liftIO $ failNXT' "close"++{-|+Deletes a given file.+-}+delete :: FileName -> NXT ()+delete = delete' False False++{-|+Same as 'delete' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error it throws+a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+deleteConfirm :: FileName -> NXT ()+deleteConfirm = delete' True False++{-|+Same as 'deleteConfirm' but it also requires that the file exists before deletion. It throws a 'NXTException' otherwise.+-}+deleteExisting :: FileName -> NXT ()+deleteExisting = delete' True True++delete' :: Bool -> Bool -> FileName -> NXT ()+delete' confirm existence filename = do+  when debug $ liftIO . hPutStrLn stderr $ "delete"+  let send = [request' confirm, 0x85] ++ nameToData filename+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      0x02:0x85:0x00:f+        | dataToString0 f == filename -> return ()+      0x02:0x85:0x87:_                -> when existence $ liftIO $ failNXT "delete" 0x87+      _:_:e:_                         -> liftIO $ failNXT "delete" e+      _                               -> liftIO $ failNXT' "delete"++-- TODO: Populate cache here?+{-|+Requests information about the first module matching a given module name (which can be a wild card). Returned module handle+can be used for followup requests and has to be closed when not needed anymore.+-}+requestFirstModule :: ModuleName -> NXT (ModuleHandle, Maybe ModuleInfo)+requestFirstModule modulename = do+  when debug $ liftIO . hPutStrLn stderr $ "requestfirstmodule"+  let send = [0x01, 0x90] ++ nameToData modulename+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x90:0x00:h:values+      | length values == 30 -> return (fromUByte [h], Just $ ModuleInfo name moduleid size iomapsize)+                                 where name      = dataToString0 . take 20 $ values+                                       moduleid  = fromULong . take 4 . drop 20 $ values+                                       size      = fromULong . take 4 . drop 24 $ values+                                       iomapsize = fromUWord . take 2 . drop 28 $ values+    0x02:0x90:0x90:h:_      -> return (fromUByte [h], Nothing) -- module not found+    _:_:e:_                 -> liftIO $ failNXT "requestFirstModule" e+    _                       -> liftIO $ failNXT' "requestFirstModule"++-- TODO: Populate cache here?+{-|+Requests information about the next module matching previously requested module name (which can be a wild card). Returned module+handle can be used for followup requests and has to be closed when not needed anymore.+-}+requestNextModule :: ModuleHandle -> NXT (ModuleHandle, Maybe ModuleInfo)+requestNextModule modulehandle = do+  when debug $ liftIO . hPutStrLn stderr $ "requestnextmodule"+  let send = [0x01, 0x91] ++ toUByte modulehandle+  sendData send+  receive <- receiveData+  case receive of+    0x02:0x91:0x00:h:values+      | length values == 30 -> return (fromUByte [h], Just $ ModuleInfo name moduleid size iomapsize)+                                 where name      = dataToString0 . take 20 $ values+                                       moduleid  = fromULong . take 4 . drop 20 $ values+                                       size      = fromULong . take 4 . drop 24 $ values+                                       iomapsize = fromUWord . take 2 . drop 28 $ values+    0x02:0x91:0x90:h:_      -> return (fromUByte [h], Nothing) -- module not found+    _:_:e:_                 -> liftIO $ failNXT "requestNextModule" e+    _                       -> liftIO $ failNXT' "requestNextModule"++{-|+Closes module handle of previously requested module information.+-}+closeModuleHandle :: ModuleHandle -> NXT ()+closeModuleHandle = closeModuleHandle' False++{-|+Same as 'closeModuleHandle' but also request a confirmation. Useful to assure the command was really accepted. In a case of an+error it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms.+-}+closeModuleHandleConfirm :: ModuleHandle -> NXT ()+closeModuleHandleConfirm = closeModuleHandle' True++closeModuleHandle' :: Bool -> ModuleHandle -> NXT ()+closeModuleHandle' confirm modulehandle = do+  when debug $ liftIO . hPutStrLn stderr $ "closemodulehandle"+  let send = [request' confirm, 0x92] ++ toUByte modulehandle+  sendData send+  when confirm $ do+    receive <- receiveData+    case receive of+      [0x02, 0x92, 0x00, h]+        | fromUByte [h] == modulehandle -> return ()+      _:_:e:_                           -> liftIO $ failNXT "closeModuleHandle" e+      _                                 -> liftIO $ failNXT' "closeModuleHandle"++-- TODO: Populate cache here?+-- TODO: Use bracket to ensure closed module handle?+{-|+Helper function to get information about all modules matching a given module name (which can be a wild card).+-}+listModules :: ModuleName -> NXT [ModuleInfo]+listModules modulename = do+  (h, first) <- requestFirstModule modulename+  case first of+    Nothing     -> do+      closeModuleHandle h+      return []+    Just first' -> do+      (h', other) <- next h+      closeModuleHandle h'+      return (first':other)+    where next h = do+            (h', mi) <- requestNextModule h+            case mi of+              Just mi' -> do+                (h'', mi'') <- next h'+                return (h'', mi':mi'')+              Nothing  -> return (h', [])++{-|+Reads data from an IO map of a given module. At most 119 bytes can be read at a time.++You probably have to know what different values at different positions mean and control. The best way is to check NXT firmware+source code.+-}+readIOMap :: ModuleID -> IOMapOffset -> IOMapLength -> NXT IOMapData+readIOMap moduleid offset len+  | offset >= 0 && len <= 119 = do+      when debug $ liftIO . hPutStrLn stderr $ "readiomap"+      let send = [0x01, 0x94] ++ toULong moduleid ++ toUWord offset ++ toUWord len+      sendData send+      receive <- receiveData+      case receive of+        0x02:0x94:0x00:mid1:mid2:mid3:mid4:r1:r2:values+          | fromULong [mid1, mid2, mid3, mid4] == moduleid && fromUWord [r1, r2] == len -> return values+        _:_:e:_                                                                         -> liftIO $ failNXT "readIOMap" e+        _                                                                               -> liftIO $ failNXT' "readIOMap"+  | otherwise                 = liftIO . throwIO $ PatternMatchFail "readIOMap"+++{-|+Writes data to an IO map of a given module. At most 54 bytes can be written at a time.++You probably have to know what different values at different positions mean and control. The best way is to check NXT firmware+source code.+-}+writeIOMap :: ModuleID -> IOMapOffset -> IOMapData -> NXT ()+writeIOMap = writeIOMap' False++{-|+Same as 'writeIOMap' but also request a confirmation. Useful to assure the command was really accepted. In a case of an error+it throws a 'NXTException'.++Confirmation requires a change of the direction of NXT Bluetooth communication which takes around 30 ms. +-}+writeIOMapConfirm :: ModuleID -> IOMapOffset -> IOMapData -> NXT ()+writeIOMapConfirm = writeIOMap' True++writeIOMap' :: Bool -> ModuleID -> IOMapOffset -> IOMapData -> NXT ()+writeIOMap' confirm moduleid offset mapdata+  | offset >= 0 && length mapdata <= 54 = do+      when debug $ liftIO . hPutStrLn stderr $ "writeiomap"+      let send = [request' confirm, 0x95] ++ toULong moduleid ++ toUWord offset ++ toUWord (length mapdata) ++ mapdata+      sendData send+      when confirm $ do+        receive <- receiveData+        case receive of+          [0x02, 0x95, 0x00, mid1, mid2, mid3, mid4, w1, w2]+            | fromULong [mid1, mid2, mid3, mid4] == moduleid && fromUWord [w1, w2] == length mapdata -> return ()+          _:_:e:_                                                                                    -> liftIO $ failNXT "writeIOMap" e+          _                                                                                          -> liftIO $ failNXT' "writeIOMap"+  | otherwise                           = liftIO . throwIO $ PatternMatchFail "writeIOMap"++{-|+Helper function to get an ID of a module matching a given module name. Each module encompass some firmware functionality.+Function caches IDs so it hopefully retrieves it from a cache of previous requests.+-}+getModuleID :: ModuleName -> NXT (Maybe ModuleID)+getModuleID modulename | '*' `elem` modulename = return Nothing -- we do not allow wild cards+                       | otherwise = do+                           mods <- getsNXT modules+                           let modulename' = map toLower modulename+                           case modulename' `lookup` mods of+                             Just (ModuleInfo _ mid _ _) -> return $ Just mid+                             Nothing                     -> do+                               (h, mi) <- requestFirstModule modulename'+                               closeModuleHandle h+                               case mi of+                                 Just mi'@(ModuleInfo _ mid _ _) -> do+                                   modifyNXT (\s -> s { modules = (modulename', mi'):mods })+                                   return $ Just mid+                                 Nothing                         -> return Nothing++request :: Bool -> Word8 +request confirm | confirm   = 0x00+                | otherwise = 0x80++request' :: Bool -> Word8 +request' confirm | confirm   = 0x01+                 | otherwise = 0x81
+ lib/Robotics/NXT/Remote.hs view
@@ -0,0 +1,82 @@+{-|+A simple message-based control of the NXT brick via remotely executed program. The main goal of this approach is to reduce latency+otherwise encountered when using NXT Bluetooth communication. Namely, changing the direction of communication takes around 30 ms.+This means that one cycle of requesting motor status, receiving data and then sending motor control update takes at least 60 ms (it+takes two changes of communication direction). And this is especially a problem if you want to control multiple motors at the same+time (and built-in synchronize mechanism is not good enough for you).++One solution to this problem is that Bluetooth is used only for communication in one direction and an additional program on the NXT+brick is checking motor status and correcting possible errors. Of course this also means that information about motor position have+to be obtained in some other way: by predicting from sent commands, or measuring/probing with regular NXT commands when there is time+for that (and latency at that time is not a problem), or by using some external sensors attached to the controlling computer and not+to the NXT brick (camera tracking system for example).++Use NBC (<http://bricxcc.sourceforge.net/nbc/>) to compile basic NXC code (@remote.nxc@) included with this module into @remote.rxe@+NXT program (or use already compiled version) and then upload it with @nxt-upload@ program. This module will then run that program+and sends it messages to control the NXT brick. Because commands will be executed and controlled directly on the NXT brick less+latency and more powerful control is possible.++Check "Robotics.NXT.MotorControl" for another (more precise but more complex) approach to message-based control. It would be great to+one day combine @MotorControl@'s precision with API (especially interruptability) of @remote.nxc@.+-}++module Robotics.NXT.Remote (+  -- * Initialization+  startRemoteProgram,+  stopRemoteProgram,+  -- * Control+  sendRemoteCommand,+  RemoteCommand(..),+  RemoteCommandType(..)+) where++import Control.Exception+import Text.Printf++import Robotics.NXT++programFilename :: FileName+programFilename = "remote.rxe"++data RemoteCommandType =+    MoveFor OutputPower TachoLimit -- ^ Move specified motors for a given number of degrees at a given speed.+  | SetTo OutputPower TachoCount -- ^ Set specified motors' position to a given offset in degrees from a zero position (it is assumed+                                 -- that motors are at zero position at @remote.rxe@ program start, you can use 'resetMotorPosition'+                                 -- to assure that, but probably not needed as the NXT brick resets things at program start) at a given speed.+                                 -- Probably not a good idea to mix 'SetTo' and 'MoveFor' on the same motor as 'MoveFor' resets+                                 -- position.+  deriving (Eq, Ord, Read, Show)+data RemoteCommand =+    RemoteCommand [OutputPort] RemoteCommandType -- ^ Data type of remote command for specified output port(s).+  deriving (Eq, Ord, Read, Show)++{-|+Sends a command to the @remote.rxe@ program. Commands can be interrupted (or supplemented, for other motors) immediately by a next command.+-}+sendRemoteCommand :: RemoteCommand -> NXT ()+sendRemoteCommand = messageWrite Inbox1 . encodeRemoteCommand++encodeRemoteCommand :: RemoteCommand -> String+encodeRemoteCommand (RemoteCommand ports command) =+  case command of+    (MoveFor power limit) | (-100) <= power && power <= 100 && 0 <= limit && limit <= 999999 -> "6" ++ ports' ++ power' ++ limit'+                          | otherwise                                                        -> throw $ PatternMatchFail "encodeRemoteCommand"+                              where power' = printf "%03d" $ power + 100+                                    limit' = printf "%06d" limit+    (SetTo power count) | 0 <= power && power <= 100 && (-99999) <= count && count <= 99999 -> "7" ++ ports' ++ power' ++ count'+                        | otherwise                                                         -> throw $ PatternMatchFail "encodeRemoteCommand"+                            where power' = printf "%03d" $ power + 100+                                  count' = printf "%+06d" count+  where ports' = show . sum . zipWith (*) [1, 2, 4] . map (fromEnum . flip elem ports) $ [A ..]++{-|+Starts @remote.rxe@ program on the NXT brick. Program has to be uploaded/available on the NXT brick.+-}+startRemoteProgram :: NXT ()+startRemoteProgram = ensureStartProgram programFilename++{-|+Stops current running program on the NXT brick. Probably this means @remote.rxe@ program on the NXT brick.+-}+stopRemoteProgram :: NXT ()+stopRemoteProgram = stopProgram
+ lib/Robotics/NXT/Sensor/Compass.hs view
@@ -0,0 +1,113 @@+{-|+This module defines an interface to @CMPS-Nx@ digital compass sensor from <http://www.mindsensors.com/> according to+@CMPS-Nx-V20-User-Guide.pdf@ documentation.+-}++module Robotics.NXT.Sensor.Compass (+  -- * Initialization+  csInit,+  -- * Measurement+  csGetMeasurement,+  -- * Configuration+  csSetMode,+  Mode(..),+  -- * Constants+  csGetVersion,+  csGetVendorID,+  csGetDeviceID+) where++import Control.Monad++import Robotics.NXT+import Robotics.NXT.Data++data Mode =+    AutoTrigOn -- ^ AutoTrig (continuous measuring) on.+  | AutoTrigOff -- ^ AutoTrig (continuous measuring) off.+  | ResultByte -- ^ Result is a byte mapped to [0-255).+  | ResultInteger -- ^ Result is an integer mapped to [0-3600).+  | Frequency50 -- ^ Sampling frequency 50 Hz (Europe standard).+  | Frequency60 -- ^ Sampling frequency 60 Hz (USA standard).+{-+ADPA (Auto Detecting Parallel Architecture) mode allows multiple sensors to be connected on the same bus (port). This mode is not+available for default address of 0x02 which is used by this module.+  | ADPAOn -- ^ Auto Detecting Parallel Architecture on.+  | ADPAOff -- ^ Auto Detecting Parallel Architecture off.+-}+  | BeginCalibration -- ^ Begin calibration mode.+  | EndCalibration -- ^ End calibration mode.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)++deviceAddress :: DeviceAddress+deviceAddress = 0x02++{-|+Initializes sensor on the given input port. It sets 'Mode' to 'ResultInteger'.+-}+csInit :: InputPort -> NXT ()+csInit input = do+  setInputModeConfirm input Lowspeed9V RawMode+  ready <- lowspeedGetStatus input+  when (ready > 0) $ lowspeedRead input >> return () -- clears any possible pending data in the buffer+  csSetMode input ResultInteger++csReadString :: InputPort -> Command -> RxDataLength -> NXT String+csReadString input command size = do+  lowspeedWrite input size [deviceAddress, command]+  s <- lowspeedRead input+  return $ dataToString0 s++-- Constants++{-|+Reads software version string (@V2.00@).+-}+csGetVersion :: InputPort -> NXT String+csGetVersion input = csReadString input 0x00 8++{-|+Reads vendor ID string (@mndsnsrs@).+-}+csGetVendorID :: InputPort -> NXT String+csGetVendorID input = csReadString input 0x08 8++{-|+Reads device ID string (@CMPS@).+-}+csGetDeviceID :: InputPort -> NXT String+csGetDeviceID input = csReadString input 0x10 8++-- Configuration++{-|+Sets current mode of operation.+-}+csSetMode :: InputPort -> Mode -> NXT ()+csSetMode input mode =+  lowspeedWriteConfirm input 0 $ [deviceAddress, 0x41] ++ command+    where command = case mode of+                      AutoTrigOn       -> [0x41, 0x02]+                      AutoTrigOff      -> [0x53, 0x01]+                      ResultByte       -> [0x42]+                      ResultInteger    -> [0x49]+                      Frequency50      -> [0x45]+                      Frequency60      -> [0x55]+{-+Currently not used.+                      ADPAOn           -> [0x4E]+                      ADPAOff          -> [0x4F]+-}+                      BeginCalibration -> [0x43]+                      EndCalibration   -> [0x44]++-- Measurement++{-|+Gets last measurement. Based on current 'Mode' it return value in [0-255) ('ResultByte' mode) or [0-3600) ('ResultInteger' mode) range.+-}+csGetMeasurement :: InputPort -> NXT Measurement+csGetMeasurement input = do+  lowspeedWrite input 2 [deviceAddress, 0x42]+  angle <- lowspeedRead input+  return $ fromUWord angle
+ lib/Robotics/NXT/Sensor/Ultrasonic.hs view
@@ -0,0 +1,238 @@+{-|+This module defines an interface to a digital ultrasonic sensor of the NXT kit.++I2C communication with ultrasonics sensor is described in Lego Mindstorms NXT Hardware Developer Kit,+Appendix 7 - Ultrasonic sensor I2C communication protocol.+-}++module Robotics.NXT.Sensor.Ultrasonic (+  -- * Initialization+  usInit,+  -- * Measurement+  usGetMeasurement,+  usGetAllMeasurements,+  -- * Configuration+  usSetMode,+  usGetMode,+  Mode(..),+  usSetContinuousInterval,+  usGetContinuousInterval,+  usSetActualZero,+  usGetActualZero,+  usSetActualScaleFactor,+  usGetActualScaleFactor,+  usSetActualScaleDivisor,+  usGetActualScaleDivisor,+  -- * Constants+  usGetVersion,+  usGetVendorID,+  usGetDeviceID,+  usGetFactoryZero,+  usGetFactoryScaleFactor,+  usGetFactoryScaleDivisor,+  usGetMeasurementUnits,+  -- * Types+  Zero,+  ScaleFactor,+  ScaleDivisor,+  ContinuousInterval,+  MeasurementNumber+) where++import Control.Applicative+import Control.Exception+import Control.Monad+import Control.Monad.Trans+import Data.Maybe++import Robotics.NXT+import Robotics.NXT.Data++-- Specification is vague whether zero, factor and divisor values are signed or unsigned++-- | Type of a zero value.+type Zero = Int+-- | Type of a scale factor value.+type ScaleFactor = Int+-- | Type of a scale divisor value.+type ScaleDivisor = Int+{-|+Type of a continuous measurement interval value. This seems to be in the range 1-15.+-}+type ContinuousInterval = Int+{-|+Type of a measurement number. Sensor stores measurements (distances) for the first 8 echoes (numbered 0-7) it receives+in 'SingleShot' mode. For 'ContinuousMeasurement' and 'EventCapture' modes use first (0) measurement (distance) number.+-}+type MeasurementNumber = Int++data Mode =+    Off -- ^ Turns sensor off.+  | SingleShot -- ^ In this mode ultrasonic sensor only makes a new measurement every time this mode is set.+               -- The sensor measures distances for up to 8 objects (8 first echoes) which can be retrieved with 'usGetMeasurement'.+  | ContinuousMeasurement -- ^ In this mode the sensor continuously makes new measurement with the specific interval. This is the default mode. +  | EventCapture -- ^ Within this mode the sensor will measure whether any other ultrasonic sensors are within the vicinity.+                 -- With this information a program can evaluate when it is best to make a new measurement which will not conflict with other ultrasonic sensors.+  | WarmReset -- ^ Requests warm reset.+  deriving (Bounded, Enum, Eq, Ord, Read, Show)++deviceAddress :: DeviceAddress+deviceAddress = 0x02++{-|+Initializes sensor on the given input port. Default is 'ContinuousMeasurement' mode.+-}+usInit :: InputPort -> NXT ()+usInit input = do+  setInputModeConfirm input Lowspeed9V RawMode+  ready <- lowspeedGetStatus input+  when (ready > 0) $ lowspeedRead input >> return () -- clears any possible pending data in the buffer+  usSetMode input WarmReset++usReadByte :: Integral a => InputPort -> Command -> NXT a+usReadByte input command = do+  lowspeedWrite input 1 [deviceAddress, command]+  b <- lowspeedRead input+  return $ fromUByte . take 1 $ b++usReadString :: InputPort -> Command -> RxDataLength -> NXT String+usReadString input command size = do+  lowspeedWrite input size [deviceAddress, command]+  s <- lowspeedRead input+  return $ dataToString0 s++-- Constants++{-|+Reads software version string (@V1.0@).+-}+usGetVersion :: InputPort -> NXT String+usGetVersion input = usReadString input 0x00 8++{-|+Reads vendor ID string (@LEGO@).+-}+usGetVendorID :: InputPort -> NXT String+usGetVendorID input = usReadString input 0x08 8++{-|+Reads vendor ID string (@Sonar@).+-}+usGetDeviceID :: InputPort -> NXT String+usGetDeviceID input = usReadString input 0x10 8++{-|+Reads factory zero value.+-}+usGetFactoryZero :: InputPort -> NXT Zero+usGetFactoryZero input = usReadByte input 0x11++{-|+Reads factory scale factor value.+-}+usGetFactoryScaleFactor :: InputPort -> NXT ScaleFactor+usGetFactoryScaleFactor input = usReadByte input 0x12++{-|+Reads factory scale divisor value.+-}+usGetFactoryScaleDivisor :: InputPort -> NXT ScaleDivisor+usGetFactoryScaleDivisor input = usReadByte input 0x13++{-|+Reads measurement units string (@10E-2m@, a centimeter).+-}+usGetMeasurementUnits :: InputPort -> NXT String+usGetMeasurementUnits input = usReadString input 0x14 7++-- Configuration++{-|+Gets current mode of operation.+-}+usGetMode :: InputPort -> NXT Mode+usGetMode input = do+  mode <- usReadByte input 0x41 :: NXT Int+  case mode of+    0x00 -> return Off+    0x01 -> return SingleShot+    0x02 -> return ContinuousMeasurement+    0x03 -> return EventCapture+    0x04 -> return WarmReset+    _    -> liftIO . throwIO $ PatternMatchFail "usGetMode"++{-|+Gets current continuous measurement interval value.+-}+usGetContinuousInterval :: InputPort -> NXT ContinuousInterval+usGetContinuousInterval input = usReadByte input 0x40++{-|+Gets current (actual) zero value.+-}+usGetActualZero :: InputPort -> NXT Zero+usGetActualZero input = usReadByte input 0x50++{-|+Gets current (actual) scale factor value.+-}+usGetActualScaleFactor :: InputPort -> NXT ScaleFactor+usGetActualScaleFactor input = usReadByte input 0x51++{-|+Gets current (actual) scale divisor value.+-}+usGetActualScaleDivisor :: InputPort -> NXT ScaleDivisor+usGetActualScaleDivisor input = usReadByte input 0x52++{-|+Sets current mode of operation.+-}+usSetMode :: InputPort -> Mode -> NXT ()+usSetMode input mode = lowspeedWriteConfirm input 0 [deviceAddress, 0x41, fromIntegral . fromEnum $ mode]++{-|+Sets current continuous measurement interval value.+-}+usSetContinuousInterval :: InputPort -> ContinuousInterval -> NXT ()+usSetContinuousInterval input interval = lowspeedWrite input 0 $ [deviceAddress, 0x40] ++ toUByte interval++{-|+Sets current (actual) zero value. This is used to calibrate sensor.+-}+usSetActualZero :: InputPort -> Zero -> NXT ()+usSetActualZero input zero = lowspeedWrite input 0 $ [deviceAddress, 0x50] ++ toUByte zero++{-|+Sets current (actual) scale factor value. This is used to calibrate sensor.+-}+usSetActualScaleFactor :: InputPort -> ScaleFactor -> NXT ()+usSetActualScaleFactor input factor = lowspeedWrite input 0 $ [deviceAddress, 0x51] ++ toUByte factor++{-|+Sets current (actual) scale divisor value. This is used to calibrate sensor.+-}+usSetActualScaleDivisor :: InputPort -> ScaleDivisor -> NXT ()+usSetActualScaleDivisor input divisor = lowspeedWrite input 0 $ [deviceAddress, 0x52] ++ toUByte divisor++-- Measurement++{-|+Gets last measurement for a given measurement number based on the current mode. To retrieve new 'SingleShot' measurements first+use 'usSetMode' (with 'SingleShot' as a parameter) to send new ultrasonic ping and after approximately 20ms read the results. (Change+of NXT Bluetooth communication direction takes around 30 ms.) In 'ContinuousMeasurement' mode new measurements are made automatically+based on the continuous measurement interval value.+-}+usGetMeasurement :: InputPort -> MeasurementNumber -> NXT (Maybe Measurement)+usGetMeasurement input number | number >= 0 && number < 8 = do measurement <- usReadByte input $ 0x42 + fromIntegral number+                                                               if measurement == 0xFF+                                                                 then return Nothing+                                                                 else return $ Just measurement+                              | otherwise                 = liftIO . throwIO $ PatternMatchFail "usGetMeasurement"++{-|+Helper function which gets all measurements available in order (closer first).+-}+usGetAllMeasurements :: InputPort -> NXT [Measurement]+usGetAllMeasurements input = mapMaybeM (usGetMeasurement input) [0..7]+  where mapMaybeM f as = catMaybes <$> (mapM f as)
+ lib/Robotics/NXT/Types.hs view
@@ -0,0 +1,9 @@+module Robotics.NXT.Types (+  -- * Types+  NXT,+  module Robotics.NXT.Externals,+  NXTInternals+) where++import Robotics.NXT.Internals+import Robotics.NXT.Externals
+ remote/remote.nxc view
@@ -0,0 +1,137 @@+#define DEBUG 0
+
+#define INBOX 1
+
+#define MOVE_FOR 6
+#define SET_TO 7
+
+#define OUTPUT_A 1
+#define OUTPUT_B 2
+#define OUTPUT_C 4
+
+#if DEBUG
+inline string portsToString(unsigned char &ports) {
+	string s = "";
+	if (ports & OUTPUT_A) strcat(s, "A");
+	if (ports & OUTPUT_B) strcat(s, "B");
+	if (ports & OUTPUT_C) strcat(s, "C");
+	return s;
+}
+#endif
+
+inline void reset(const byte port) {
+	SetOutput(port, UpdateFlags, UF_UPDATE_RESET_COUNT | UF_UPDATE_RESET_BLOCK_COUNT | UF_UPDATE_RESET_ROTATION_COUNT);
+}
+
+inline void output_reset(const byte port, char &power, unsigned long &limit) {
+	SetOutput(port, Power, power, TachoLimit, limit, OutputMode, OUT_MODE_MOTORON | OUT_MODE_BRAKE, RegMode, OUT_REGMODE_IDLE, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE | UF_UPDATE_SPEED | UF_UPDATE_TACHO_LIMIT | UF_UPDATE_RESET_BLOCK_COUNT | UF_UPDATE_RESET_ROTATION_COUNT);
+}
+
+inline void output_reset_all(const byte port, char &power, unsigned long &limit) {
+	SetOutput(port, Power, power, TachoLimit, limit, OutputMode, OUT_MODE_MOTORON | OUT_MODE_BRAKE, RegMode, OUT_REGMODE_IDLE, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE | UF_UPDATE_SPEED | UF_UPDATE_TACHO_LIMIT | UF_UPDATE_RESET_BLOCK_COUNT | UF_UPDATE_RESET_ROTATION_COUNT | UF_UPDATE_RESET_COUNT);
+}
+
+inline void output_absolute(const byte port, char &power, long &count, long &old_count) {
+	long target = count - old_count;
+	unsigned long limit = abs(target);	
+	power *= sign(target);
+	old_count = count;
+	output_reset(port, power, limit);	
+}
+
+task main() {
+	unsigned char command, ports;
+	char power;
+	long old_count = 0; // TODO: Should we check this? Reset all counters at the beginning? Set it to tacho count?
+	
+	RandomNumberType random;
+	KeepAliveType keepalive;
+
+	MessageReadType args;
+	args.QueueID = INBOX;
+	args.Remove = true;
+	while (true) {
+		#if DEBUG
+			TextOut(0, LCD_LINE6, "              ");
+			TextOut(0, LCD_LINE6, StrCat("A ", NumToStr(MotorTachoCount(OUT_A))));
+			TextOut(0, LCD_LINE7, "              ");
+			TextOut(0, LCD_LINE7, StrCat("B ", NumToStr(MotorTachoCount(OUT_B))));
+			TextOut(0, LCD_LINE8, "              ");
+			TextOut(0, LCD_LINE8, StrCat("C ", NumToStr(MotorTachoCount(OUT_C))));
+		#endif
+	
+		SysMessageRead(args);
+		if (args.Result != NO_ERR) continue;
+		
+		command = StrToNum(MidStr(args.Message, 0, 1));
+		ports = StrToNum(MidStr(args.Message, 1, 1));
+		power = StrToNum(MidStr(args.Message, 2, 3)) - 100;
+		
+		if (command == MOVE_FOR) {
+			unsigned long limit = StrToNum(MidStr(args.Message, 5, 6));
+
+			#if DEBUG
+				TextOut(0, LCD_LINE1, "              ");
+				TextOut(0, LCD_LINE1, "MOVE_FOR");
+				TextOut(0, LCD_LINE2, "              ");
+				TextOut(0, LCD_LINE2, portsToString(ports));
+				TextOut(0, LCD_LINE3, "              ");
+				TextOut(0, LCD_LINE3, NumToStr(power));
+				TextOut(0, LCD_LINE4, "              ");
+				TextOut(0, LCD_LINE4, NumToStr(limit));
+			#endif
+
+			SysRandomNumber(random);
+			
+			// We randomly select order so that possible latencies are averaged out
+			if (random.Result & 1) { // Tests one bit for two choices
+				if (ports & OUTPUT_A) reset(OUT_A);
+				if (ports & OUTPUT_B) reset(OUT_B);
+				if (ports & OUTPUT_C) reset(OUT_C);
+
+				if (ports & OUTPUT_A) output_reset_all(OUT_A, power, limit);
+				if (ports & OUTPUT_B) output_reset_all(OUT_B, power, limit);
+				if (ports & OUTPUT_C) output_reset_all(OUT_C, power, limit);
+			}
+			else {
+				if (ports & OUTPUT_C) reset(OUT_C);
+				if (ports & OUTPUT_B) reset(OUT_B);
+				if (ports & OUTPUT_A) reset(OUT_A);
+				
+				if (ports & OUTPUT_C) output_reset_all(OUT_C, power, limit);
+				if (ports & OUTPUT_B) output_reset_all(OUT_B, power, limit);
+				if (ports & OUTPUT_A) output_reset_all(OUT_A, power, limit);
+			}
+		}
+		else if (command == SET_TO) {
+			long count = StrToNum(MidStr(args.Message, 5, 6));
+			
+			#if DEBUG
+				TextOut(0, LCD_LINE1, "              ");
+				TextOut(0, LCD_LINE1, "SET_TO");
+				TextOut(0, LCD_LINE2, "              ");
+				TextOut(0, LCD_LINE2, portsToString(ports));
+				TextOut(0, LCD_LINE3, "              ");
+				TextOut(0, LCD_LINE3, NumToStr(power));
+				TextOut(0, LCD_LINE4, "              ");
+				TextOut(0, LCD_LINE4, NumToStr(count));
+			#endif
+			
+			SysRandomNumber(random);
+			
+			// We randomly select order so that possible latencies are averaged out
+			if (random.Result & 1) { // Tests one bit for two choices
+				if (ports & OUTPUT_A) output_absolute(OUT_A, power, count, old_count);
+				if (ports & OUTPUT_B) output_absolute(OUT_B, power, count, old_count);
+				if (ports & OUTPUT_C) output_absolute(OUT_C, power, count, old_count);
+			}
+			else {
+				if (ports & OUTPUT_C) output_absolute(OUT_C, power, count, old_count);
+				if (ports & OUTPUT_B) output_absolute(OUT_B, power, count, old_count);
+				if (ports & OUTPUT_A) output_absolute(OUT_A, power, count, old_count);
+			}
+		}
+		
+		SysKeepAlive(keepalive);
+	}
+}
+ remote/remote.rxe view

binary file changed (absent → 5102 bytes)

+ src/Shutdown.hs view
@@ -0,0 +1,50 @@+module Main (+  main+) where++import Control.Monad.State+import Data.Maybe+import Data.List+import System.Console.GetOpt+import System.Environment+import System.Exit+import System.IO++import Robotics.NXT++data Option = Help | Device FilePath deriving (Eq, Show)++isDevice :: Option -> Bool+isDevice (Device _) = True+isDevice _          = False++options :: [OptDescr Option]+options = [+    Option "h" ["help"] (NoArg Help) "show this help",+    Option "d" ["device"] (ReqArg Device "filename") "serial port device"+  ]++main :: IO ()+main = do+  programName <- getProgName+  let header = programName ++ " [option ...]" ++ "\n\nOptions:"+      usage  = "Usage:\n" ++ usageInfo header options+  +  args <- getArgs+  opts <- case getOpt Permute options args of+                 (o, [], [])  -> return o+                 (_, _, [])   -> do+                   hPutStrLn stderr $ "Error(s):\n" ++ "excess argument(s)\n\n" ++ usage+                   exitWith $ ExitFailure 1+                 (_, _, errs) -> do+                   hPutStrLn stderr $ "Error(s):\n" ++ concat errs ++ "\n" ++ usage+                   exitWith $ ExitFailure 1+  +  when (Help `elem` opts) $ do+    putStrLn "Remotely shutdowns a NXT brick.\n"+    putStrLn usage+    exitWith ExitSuccess+  +  let Device device = fromMaybe (Device defaultDevice) . find isDevice $ opts+  +  withNXT device shutdown
+ src/UploadFiles.hs view
@@ -0,0 +1,72 @@+module Main (+  main+) where++import Control.Applicative+import Control.Monad.State+import qualified Data.ByteString.Lazy as B+import Data.Maybe+import Data.List+import System.Console.GetOpt+import System.Environment+import System.Exit+import System.IO+import System.FilePath++import Robotics.NXT++data Option = Help | Device FilePath deriving (Eq, Show)++isDevice :: Option -> Bool+isDevice (Device _) = True+isDevice _          = False++options :: [OptDescr Option]+options = [+    Option "h" ["help"] (NoArg Help) "show this help",+    Option "d" ["device"] (ReqArg Device "filename") "serial port device"+  ]++main :: IO ()+main = do+  programName <- getProgName+  let header = programName ++ " [option ...] <file ...>" ++ "\n\nOptions:"+      usage  = "Usage:\n" ++ usageInfo header options+  +  args <- getArgs+  (opts, files) <- case getOpt Permute options args of+                     (o, fs, [])  -> return (o, fs)+                     (_, _, errs) -> do+                       hPutStrLn stderr $ "Error(s):\n" ++ concat errs ++ "\n" ++ usage+                       exitWith $ ExitFailure 1+  +  when (Help `elem` opts) $ do+    putStrLn "Uploads files to a NXT brick.\n"+    putStrLn usage+    exitWith ExitSuccess+  +  when (null files) $ do+    hPutStrLn stderr $ "Error(s):\nno files to upload specified\n" ++ "\n" ++ usage+    exitWith $ ExitFailure 1+  +  let Device device = fromMaybe (Device defaultDevice) . find isDevice $ opts+  +  withNXT device (uploadFiles files)++uploadFiles :: [String] -> NXT ()+uploadFiles args = do+  stopProgramConfirm+  mapM_ uploadFile args+    where uploadFile file = do+            liftIO $ putStrLn $ "Uploading " ++ file+            h <- liftIO $ openBinaryFile file ReadMode+            size <- liftIO $ hFileSize h+            content <- liftIO $ B.unpack <$> B.hGetContents h+            let filename = takeFileName file+            deleteConfirm filename+            h' <- openWrite filename (fromIntegral size)+            mapM_ (write h') $ chunk 61 content+            close h'+          chunk _ [] = [[]]+          chunk n xs = y1 : chunk n y2+            where (y1, y2) = splitAt n xs