[nxtlisp-cvs] r2 - nxtlisp

[mcermak at common-lisp.net]

Author: mcermak
Date: Mon Aug 20 16:05:17 2007
New Revision: 2

Modified:
   nxtlisp/remote-commands.lisp
Log:
set-sensor-state and sensor remote commands done but not tested yet


Modified: nxtlisp/remote-commands.lisp
==============================================================================
--- nxtlisp/remote-commands.lisp	(original)
+++ nxtlisp/remote-commands.lisp	Mon Aug 20 16:05:17 2007
@@ -57,10 +57,10 @@
 ;;       change-nxt-id (a wrapper around set-brick-name-command)
 ;;       set-sensor-state (uses set-input-mode direct command and
 ;;                         reset-input-scaled-value if clear flag is set)
-;;       sensor - done just partly, consult next steps with Dr.K
+;;       sensor (type and mode are possible &optional arguments, maybe not the
+;;               best solution)
                            
 
-
 ;; differences between RCX and NXT:
 ;; play-system-sound-file plays a sound file in NXT's memory, possibly in a loop
 ;; start-nxt-program takes and string as an program name; rcx requires a number
@@ -74,6 +74,53 @@
 (EXPORT '(var set-var message send-message sensor effector set-effector-state 
           read-rcx-executable))
 
+(defvar *last-sensor-type* nil
+  "If sensor is called with no modifier arguments, *last-sensor-type* is checked
+to obtain sensor type. It also maintains backward compatibility with RCXLisp.")
+
+(defvar *last-sensor-mode* nil
+  "If sensor is called with no modifier arguments, *last-sensor-mode* is checked
+to obtain sensor mode. It also maintains backward compatibility with RCXLisp.")
+
+(defun update-last-sensor-type (port sensor-type)
+  "Updates the *last-sensor-type* variable according to last call of set-sensor-state."
+  (if (or (eql *last-sensor-type* nil) ; port has not been used yet (not in list)
+	  (not (let ((used nil)) ; there's gotta be a better way...
+		 (dolist (i *last-sensor-type*)
+		   (if (eql (car i) port)
+		       (setf used t)))
+		 used)))
+      (push (list port sensor-type) *last-sensor-type*) 
+      (loop for element in *last-sensor-type*           ; port has been used, update
+	    when (eql (car element) port)
+	    return (setf (cdr element) sensor-type))))
+
+(defun update-last-sensor-mode (port sensor-mode)
+  "Updates the *last-sensor-mode* variable according to last call of set-sensor-state."
+  (if (or (eql *last-sensor-mode* nil) ; port has not been used yet (not in list) 
+	  (not (let ((used nil)) 
+		 (dolist (i *last-sensor-mode*)
+		   (if (eql (car i) port)
+		       (setf used t)))
+		 used)))
+      (push (list port sensor-mode) *last-sensor-mode*) 
+      (loop for element in *last-sensor-mode*           ; port has been used, update
+	    when (eql (car element) port)
+	    return (setf (cdr element) sensor-mode))))
+
+(defun extract-type (port)
+  "Returns the last set type of sensor on port."
+  (loop for element in *last-sensor-type*
+	when (eql (car element) port)
+	return (cdr element)))
+
+(defun extract-mode (port)
+  "Returns the last set mode of sensor on port."
+  (loop for element in *last-sensor-mode*
+	when (eql (car element) port)
+	return (cdr element)))
+
+
 (defun play-tone (frequency duration &optional (stream *standard-nxt-io*))
   "Plays a tone on the NXT. Frequency is in Hz (200-14000), duration in ms."
   (direct-play-tone frequency duration :stream stream))
@@ -147,12 +194,45 @@
 		       (:slope-mask #x1F)
 		       (:mode-mask #xE0)))) ; has the same value as :angle-steps in docs (?)
     (set-input-mode port sensor-type sensor-mode :stream stream)
-    (if clear (reset-input-scaled-value port)))) ; if the clear flag is set, reset the counter
+    (if clear (reset-input-scaled-value port)) ; if the clear flag is set, reset the counter
+    ;; see the manual or documentation in the source code for description
+    (update-last-sensor-type port sensor-type)
+    (update-last-sensor-mode port sensor-mode)))
+
 
-(defun sensor (port &optional (stream *standard-nxt-io*))
+;; TODO: should the type/mode handling be this way? rethink
+(defun sensor (port &optional (type :raw type-supplied-p)
+	       (mode :raw mode-supplied-p) (stream *standard-nxt-io*))
   "Retrieves values from the sensor plugged in <port> port."
-  ;; TODO: handle the return value (consult with Dr. K)
-  (get-input-values port))
+  (let ((raw-packet (get-input-values port))
+	;; if type or mode was not supplied, use values set
+	;; in last call to set-sensor-state
+	(sensor-type (if type-supplied-p
+			 type
+			 (extract-type port)))
+	(sensor-mode (if mode-supplied-p
+			 mode
+			 (extract-mode port))))
+    ;; let's decode the packet according to type/mode
+    (if (not (aref raw-packet 1))
+	(warn "sensor: Packet marked as not valid."))
+    ;; sensor-type has higher priority to sensor-mode
+    (case sensor-type
+      (:angle (extract-number (aref raw-packet 9)
+			      (aref raw-packet 10)
+			      :sword))
+      ((or :light-active
+	   :light-inactive) (extract-number (aref raw-packet 7)
+					    (aref raw-packet 8)
+					    :uword))
+      (t (case sensor-mode
+	   ((or :boolean :transition-cnt :period-counter
+		:celsius :fahrenheit) (extract-number (aref raw-packet 9)
+						      (aref raw-packet 10)
+						      :sword))
+	   (t (extract-number (aref raw-packet 5)
+			      (aref raw-packet 6)
+			      :uword)))))))
 
 (defun effector (effector feature &optional (stream *standard-nxt-io*))
   "Returns the current value of <feature> for <effector>."
@@ -163,3 +243,10 @@
 (defun set-effector-state (effector feature value &optional (stream *standard-nxt-io*))
   "TODO: document")
 
+(defun extract-number (byte-1 byte-2 mode)
+  "Return a number constructed from 2 bytes, either a signed word or unsigned word."
+  (case mode
+    (:uword (+ byte-1 (* byte-2 256)))
+    (:sword (if (< byte-2 127) ; possitive?
+		(+ byte-1 (* byte-2 256))
+		(1+ (lognot (+ byte-1 (* byte-2 256))))))))
\ No newline at end of file