Skip to content
Browse files

Remote NXT control with messages.

  • Loading branch information...
1 parent ee55678 commit aec02f1a14019f2e96e82721eee6049d9ea3de3d @mitar committed Sep 18, 2010
Showing with 208 additions and 13 deletions.
  1. +11 −1 lib/NXT.hs
  2. +41 −0 lib/Remote.hs
  3. +3 −0 lib/Types.hs
  4. +137 −0 remote/remote.nxc
  5. +16 −12 src/UploadFile.hs
View
12 lib/NXT.hs
@@ -440,6 +440,14 @@ getBatteryLevel = do
_:_:e:_ -> failNXT "getBatteryLevel" e
_ -> fail "getBatteryLevel"
+-- Is battery rechargeable?
+isBatteryRechargeable :: NXT Bool
+isBatteryRechargeable = do
+ when debug $ io . hPutStrLn stderr $ "isbatteryrechargeable"
+ mid <- getModuleID "Ui.mod"
+ r <- readIOMap (fromJust mid) 35 1
+ return $ (/=) 0 (head r)
+
-- Stops sound playback
stopSoundPlayback :: NXT ()
stopSoundPlayback = stopSoundPlayback' False
@@ -576,15 +584,17 @@ messageRead inbox remove = do
-- Stops all NXT activities: stops motors and disables sensors
stopEverything :: NXT ()
stopEverything = do
+ when debug $ io . 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
shutdown :: NXT ()
shutdown = do
+ when debug $ io . hPutStrLn stderr $ "shutdown"
mid <- getModuleID "IOCtrl.mod"
- writeIOMap (fromJust mid) 0 [0x00, 0x5a]
+ writeIOMap (fromJust mid) 0 [0x5A, 0x00]
-- Opens a file for writing a linked list of flash sectors
openWrite :: FileName -> FileSize -> NXT FileHandle
View
41 lib/Remote.hs
@@ -0,0 +1,41 @@
+module NXT.Remote where
+
+import Control.Exception
+import Text.Printf
+
+import NXT.NXT
+import NXT.Types
+
+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 ..]
+
+startRemoteProgram :: NXT ()
+startRemoteProgram = do
+ stopProgram
+ startProgramConfirm "remote.rxe"
+
+stopRemoteProgram :: NXT ()
+stopRemoteProgram = stopProgram
+
+-- Sends a message to MotorControl program on NXT
+-- Be careful to obey required pauses between commands (threadDelay is your friend)
+motorControlSend :: String -> NXT ()
+motorControlSend = messageWrite Inbox1
+
+-- Reads a message from MotorControl program on NXT
+-- Be careful to obey required pauses between commands (threadDelay is your friend)
+motorControlReceive :: NXT String
+motorControlReceive = messageRead RemoteInbox0 True
View
3 lib/Types.hs
@@ -110,3 +110,6 @@ data ModuleInfo = ModuleInfo ModuleName ModuleID ModuleSize ModuleIOMapSize deri
type IOMapOffset = Int -- unsigned word
type IOMapLength = Int -- unsigned word
type IOMapData = [Word8]
+
+data RemoteCommandType = MoveFor OutputPower TachoLimit | SetTo OutputPower TachoCount deriving (Eq, Show)
+data RemoteCommand = RemoteCommand [OutputPort] RemoteCommandType deriving (Eq, Show)
View
137 remote/remote.nxc
@@ -0,0 +1,137 @@
+#define DEBUG 1
+
+#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?
+
+ 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) { // Test 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) { // Test 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);
+ }
+}
View
28 src/UploadFile.hs
@@ -16,15 +16,19 @@ upload = do
bracket initialize terminate (evalStateT (uploadFiles args))
uploadFiles :: [String] -> NXT ()
-uploadFiles args = mapM_ uploadFile args
- where uploadFile file = do
- io $ putStrLn $ "Uploading " ++ file
- h <- io $ openBinaryFile file ReadMode
- size <- io $ hFileSize h
- content <- io $ hGetContents h
- h' <- openWrite (takeFileName file) (fromIntegral size)
- mapM_ (write h' . stringToData) $ chunk 61 content
- close h'
- chunk _ [] = [[]]
- chunk n xs = y1 : chunk n y2
- where (y1, y2) = splitAt n xs
+uploadFiles args = do
+ stopProgram
+ mapM_ uploadFile args
+ where uploadFile file = do
+ io $ putStrLn $ "Uploading " ++ file
+ h <- io $ openBinaryFile file ReadMode
+ size <- io $ hFileSize h
+ content <- io $ hGetContents h
+ let filename = takeFileName file
+ delete filename
+ h' <- openWrite filename (fromIntegral size)
+ mapM_ (write h' . stringToData) $ chunk 61 content
+ close h'
+ chunk _ [] = [[]]
+ chunk n xs = y1 : chunk n y2
+ where (y1, y2) = splitAt n xs

0 comments on commit aec02f1

Please sign in to comment.
Something went wrong with that request. Please try again.