aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authormarcusw2011-02-17 23:11:38 +0000
committermarcusw2011-02-17 23:11:38 +0000
commitc8c17470a0fc0211628a2b5b87c82a86990e4525 (patch)
tree4a916d737a0d9982becd330a671f013396c9e1ca
parent8a5a3277494d887d5d9dccb5ab72ed56a6fb0ba8 (diff)
Preliminary support for Linus Atorf's MotorControl NXC interface.
-rw-r--r--MANIFEST.in1
-rw-r--r--nxt/brick.py2
-rw-r--r--nxt/motcont.py133
3 files changed, 136 insertions, 0 deletions
diff --git a/MANIFEST.in b/MANIFEST.in
index 9dd36e1..ac4829a 100644
--- a/MANIFEST.in
+++ b/MANIFEST.in
@@ -1,4 +1,5 @@
include docs/*.html *.url *.css *.txt
include README LICENSE install.bat MANIFEST.in
include examples/*.py
+include motcont/*
exclude arduino/*
diff --git a/nxt/brick.py b/nxt/brick.py
index 740ccbd..ece5a7a 100644
--- a/nxt/brick.py
+++ b/nxt/brick.py
@@ -18,6 +18,7 @@ from threading import Lock
from .error import FileNotFound, ModuleNotFound
from .telegram import OPCODES, Telegram
from .sensor import get_sensor
+from .motcont import MotCont
def _make_poller(opcode, poll_func, parse_func):
def poll(self, *args, **kwargs):
@@ -211,6 +212,7 @@ class Brick(object): #TODO: this begs to have explicit methods
def __init__(self, sock):
self.sock = sock
self.lock = Lock()
+ self.mc = MotCont(self)
def play_tone_and_wait(self, frequency, duration):
self.play_tone(frequency, duration)
diff --git a/nxt/motcont.py b/nxt/motcont.py
new file mode 100644
index 0000000..1457e70
--- /dev/null
+++ b/nxt/motcont.py
@@ -0,0 +1,133 @@
+# nxt.motcont module -- Interface to Linus Atorf's MotorControl NXC
+# Copyright (C) 2011 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+import nxt, nxt.error, time
+from threading import Lock
+
+class MotorConError(nxt.error.ProtocolError):
+ pass
+
+def _power(power):
+ pw = abs(power)
+ psign = int(power >= 0) * 2 - 1
+ if psign == -1:
+ pw += 100
+ pw = str(pw)
+ pw = '0'*(3-len(pw))+pw #pad front with 0s to make 3 chars
+ return pw
+
+def _tacho(tacholimit):
+ tacho = str(tacholimit)
+ tacho = '0'*(6-len(tacho))+tacho #pad front with 0s to make 6 chars
+ return tacho
+
+def interval(delay, lastrun):
+ if lastrun+delay > time.time():
+ diff = time.time() - lastrun
+ time.sleep(0.010 - diff)
+
+class MotCont():
+ '''
+This class provides an interface to Linus Atorf's MotorControl NXC
+program. It is a wrapper which follows the documentation at
+http://www.mindstorms.rwth-aachen.de/trac/wiki/MotorControl
+and provides command strings and timing intervals as dictated there. To
+use this module, you will need to put MotorControl22.rxe on your NXT
+brick. This file and its corresponding source can be found at
+http://www.mindstorms.rwth-aachen.de/trac/browser/trunk/tools/MotorControl
+You can use nxt_push or any other nxt file manager to put the file on
+the NXT. Before using any of the functions here, use MotCont.start() to
+start the program. You can also start it manually my using the menu on
+the brick. When your script exits, it would be a good idea to do
+b.stop_program().
+'''
+ def __init__(self, brick):
+ self.brick = brick
+ self.is_ready_lock = Lock()
+ self.last_is_ready = time.time()-1
+ self.last_cmd = {}
+ #TODO: Timing code (15ms between cmds for different motors; 10ms for anything after is_ready)
+ #TODO: Check timing code
+
+ def cmd(self, port, power, tacholimit, speedreg=1, smoothstart=0, brake=0):
+ '''
+Sends a "CONTROLLED_MOTORCMD" to MotorControl. port is
+nxt.motor.PORT_[A-C], power is -100-100, tacholimit is 0-999999,
+speedreg is whether to try to maintain speeds under load, and brake is
+whether to enable active braking after the motor is in the specified
+place (DIFFERENT from the nxt.motor.turn() function's brake arg).'''
+ interval(0.010, self.last_is_ready)
+ if port in self.last_cmd:
+ interval(0.015, self.last_cmd[port])
+ mode = str(
+ 0x01*int(brake)+
+ 0x02*int(speedreg)+
+ 0x04*int(smoothstart)
+ )
+ command = '1'+str(port)+_power(power)+_tacho(tacholimit)+mode
+ self.brick.message_write(1, command)
+ self.last_cmd[port] = time.time()
+
+ def reset_tacho(self, port):
+ '''
+Sends a "RESET_ERROR_CORRECTION" to MotorControl, which causes it to
+reset the current tacho count for that motor.'''
+ interval(0.010, self.last_is_ready)
+ self.brick.message_write(1, '2'+str(port))
+ self.last_cmd[port] = time.time()
+
+ def is_ready(self, port):
+ '''
+Sends an "ISMOTORREADY" to MotorControl and returns the reply.'''
+ interval(0.010, self.last_is_ready)
+ with self.is_ready_lock:
+ self.brick.message_write(1, '3'+str(port))
+ time.sleep(0.015) #10ms pause from the docs seems to not be adequate
+ reply = self.brick.message_read(0, 1, 1)[1]
+ if reply[0] != str(port):
+ raise MotorConError, 'Wrong port returned from ISMOTORREADY'
+ self.last_is_ready = time.time()
+ return bool(int(reply[1]))
+
+ def set_output_state(self, port, power, tacholimit, speedreg=1):
+ '''
+Sends a "CLASSIC_MOTORCMD" to MotorControl. Brick is a brick object,
+port is nxt.motor.PORT_[A-C], power is -100-100, tacholimit is 0-999999,
+speedreg is whether to try to maintain speeds under load, and brake is
+whether to enable active braking after the motor is in the specified
+place (DIFFERENT from the nxt.motor.turn() function's brake arg).'''
+ interval(0.010, self.last_is_ready)
+ if port in self.last_cmd:
+ interval(0.015, self.last_cmd[port])
+ command = '4'+str(port)+_power(power)+_tacho(tacholimit)+str(speedreg)
+ self.brick.message_write(1, command)
+ self.last_cmd[port] = time.time()
+
+ def start(self, version=22):
+ '''
+Starts the MotorControl program on the brick. It needs to already be
+present on the brick's flash and named MotorControlXX.rxc, where XX is
+the version number passed as the version arg (default is whatever is
+bundled with this version of nxt-python).'''
+ try:
+ self.brick.stop_program()
+ except nxt.error.DirProtError:
+ pass
+ self.brick.start_program('MotorControl%d.rxe' % version)
+ time.sleep(0.1)
+
+ def stop(self):
+ '''
+Used to stop the MotorControl program. All this actually does is stop
+the currently running rxe.'''
+ self.brick.stop_program()