nxt-python-2.2.2/0000755000175000001440000000000011760013147014316 5ustar marcususers00000000000000nxt-python-2.2.2/MANIFEST.in0000644000175000001440000000022411565504062016056 0ustar marcususers00000000000000include docs/*.html *.url *.css *.txt include README LICENSE install.bat MANIFEST.in include examples/*.py include motcont/* exclude arduino/* nxt-python-2.2.2/scripts/0000755000175000001440000000000011760013147016005 5ustar marcususers00000000000000nxt-python-2.2.2/scripts/nxt_test0000644000175000001440000000175111760011324017577 0ustar marcususers00000000000000#!/usr/bin/env python #Lists various information from all bricks it can connect to. import sys, traceback if '--help' in sys.argv: print '''nxt_test -- Tests the nxt-python setup and brick firmware interaction Usage: nxt_test (takes no arguments except --help)''' exit(0) import nxt.locator import nxt.brick try: b = nxt.locator.find_one_brick() name, host, signal_strength, user_flash = b.get_device_info() print 'NXT brick name: %s' % name print 'Host address: %s' % host print 'Bluetooth signal strength: %s' % signal_strength print 'Free user flash: %s' % user_flash prot_version, fw_version = b.get_firmware_version() print 'Protocol version %s.%s' % prot_version print 'Firmware version %s.%s' % fw_version millivolts = b.get_battery_level() print 'Battery level %s mV' % millivolts except: print "Error while running test:" traceback.print_tb(sys.exc_info()[2]) print str(sys.exc_info()[1]) if b: b.sock.close() nxt-python-2.2.2/scripts/nxt_filer0000644000175000001440000001140111605102443017713 0ustar marcususers00000000000000#!/usr/bin/env python # # nxt_filer program -- Simple GUI to manage files on a LEGO Mindstorms NXT # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2010 rhn # # 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 2 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 cStringIO import gtk import os.path import urllib2 import sys import nxt.locator from nxt.brick import FileFinder, FileReader, FileWriter from nxt.utils import parse_command_line_arguments def read_file(b, fname): with FileReader(b, fname) as r: with open(fname, 'wb') as f: for data in r: f.write(data) class NXTListing(gtk.ListStore): def __init__(self, brick): gtk.ListStore.__init__(self, str, str) self.set_sort_column_id(0, gtk.SORT_ASCENDING) self.populate(brick, '*.*') def populate(self, brick, pattern): f = FileFinder(brick, pattern) for (fname, size) in f: self.append((fname, str(size))) def write_file(b, fname, data): w = FileWriter(b, fname, len(data)) print 'Pushing %s (%d bytes) ...' % (fname, w.size), sys.stdout.flush() w.write(data) print 'wrote %d bytes' % len(data) w.close() def write_files(b, names): for fname in names.split('\r\n'): if fname: print 'File:', fname bname = os.path.basename(fname) url = urllib2.urlopen(fname) try: data = url.read() finally: url.close() print 'name %s, size: %d ' % (bname, len(data)) write_file(b, bname, data) class NXT_Filer(gtk.Window): TARGETS = gtk.target_list_add_uri_targets() def __init__(self, brick): gtk.Window.__init__(self) self.brick = brick self.set_border_width(6) self.nxt_model = NXTListing(brick) h = gtk.HBox() h.pack_start(self.make_file_panel(str(brick.sock), self.nxt_model), True) self.add(h) self.connect('destroy', self.quit) def make_file_list(self): tv = gtk.TreeView() tv.set_headers_visible(True) tv.set_property('fixed_height_mode', True) r = gtk.CellRendererText() c = gtk.TreeViewColumn('File name', r, text=0) c.set_fixed_width(200) c.set_sizing(gtk.TREE_VIEW_COLUMN_FIXED) tv.append_column(c) r = gtk.CellRendererText() c = gtk.TreeViewColumn('Bytes', r, text=1) c.set_sizing(gtk.TREE_VIEW_COLUMN_FIXED) c.set_fixed_width(80) tv.append_column(c) # tv.enable_model_drag_source(gtk.gdk.BUTTON1_MASK, self.TARGETS, # gtk.gdk.ACTION_DEFAULT | gtk.gdk.ACTION_MOVE) tv.enable_model_drag_dest(self.TARGETS, gtk.gdk.ACTION_COPY) # tv.connect("drag_data_get", self.drag_data_get_data) tv.connect("drag_data_received", self.drag_data_received_data) return tv def make_file_panel(self, name, model): v = gtk.VBox() v.pack_start(gtk.Label(name), False) tv = self.make_file_list() tv.set_model(model) s = gtk.ScrolledWindow() s.set_policy(gtk.POLICY_NEVER, gtk.POLICY_ALWAYS) s.add(tv) s.set_border_width(2) v.pack_start(s, True) return v def drag_data_get_data(self, treeview, context, selection, target_id, etime): treeselection = treeview.get_selection() model, iter = treeselection.get_selected() data = model.get_value(iter, 0) print data selection.set(selection.target, 8, data) def drag_data_received_data(self, treeview, context, x, y, selection, info, etime): if context.action == gtk.gdk.ACTION_COPY: write_files(self.brick, selection.data) # FIXME: update file listing after writing files # FIXME: finish context def quit(self, w): 'Quit the program' self.brick.sock.close() gtk.main_quit() if __name__ == '__main__': # FIXME: add dialog with progress bar when scanning bluetooth devices arguments, keyword_arguments = parse_command_line_arguments(sys.argv) if '--help' in arguments: print '''nxt_filer -- Simple GUI to manage files on a LEGO Mindstorms NXT Usage: nxt_filer [--host ]''' exit(0) brick = nxt.locator.find_one_brick(keyword_arguments.get('host',None)) win = NXT_Filer(brick) win.show_all() gtk.main() nxt-python-2.2.2/scripts/nxt_server0000644000175000001440000000453311602375211020132 0ustar marcususers00000000000000#!/usr/bin/env python # # nxt_server program -- Serve an interface to the NXT brick # Copyright (C) 2011 zonedabone, 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 2 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 import socket, sys, traceback from nxt.utils import parse_command_line_arguments def serve(brick, channel, details): 'handles serving the client' print "Connection started (" + details[0] + ')' run = True try: while run: data = channel.recv(1024) if not data: break code = data[0] if code == '\x00' or code == '\x01' or code == '\x02': brick.sock.send(data) reply = brick.sock.recv() channel.send(reply) elif code == '\x80' or code == '\x81': brick.sock.send(data) elif code == '\x98': channel.send(brick.sock.type) elif code == '\x99': run = False except: traceback.print_exc() finally: channel.close() print "Connection Finished" if __name__ == "__main__": arguments, keyword_arguments = parse_command_line_arguments(sys.argv) if '--help' in arguments: print( """nxt_server -- command server for NXT brick Usage: nxt_server.py [--host ][--name ] [--iphost ][--ipport ]""") exit(0) print "Connecting to NXT..." brick = nxt.find_one_brick(keyword_arguments.get('host',None),keyword_arguments.get('name',None)) print "Brick found." print "Starting server..." server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server.bind((keyword_arguments.get('iphost',''), int(keyword_arguments.get('ipport','2727')))) server.listen(1) # Have the server serve "forever": while True: channel, details = server.accept() #TODO: sasl authentication here? serve(brick, channel, details) nxt-python-2.2.2/scripts/nxt_push0000644000175000001440000000323211662266605017612 0ustar marcususers00000000000000#!/usr/bin/env python # # nxt_push program -- Push a file to a LEGO Mindstorms NXT brick # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2010 rhn # # 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 2 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 sys import nxt.locator from nxt.brick import FileWriter from nxt.error import FileNotFound from nxt.utils import parse_command_line_arguments def _write_file(b, fname, data): w = FileWriter(b, fname, len(data)) print 'Pushing %s (%d bytes) ...' % (fname, w.size), sys.stdout.flush() w.write(data) print 'wrote %d bytes' % len(data) w.close() def write_file(b, fname): f = open(fname) data = f.read() f.close() try: b.delete(fname) print 'Overwriting %s on NXT' % fname except FileNotFound: pass _write_file(b, fname, data) if __name__ == '__main__': arguments, keyword_arguments = parse_command_line_arguments(sys.argv) if '--help' in arguments: print '''nxt_push -- Push a file to a LEGO Mindstorms NXT brick Usage: nxt_push [--host ] ''' exit(0) brick = nxt.locator.find_one_brick(keyword_arguments.get('host',None)) for filename in arguments: write_file(brick, filename) brick.sock.close() nxt-python-2.2.2/motcont/0000755000175000001440000000000011760013147016001 5ustar marcususers00000000000000nxt-python-2.2.2/motcont/FlashNXTWithFirmware1.29.bat0000644000175000001440000000446211565504122022772 0ustar marcususers00000000000000@echo off REM * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * REM * The program NeXTTool.exe must exist in the same folder as this file or be on the path!* REM * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * if not exist NeXTTool.exe goto nexttoolmissing if not exist "LEGO_MINDSTORMS_NXT_Firmware_V1.29.rfw" goto firmwaremissing goto flash goto end :nexttoolmissing echo. echo The NeXTTool, NeXTTool.exe, is missing. echo This file has to be in the same directory as this script echo or must reside in the Windows search path. echo. echo Go to http://bricxcc.sourceforge.net/utilities.html echo and download the latest NeXTTool utility, echo or download BricxCC, where this tool is echo already included. echo Then place NeXTTool.exe in this folder restart the script: echo. cd echo. goto end :firmwaremissing echo. echo The firmware file is missing: echo "LEGO_MINDSTORMS_NXT_Firmware_V1.29.rfw" echo This file has to be in the same directory as this script echo or must reside in the Windows search path. echo. echo Obtain the file from the internet at: echo http://mindstorms.lego.com/en-us/support/files/Driver.aspx#Firmware echo or from the NXT-G software package. If you modify this script, echo you can also use J. Hansens enhanced Firmware 1.28, echo or the NXT Retail 2.0 Firmware 1.28. echo. echo Place the file in this folder and restart the script: echo. cd echo. echo LEGO mispelled the firmware file in one of their download archives, echo if you downloaded "LEGO_MINDSTORMS_NXT_Firrmware_V1.29.rfw", echo rename the file to "LEGO_MINDSTORMS_NXT_Firmware_V1.29.rfw" echo. goto end :flash echo. echo This program will update the NXT's firmware to version 1.29 echo Please connect a single NXT brick via a USB cable and turn it on. echo This will take a bit, the NXT will make a clicking sound. echo If it fails, you can reset the NXT brick and/or restart the script. echo After a successful download the NXT should automatically boot. echo. pause echo. echo Flashing firmware... echo. NeXTTool /COM=usb -firmware="LEGO_MINDSTORMS_NXT_Firmware_V1.29.rfw" :end echo. echo Script finished. pausenxt-python-2.2.2/motcont/Controller.nxc0000644000175000001440000002113511565504122020641 0ustar marcususers00000000000000//#!C /* % Part of MotorControl containing the parameters of the PD-controller % and regulation function, includes other functions/files % % Signature % Author: Linus Atorf (see AUTHORS) % Date: 2009/06/09 % Copyright: 2007-2009, RWTH Aachen University % % % *********************************************************************************************** % * This file is part of the RWTH - Mindstorms NXT Toolbox. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * You should have received a copy of the GNU General Public License along with the * % * RWTH - Mindstorms NXT Toolbox. If not, see . * % *********************************************************************************************** */ #ifndef __CONTROLLER_NXC__ #define __CONTROLLER_NXC__ // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // * #INCLUDES ARE BELOW, to make sure params are set before! * // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // All defines marked with PARAM can be adjusted / tweaked, others NOT! // Driving "stages" or "phases" during controlled motor loop... #define STAGE_RAMPUP 1 #define STAGE_DRIVING 2 #define STAGE_BRAKING 3 #define STAGE_ENDGAME 4 // CONSTANT LOOP TIMING // with 4 full tasks, 15 or 16 seems to be the absolute minimum possible! // change according MINIMALIDEALSPEED below, too #define LOOP_DURATION 15 //PARAM // how many loop-iterations to integrate over for speed measurement // question is: 2 or 3? change according MINIMALIDEALSPEED below, too #define SPEEDHISTORY 3 //PARAM // adjust this value to the minimal speed that can be measured with the timing // parameters above, i.e. set to x = 1000 / (LOOP_DURATION * SPEEDHISTORY) #define MINIMALIDEALSPEED 23 //PARAM // how many degrees before goal to start endgame mode // very important tweaking param for precision! // 2 seems high (good precision, possible stalling / unsmooth breaking) // 1 seems low (worse, but still good precision, less change of stalling, higher // chance to miss the goal "big time" [i.e. a couple of degrees]) #define ENDGAMEBRAKINGSTARTDIST 2 //PARAM // maximum time for endgame mode, after that brake will jam in anyway // lower this time cautiously, if the motor stalls during this loop, probably // for a good reason #define ENDGAME_INNERLOOP_TIMEOUT 350 //PARAM // enable this to free/sleep 1ms of CPU time every waiting-loop iteration // during rampup and driving stages... // pro: improved responsiveness / better PID control / more CPU time for other // processes during driving, when CPU is not as much needed // con: may lead to less accuracy?!?! later starting of braking? #define SLEEP_DURING_WAIT_WHEN_DRIVING // PID parameters #define PIDUPSCALING 10000 // HAVE TO BE UPDATED ALL THE TIME MANUALLY, depend on PIDUPSCALING!!! #define HARDCODED_KP -650 //PARAM #define HARDCODED_KD -2500 //PARAM // slightly different set for RunMotor2 (synced driving) #define HARDCODED_KP_SYNC -200 //PARAM #define HARDCODED_KD_SYNC -1800 //PARAM /* // this was the "original formula" long KP = -1 * PIDUPSCALING / 20; // -1/40 to -1/60 ???? long KD = -1 * PIDUPSCALING / 4; // -1/2 to -1/3 */ // when driving with speed regulation, braking happens WITHOUT regulation // (it always does). To avoid that the controller overpowers the motor, we // clip the maximum allowed power value. But for speed reg, we must set this // limit a bit higher (i.e. to model our own regulation). #define ADDITIONALMAXSPEEDREGBRAKINGPOWER 30; //PARAM // when STAGE_RAMPUP, "manual rampup" can be applied. how many constant-time // loop iterations should it take to get to power 100? if power < 100 is reached // before, rampup is finished. Multiply this with LOOP_DURATION to get the maximum // starting/rampup time for the power=100 case... #define SMOOTHSTARTSTEPS 18 // --- DEBUGGING OPTIONS ----------- //#define ENABLEDEBUGGING_LCD //#define ENABLEDEBUGGING_WAITAFTERLCD //#define ENABLEDEBUGGING_LCD_SLOW_ANTIBUG //#define ENABLEDEBUGGING_REMOTE //#define ENABLEDEBUGGING_ACOUSTIC //#define ENABLEDEBUGGING_ACOUSTICPROTOCOL //#define ENABLEDEBUGGING_OLDLCDTIMING //#define ENABLEDEBUGGING_REMOTELOGGING //#define ENABLEDEBUGGING_SHOWTASKS // --------------------------------- // *** SOME INCLUDES! #include "MotorFunctions.nxc" #include "SpeedFromPosLookup.nxc" //remove this debug info? #ifdef ENABLEDEBUGGING_OLDLCDTIMING long receivedMsgTime; long motorStartedTime; #endif #ifdef ENABLEDEBUGGING_REMOTELOGGING //long debugStartTick = CurrentTick(); long remoteDebugMsgCounter = 0; safecall void SendDebugMessage(const byte &port, const string msg) { //string time = NumToStr(CurrentTick() - FirstTick()); string p = NumToStr(port); string n = NumToStr(remoteDebugMsgCounter); //string data = StrCat(time, "|", n, "|", p, ":", msg); string data = StrCat(n, "|", p, ":", msg); remoteDebugMsgCounter++; SendMessage(DEBUG_OUTBOX, data); }//end void #endif //TODO optimize message-parsing (ReceiveRemoteNumber, etc?) //TODO remember to check what happens when CHANGING SPEED during RUNTIME?!?!!?!! //TODO add anti-deadlock anti-hang stuff if direct command (other than stop) interferes? /* //TODO Implement LOOKUP TABLE in here!!!? inline long GetAbsBrakingDistFromAbsSpeed(const long &absSpeed) { // absolute max realistic speed with full batteries (not akkus) is 1100 deg per sec //return (absSpeed / 4) + 30; //TODO maybe the plus-term below is not necessary after all... // to avoid braking mode being skipped... return ((absSpeed * absSpeed) / 2700 ) + ENDGAMEBRAKINGSTARTDIST + 1; }//end function //2nd routine for synced driving, need a bit more space to brake //TODO Implement LOOKUP TABLE in here!!!? inline long GetAbsBrakingDistFromAbsSpeed2(const long &absSpeed) { // absolute max realistic speed with full batteries (not akkus) is 1100 deg per sec //TODO maybe the plus-term below is not necessary after all... // to avoid braking mode being skipped... return ((absSpeed * absSpeed) / 2200 ) + ENDGAMEBRAKINGSTARTDIST + 5; }//end function */ // Following is a "dirty" preprocessor trick. We include ControllerCore.nxc // twice, with different macros defined each time, leading two two different // functions being imported (RunMotor and RunMotor2). // Old comment: To get functionality for TWO SYNCED MOTORS, do the following: // Always take THIS function, copy-paste it, call it RunMotor2, add another // argument port2 to it, and done. BEFORE the function, define RUNMOTOR2_SYNCMODE #undef RUNMOTOR2_SYNCMODE // just to be safe :-) // this time, RunMotor will be imported! #define RUNMOTOR_CURRENTPORTNAME A #include "ControllerCore.nxc" #undef RUNMOTOR_CURRENTPORTNAME #define RUNMOTOR_CURRENTPORTNAME B #include "ControllerCore.nxc" #undef RUNMOTOR_CURRENTPORTNAME #define RUNMOTOR_CURRENTPORTNAME C #include "ControllerCore.nxc" #undef RUNMOTOR_CURRENTPORTNAME // Old comment: the following function must be an excact copy-paste-replica of the RunMotor // function from above, but with the name RunMotor2, the additional param port2, // RUNMOTOR2_SYNCMODE defined, and always speedreg=false. It also doesn't need // to be inline (to save space)... #define RUNMOTOR2_SYNCMODE // this time, RunMotor2 will be imported! #define RUNMOTOR_CURRENTPORTNAME 2 #include "ControllerCore.nxc" #undef RUNMOTOR2_SYNCMODE // just to be safe :-) #endif nxt-python-2.2.2/motcont/SpeedFromPosLookup.nxc0000644000175000001440000002721111565504122022257 0ustar marcususers00000000000000//#!C /* % Part of MotorControl containing a lookup table for speed / position regulation, % to be used by the PD-controller and generated by "TestVFromPos.m" % % Signature % Author: Linus Atorf (see AUTHORS) % Date: 2009/08/21 % Copyright: 2007-2009, RWTH Aachen University % % % *********************************************************************************************** % * This file is part of the RWTH - Mindstorms NXT Toolbox. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * You should have received a copy of the GNU General Public License along with the * % * RWTH - Mindstorms NXT Toolbox. If not, see . * % *********************************************************************************************** */ #ifndef __SPEEDFROMPOSLOOKUP_NXC__ #define __SPEEDFROMPOSLOOKUP_NXC__ #define SPEEDLUTSIZE 301 #define SPEEDLUTSCALING 10000 // scaled * 100, + to get percentages * 100 ... int SpeedFromPosLUT[SPEEDLUTSIZE]; inline long GetIdealSpeedFromPos(long x, long maxSpeed, long brakingDist) { int i; long tmp; i = (x * SPEEDLUTSIZE) / brakingDist; if (i < 0) { i = 0; } else if (i > (SPEEDLUTSIZE-1)) { i = SPEEDLUTSIZE-1; }//end if // scaling *100 and another *100 to get percentages tmp = (SpeedFromPosLUT[i] * maxSpeed) / SPEEDLUTSCALING; // don't return too small speeds, we can neither measure nor set such small speeds if (abs(tmp) >= MINIMALIDEALSPEED) { return tmp; } else { return MINIMALIDEALSPEED * sign(maxSpeed); }// end if }//end function safecall void InitSpeedFromPosLUT() { SpeedFromPosLUT[0] = 10000; SpeedFromPosLUT[1] = 9983; SpeedFromPosLUT[2] = 9967; SpeedFromPosLUT[3] = 9950; SpeedFromPosLUT[4] = 9933; SpeedFromPosLUT[5] = 9916; SpeedFromPosLUT[6] = 9899; SpeedFromPosLUT[7] = 9883; SpeedFromPosLUT[8] = 9866; SpeedFromPosLUT[9] = 9849; SpeedFromPosLUT[10] = 9832; SpeedFromPosLUT[11] = 9815; SpeedFromPosLUT[12] = 9798; SpeedFromPosLUT[13] = 9781; SpeedFromPosLUT[14] = 9764; SpeedFromPosLUT[15] = 9747; SpeedFromPosLUT[16] = 9730; SpeedFromPosLUT[17] = 9713; SpeedFromPosLUT[18] = 9695; SpeedFromPosLUT[19] = 9678; SpeedFromPosLUT[20] = 9661; SpeedFromPosLUT[21] = 9644; SpeedFromPosLUT[22] = 9626; SpeedFromPosLUT[23] = 9609; SpeedFromPosLUT[24] = 9592; SpeedFromPosLUT[25] = 9574; SpeedFromPosLUT[26] = 9557; SpeedFromPosLUT[27] = 9539; SpeedFromPosLUT[28] = 9522; SpeedFromPosLUT[29] = 9504; SpeedFromPosLUT[30] = 9487; SpeedFromPosLUT[31] = 9469; SpeedFromPosLUT[32] = 9452; SpeedFromPosLUT[33] = 9434; SpeedFromPosLUT[34] = 9416; SpeedFromPosLUT[35] = 9399; SpeedFromPosLUT[36] = 9381; SpeedFromPosLUT[37] = 9363; SpeedFromPosLUT[38] = 9345; SpeedFromPosLUT[39] = 9327; SpeedFromPosLUT[40] = 9309; SpeedFromPosLUT[41] = 9292; SpeedFromPosLUT[42] = 9274; SpeedFromPosLUT[43] = 9256; SpeedFromPosLUT[44] = 9238; SpeedFromPosLUT[45] = 9220; SpeedFromPosLUT[46] = 9201; SpeedFromPosLUT[47] = 9183; SpeedFromPosLUT[48] = 9165; SpeedFromPosLUT[49] = 9147; SpeedFromPosLUT[50] = 9129; SpeedFromPosLUT[51] = 9110; SpeedFromPosLUT[52] = 9092; SpeedFromPosLUT[53] = 9074; SpeedFromPosLUT[54] = 9055; SpeedFromPosLUT[55] = 9037; SpeedFromPosLUT[56] = 9018; SpeedFromPosLUT[57] = 9000; SpeedFromPosLUT[58] = 8981; SpeedFromPosLUT[59] = 8963; SpeedFromPosLUT[60] = 8944; SpeedFromPosLUT[61] = 8926; SpeedFromPosLUT[62] = 8907; SpeedFromPosLUT[63] = 8888; SpeedFromPosLUT[64] = 8869; SpeedFromPosLUT[65] = 8851; SpeedFromPosLUT[66] = 8832; SpeedFromPosLUT[67] = 8813; SpeedFromPosLUT[68] = 8794; SpeedFromPosLUT[69] = 8775; SpeedFromPosLUT[70] = 8756; SpeedFromPosLUT[71] = 8737; SpeedFromPosLUT[72] = 8718; SpeedFromPosLUT[73] = 8699; SpeedFromPosLUT[74] = 8679; SpeedFromPosLUT[75] = 8660; SpeedFromPosLUT[76] = 8641; SpeedFromPosLUT[77] = 8622; SpeedFromPosLUT[78] = 8602; SpeedFromPosLUT[79] = 8583; SpeedFromPosLUT[80] = 8563; SpeedFromPosLUT[81] = 8544; SpeedFromPosLUT[82] = 8524; SpeedFromPosLUT[83] = 8505; SpeedFromPosLUT[84] = 8485; SpeedFromPosLUT[85] = 8466; SpeedFromPosLUT[86] = 8446; SpeedFromPosLUT[87] = 8426; SpeedFromPosLUT[88] = 8406; SpeedFromPosLUT[89] = 8386; SpeedFromPosLUT[90] = 8367; SpeedFromPosLUT[91] = 8347; SpeedFromPosLUT[92] = 8327; SpeedFromPosLUT[93] = 8307; SpeedFromPosLUT[94] = 8287; SpeedFromPosLUT[95] = 8266; SpeedFromPosLUT[96] = 8246; SpeedFromPosLUT[97] = 8226; SpeedFromPosLUT[98] = 8206; SpeedFromPosLUT[99] = 8185; SpeedFromPosLUT[100] = 8165; SpeedFromPosLUT[101] = 8145; SpeedFromPosLUT[102] = 8124; SpeedFromPosLUT[103] = 8103; SpeedFromPosLUT[104] = 8083; SpeedFromPosLUT[105] = 8062; SpeedFromPosLUT[106] = 8042; SpeedFromPosLUT[107] = 8021; SpeedFromPosLUT[108] = 8000; SpeedFromPosLUT[109] = 7979; SpeedFromPosLUT[110] = 7958; SpeedFromPosLUT[111] = 7937; SpeedFromPosLUT[112] = 7916; SpeedFromPosLUT[113] = 7895; SpeedFromPosLUT[114] = 7874; SpeedFromPosLUT[115] = 7853; SpeedFromPosLUT[116] = 7832; SpeedFromPosLUT[117] = 7810; SpeedFromPosLUT[118] = 7789; SpeedFromPosLUT[119] = 7767; SpeedFromPosLUT[120] = 7746; SpeedFromPosLUT[121] = 7724; SpeedFromPosLUT[122] = 7703; SpeedFromPosLUT[123] = 7681; SpeedFromPosLUT[124] = 7659; SpeedFromPosLUT[125] = 7638; SpeedFromPosLUT[126] = 7616; SpeedFromPosLUT[127] = 7594; SpeedFromPosLUT[128] = 7572; SpeedFromPosLUT[129] = 7550; SpeedFromPosLUT[130] = 7528; SpeedFromPosLUT[131] = 7506; SpeedFromPosLUT[132] = 7483; SpeedFromPosLUT[133] = 7461; SpeedFromPosLUT[134] = 7439; SpeedFromPosLUT[135] = 7416; SpeedFromPosLUT[136] = 7394; SpeedFromPosLUT[137] = 7371; SpeedFromPosLUT[138] = 7348; SpeedFromPosLUT[139] = 7326; SpeedFromPosLUT[140] = 7303; SpeedFromPosLUT[141] = 7280; SpeedFromPosLUT[142] = 7257; SpeedFromPosLUT[143] = 7234; SpeedFromPosLUT[144] = 7211; SpeedFromPosLUT[145] = 7188; SpeedFromPosLUT[146] = 7165; SpeedFromPosLUT[147] = 7141; SpeedFromPosLUT[148] = 7118; SpeedFromPosLUT[149] = 7095; SpeedFromPosLUT[150] = 7071; SpeedFromPosLUT[151] = 7047; SpeedFromPosLUT[152] = 7024; SpeedFromPosLUT[153] = 7000; SpeedFromPosLUT[154] = 6976; SpeedFromPosLUT[155] = 6952; SpeedFromPosLUT[156] = 6928; SpeedFromPosLUT[157] = 6904; SpeedFromPosLUT[158] = 6880; SpeedFromPosLUT[159] = 6856; SpeedFromPosLUT[160] = 6831; SpeedFromPosLUT[161] = 6807; SpeedFromPosLUT[162] = 6782; SpeedFromPosLUT[163] = 6758; SpeedFromPosLUT[164] = 6733; SpeedFromPosLUT[165] = 6708; SpeedFromPosLUT[166] = 6683; SpeedFromPosLUT[167] = 6658; SpeedFromPosLUT[168] = 6633; SpeedFromPosLUT[169] = 6608; SpeedFromPosLUT[170] = 6583; SpeedFromPosLUT[171] = 6557; SpeedFromPosLUT[172] = 6532; SpeedFromPosLUT[173] = 6506; SpeedFromPosLUT[174] = 6481; SpeedFromPosLUT[175] = 6455; SpeedFromPosLUT[176] = 6429; SpeedFromPosLUT[177] = 6403; SpeedFromPosLUT[178] = 6377; SpeedFromPosLUT[179] = 6351; SpeedFromPosLUT[180] = 6325; SpeedFromPosLUT[181] = 6298; SpeedFromPosLUT[182] = 6272; SpeedFromPosLUT[183] = 6245; SpeedFromPosLUT[184] = 6218; SpeedFromPosLUT[185] = 6191; SpeedFromPosLUT[186] = 6164; SpeedFromPosLUT[187] = 6137; SpeedFromPosLUT[188] = 6110; SpeedFromPosLUT[189] = 6083; SpeedFromPosLUT[190] = 6055; SpeedFromPosLUT[191] = 6028; SpeedFromPosLUT[192] = 6000; SpeedFromPosLUT[193] = 5972; SpeedFromPosLUT[194] = 5944; SpeedFromPosLUT[195] = 5916; SpeedFromPosLUT[196] = 5888; SpeedFromPosLUT[197] = 5859; SpeedFromPosLUT[198] = 5831; SpeedFromPosLUT[199] = 5802; SpeedFromPosLUT[200] = 5774; SpeedFromPosLUT[201] = 5745; SpeedFromPosLUT[202] = 5715; SpeedFromPosLUT[203] = 5686; SpeedFromPosLUT[204] = 5657; SpeedFromPosLUT[205] = 5627; SpeedFromPosLUT[206] = 5598; SpeedFromPosLUT[207] = 5568; SpeedFromPosLUT[208] = 5538; SpeedFromPosLUT[209] = 5508; SpeedFromPosLUT[210] = 5477; SpeedFromPosLUT[211] = 5447; SpeedFromPosLUT[212] = 5416; SpeedFromPosLUT[213] = 5385; SpeedFromPosLUT[214] = 5354; SpeedFromPosLUT[215] = 5323; SpeedFromPosLUT[216] = 5292; SpeedFromPosLUT[217] = 5260; SpeedFromPosLUT[218] = 5228; SpeedFromPosLUT[219] = 5196; SpeedFromPosLUT[220] = 5164; SpeedFromPosLUT[221] = 5132; SpeedFromPosLUT[222] = 5099; SpeedFromPosLUT[223] = 5066; SpeedFromPosLUT[224] = 5033; SpeedFromPosLUT[225] = 5000; SpeedFromPosLUT[226] = 4967; SpeedFromPosLUT[227] = 4933; SpeedFromPosLUT[228] = 4899; SpeedFromPosLUT[229] = 4865; SpeedFromPosLUT[230] = 4830; SpeedFromPosLUT[231] = 4796; SpeedFromPosLUT[232] = 4761; SpeedFromPosLUT[233] = 4726; SpeedFromPosLUT[234] = 4690; SpeedFromPosLUT[235] = 4655; SpeedFromPosLUT[236] = 4619; SpeedFromPosLUT[237] = 4583; SpeedFromPosLUT[238] = 4546; SpeedFromPosLUT[239] = 4509; SpeedFromPosLUT[240] = 4472; SpeedFromPosLUT[241] = 4435; SpeedFromPosLUT[242] = 4397; SpeedFromPosLUT[243] = 4359; SpeedFromPosLUT[244] = 4320; SpeedFromPosLUT[245] = 4282; SpeedFromPosLUT[246] = 4243; SpeedFromPosLUT[247] = 4203; SpeedFromPosLUT[248] = 4163; SpeedFromPosLUT[249] = 4123; SpeedFromPosLUT[250] = 4082; SpeedFromPosLUT[251] = 4041; SpeedFromPosLUT[252] = 4000; SpeedFromPosLUT[253] = 3958; SpeedFromPosLUT[254] = 3916; SpeedFromPosLUT[255] = 3873; SpeedFromPosLUT[256] = 3830; SpeedFromPosLUT[257] = 3786; SpeedFromPosLUT[258] = 3742; SpeedFromPosLUT[259] = 3697; SpeedFromPosLUT[260] = 3651; SpeedFromPosLUT[261] = 3606; SpeedFromPosLUT[262] = 3559; SpeedFromPosLUT[263] = 3512; SpeedFromPosLUT[264] = 3464; SpeedFromPosLUT[265] = 3416; SpeedFromPosLUT[266] = 3367; SpeedFromPosLUT[267] = 3317; SpeedFromPosLUT[268] = 3266; SpeedFromPosLUT[269] = 3215; SpeedFromPosLUT[270] = 3162; SpeedFromPosLUT[271] = 3109; SpeedFromPosLUT[272] = 3055; SpeedFromPosLUT[273] = 3000; SpeedFromPosLUT[274] = 2944; SpeedFromPosLUT[275] = 2887; SpeedFromPosLUT[276] = 2828; SpeedFromPosLUT[277] = 2769; SpeedFromPosLUT[278] = 2708; SpeedFromPosLUT[279] = 2646; SpeedFromPosLUT[280] = 2582; SpeedFromPosLUT[281] = 2517; SpeedFromPosLUT[282] = 2449; SpeedFromPosLUT[283] = 2380; SpeedFromPosLUT[284] = 2309; SpeedFromPosLUT[285] = 2236; SpeedFromPosLUT[286] = 2160; SpeedFromPosLUT[287] = 2082; SpeedFromPosLUT[288] = 2000; SpeedFromPosLUT[289] = 1915; SpeedFromPosLUT[290] = 1826; SpeedFromPosLUT[291] = 1732; SpeedFromPosLUT[292] = 1633; SpeedFromPosLUT[293] = 1528; SpeedFromPosLUT[294] = 1414; SpeedFromPosLUT[295] = 1291; SpeedFromPosLUT[296] = 1155; SpeedFromPosLUT[297] = 1000; SpeedFromPosLUT[298] = 816; SpeedFromPosLUT[299] = 577; SpeedFromPosLUT[300] = 300; // was actually 0, but having that is not cool! }//end void #endif nxt-python-2.2.2/motcont/TransferMotorControlBinaryToNXT.bat0000644000175000001440000000316611565504122024710 0ustar marcususers00000000000000@echo off REM * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * REM * The program NeXTTool.exe must exist in the same folder as this file or be on the path!* REM * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * if not exist NeXTTool.exe goto nexttoolmissing if not exist MotorControl22.rxe goto binarymissing goto download goto end :nexttoolmissing echo. echo The NeXTTool, NeXTTool.exe, is missing. echo This file has to be in the same directory as this script echo or must reside in the Windows search path. echo. echo Go to http://bricxcc.sourceforge.net/utilities.html echo and download the latest NeXTTool utility, echo or download BricxCC, where this tool is echo already included. echo Then place NeXTTool.exe in this folder restart the script: echo. cd echo. goto end :binarymissing echo. echo Cannot find binary RXE file to transfer. Aborting... echo. goto end :download echo. echo This program downloads MotorControl22.rxe echo to the NXT brick. Please connect a single NXT brick echo via a USB cable and turn it on. echo Make sure Firmware 1.28 (or higher) is installed on the NXT. echo. pause echo. echo Transferring program... NeXTTool /COM=usb -download=MotorControl22.rxe echo Verifying download... echo. echo If the next paragraph shows echo MotorControl22.rxe=xxxxx echo then everything worked. echo A blank line indicates failure! echo. NeXTTool /COM=usb -listfiles=MotorControl22.rxe :end echo. echo Script finished. echo. pausenxt-python-2.2.2/motcont/MotorControl22.nxc0000644000175000001440000006713111565504122021331 0ustar marcususers00000000000000//#!C /* % The NXT Program MotorControl enables precise motor movement via direct % commands. It listens to "NXT BT messages", interprets their content (own user- % defined "protocol", if you will), and carries out highly precise motor actions % in a different thread for each motor, allowing the upper level program (in % this case, MATLAB) to carry on with execution... % % See also: http://www.mindstorms.rwth-aachen.de/trac/wiki/MotorControl % % CHANGELOG: % % * Version 2.2, 2010/09/14 % First released with toolbox version 4.04 % - Commented dead code in case compiler optimization doesn't find it % - No real code changes % - Version to be recompiled with newer NBC/NXC compiler versions, % as some multi-threading / optimization bugs were fixed. % - Not recommending FW version 1.26 anymore % - Updated version numbers % % * Version 2.1, 2009/08/31 % First released with toolbox version 4.01 % - Considered to be stable and working % - Very occasional freezes with FW 1.26 during % massive production use, see also: % http://www.mindstorms.rwth-aachen.de/trac/ticket/51 % % % Signature % Author: Linus Atorf (see AUTHORS) % Date: 2010/09/14 % Copyright: 2007-2010, RWTH Aachen University % % % *********************************************************************************************** % * This file is part of the RWTH - Mindstorms NXT Toolbox. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * You should have received a copy of the GNU General Public License along with the * % * RWTH - Mindstorms NXT Toolbox. If not, see . * % *********************************************************************************************** */ // *************** GLOBAL DEFINES // important to be before includes, // since they might be used inside the includes! #define PROGRAM_VERSION "2.2" #define INBOX 1 #define OUTBOX 0 #define DEBUG_OUTBOX 2 // Protocol constants #define PROTO_CONTROLLED_MOTORCMD 1 #define PROTO_RESET_ERROR_CORRECTION 2 #define PROTO_ISMOTORREADY 3 #define PROTO_CLASSIC_MOTORCMD 4 #define PROTO_JUMBOPACKET 5 // *************** INCLUDES #include "Controller.nxc" // Current file structure: // + MotorControl.nxc (this file) // |-+ Controller.nxc // |-- MotorFunctions.nxc // |-- SpeedFromPosLookup.nxc // |-- ControllerCore.nxc (for Motor A) // |-- ControllerCore.nxc (for Motor B) // |-- ControllerCore.nxc (for Motor C) // |-- ControllerCore.nxc (for sync mode) // *************** PROGRAM STARTS struct typRunMotorParams { int power; long tacholimit; bool speedreg; bool holdbrake; bool smoothstart; int turnratio; }; byte SyncPorts; typRunMotorParams motorParamsA; typRunMotorParams motorParamsB; typRunMotorParams motorParamsC; typRunMotorParams motorParamsSync; //TODO are the mutexes really still needed? mutex movingA; mutex movingB; mutex movingC; // these semaphores are actually interpreted as "motorArunning", as they are // also set and interpreted for synced movements... a bit like the mutexes above bool taskArunning; bool taskBrunning; bool taskCrunning; #ifdef ENABLEDEBUGGING_SHOWTASKS bool taskSyncRunning; #endif #ifdef ENABLEDEBUGGING_OLDLCDTIMING long parsedTime; long taskStartedTime; #endif safecall void DisplayMainScreen() { string tmp; // nice message to screen: tmp = StrCat("MotorControl ", PROGRAM_VERSION); TextOut(0,LCD_LINE1, tmp, true); TextOut(0,LCD_LINE2, "for FW 1.28-1.29", false); TextOut(3,LCD_LINE4, "(C) RWTH Aachen", false); TextOut(3,LCD_LINE5, "University, LfB", false); TextOut(4,LCD_LINE7, "Press ORANGE to", false); TextOut(4,LCD_LINE8, "stop all motors", false); #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, LCD_LINE8, "SLOW ANTIBUG ON ", true); #endif// - - - - - - - }//end void safecall void TaskBusySignal() { PlayTone(2000, 50); Wait(50); PlayTone(1500, 50); }//end TaskBusySignal // tasks task MoveA() { //TextOut(0,LCD_LINE6, "Task A started"); //Acquire(movingA); taskArunning = true; bool stoppedByDirectCmd; stoppedByDirectCmd = RunMotorA(OUT_A, motorParamsA.power, motorParamsA.tacholimit, motorParamsA.speedreg, motorParamsA.holdbrake, motorParamsA.smoothstart); // if we exited from external NXTMotor.Stop command, we might've overwritten // the power value before exiting the main controller loop, so restore defined // end state here again: if (stoppedByDirectCmd) { if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) { MotorBrake(OUT_A); } else { MotorOff(OUT_A); }//end if }//end if taskArunning = false; //Release(movingA); //TextOut(0,LCD_LINE6, "Task A stopped"); }//MoveA task MoveB() { //TextOut(0,LCD_LINE7, "Task B started"); //Acquire(movingB); taskBrunning = true; #ifdef ENABLEDEBUGGING_OLDLCDTIMING taskStartedTime = CurrentTick() - receivedMsgTime; // NEW DEBUG #endif bool stoppedByDirectCmd; stoppedByDirectCmd = RunMotorB(OUT_B, motorParamsB.power, motorParamsB.tacholimit, motorParamsB.speedreg, motorParamsB.holdbrake, motorParamsB.smoothstart); // if we exited from external NXTMotor.Stop command, we might've overwritten // the power value before exiting the main controller loop, so restore defined // end state here again: if (stoppedByDirectCmd) { if (MotorRegulation(OUT_B) == OUT_REGMODE_SPEED) { MotorBrake(OUT_B); } else { MotorOff(OUT_B); }//end if }//end if #ifdef ENABLEDEBUGGING_OLDLCDTIMING string tmp = ""; // NEW DEBUG tmp = NumToStr(parsedTime); tmp = StrCat("Parsed ", tmp, "ms"); TextOut(0,LCD_LINE3, tmp, true); tmp = NumToStr(taskStartedTime); tmp = StrCat("Task ", tmp, "ms"); TextOut(0,LCD_LINE5, tmp); tmp = NumToStr(motorStartedTime); tmp = StrCat("Motor ", tmp, "ms"); TextOut(0,LCD_LINE7, tmp); #endif taskBrunning = false; //Release(movingB); }//MoveB task MoveC() { //TextOut(0,LCD_LINE8, "Task C started"); //Acquire(movingC); taskCrunning = true; bool stoppedByDirectCmd; stoppedByDirectCmd = RunMotorC(OUT_C, motorParamsC.power, motorParamsC.tacholimit, motorParamsC.speedreg, motorParamsC.holdbrake, motorParamsC.smoothstart); // if we exited from external NXTMotor.Stop command, we might've overwritten // the power value before exiting the main controller loop, so restore defined // end state here again: if (stoppedByDirectCmd) { if (MotorRegulation(OUT_C) == OUT_REGMODE_SPEED) { MotorBrake(OUT_C); } else { MotorOff(OUT_C); }//end if }//end if taskCrunning = false; //Release(movingC); //TextOut(0,LCD_LINE8, "Task C stopped"); }//MoveC task MoveSync() { #ifdef ENABLEDEBUGGING_SHOWTASKS taskSyncRunning = true; #endif bool stoppedByDirectCmd; if (SyncPorts == 3) { // OUT_AB //Acquire(movingA); //Acquire(movingB); taskArunning = true; taskBrunning = true; stoppedByDirectCmd = RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_B); if (stoppedByDirectCmd) { if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) { MotorBrake2(OUT_A, OUT_B); } else { MotorOff2(OUT_A, OUT_B); }//end if }//end if taskArunning = false; taskBrunning = false; //Release(movingA); //Release(movingB); } else if (SyncPorts == 4) { // OUT_AC //Acquire(movingA); //Acquire(movingC); taskArunning = true; taskCrunning = true; stoppedByDirectCmd = RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C); if (stoppedByDirectCmd) { if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) { MotorBrake2(OUT_A, OUT_C); } else { MotorOff2(OUT_A, OUT_C); }//end if }//end if taskArunning = false; taskCrunning = false; //Release(movingA); //Release(movingC); } else if (SyncPorts == 5) { // OUT_BC //Acquire(movingB); //Acquire(movingC); taskBrunning = true; taskCrunning = true; stoppedByDirectCmd = RunMotor2(OUT_B, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C); if (stoppedByDirectCmd) { if (MotorRegulation(OUT_B) == OUT_REGMODE_SPEED) { MotorBrake2(OUT_B, OUT_C); } else { MotorOff2(OUT_B, OUT_C); }//end if }//end if taskBrunning = false; taskCrunning = false; //Release(movingB); //Release(movingC); }//end if #ifdef ENABLEDEBUGGING_SHOWTASKS taskSyncRunning = false; #endif }//MoveSync inline bool IsMotorReady(const byte &port) { // check output state info, this is simple if (MotorIsRunning(port)) { return false; } else { // maybe braking or coasting or whatever: switch(port) { case OUT_A: return !(taskArunning); break; case OUT_B: return !(taskBrunning); break; case OUT_C: return !(taskCrunning); break; }//end switch }//end if }//end function /* task dummyA() { int i = 0; int j = 0; while(true){ i++; i = j; j = MotorTachoCount(OUT_A); }//end while }//end task task dummyB() { int i = 0; int j = 0; while(true){ i++; i = j; j = MotorTachoCount(OUT_B); }//end while }//end task task dummyC() { int i = 0; int j = 0; while(true){ i++; i = j; j = MotorTachoCount(OUT_C); }//end while }//end task task dummyD() { int i = 0; int j = 0; while(true){ i++; i = j; j = MotorTachoCount(OUT_A); }//end while }//end task task dummyE() { int i = 0; int j = 0; while(true){ i++; i = j; j = MotorTachoCount(OUT_B); }//end while }//end task */ // ------------------------------------------------- // main task /* task TESTmain() { byte port = OUT_B; // **** Init DisplayMainScreen(); //Wait(9000); // don't forget this InitSpeedFromPosLUT(); // launch 3 dummy tasks start dummyA; start dummyB; //start dummyC; //start dummyD; //start dummyE; Wait(20); // **** Init / Reset motor MotorOff(OUT_A); MotorOff(OUT_B); MotorOff(OUT_C); ResetErrorCorrectionAndBlockCount(OUT_A); ResetErrorCorrectionAndBlockCount(OUT_B); ResetErrorCorrectionAndBlockCount(OUT_C); Wait(1100); // **** Try out new function //RunMotor(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart) //RunMotor(port, 100, 1000, true, false, false); //RunMotor2(OUT_B, 100, 1000, false, false, true, OUT_C); motorParamsA.power = 100; motorParamsA.tacholimit = 1000; motorParamsA.speedreg = false; motorParamsA.holdbrake = false; motorParamsA.smoothstart = true; motorParamsB.power = 100; motorParamsB.tacholimit = 1000; motorParamsB.speedreg = false; motorParamsB.holdbrake = false; motorParamsB.smoothstart = true; motorParamsC.power = 100; motorParamsC.tacholimit = 1000; motorParamsC.speedreg = false; motorParamsC.holdbrake = false; motorParamsC.smoothstart = true; start MoveA; start MoveB; start MoveC; //Wait(4000); //NumOut(0, LCD_LINE1, MotorTachoCount(OUT_B), true); //NumOut(0, LCD_LINE2, MotorTachoCount(OUT_C)); }//end task */ task main(){ // parameter initialization string in = ""; int i = 0; byte packetType; byte port; int power = 0; long angle = 0; int turnratio = 0; byte modebits = 0; bool speedreg = false; bool holdbrake = false; bool smoothstart = false; byte port1; byte port2; const byte bit1 = 1; const byte bit2 = 2; const byte bit3 = 4; string tmp = ""; //string tmp2 = ""; // init! taskArunning = false; taskBrunning = false; taskCrunning = false; DisplayMainScreen(); //Wait(9000); // don't forget this InitSpeedFromPosLUT(); // purge mailboxes to make sure, in case something is left over, // I don't know... in = "..."; while(StrLen(in) > 0) { ReceiveRemoteString(INBOX, true, in); }//end while in = "..."; while(StrLen(in) > 0) { ReceiveRemoteString(OUTBOX, true, in); }//end while #ifdef ENABLEDEBUGGING_REMOTELOGGING in = "..."; while(StrLen(in) > 0) { ReceiveRemoteString(DEBUG_OUTBOX, true, in); }//end while #endif in = ""; #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(99, "Remote logging enabled!"); #endif while(true){ ReceiveRemoteString(INBOX, true, in); if(StrLen(in) > 0) { #ifdef ENABLEDEBUGGING_OLDLCDTIMING receivedMsgTime = CurrentTick(); // NEW DEBUG #endif #ifdef ENABLEDEBUGGING_ACOUSTICPROTOCOL PlayTone(200, 50); #endif // take first value, decide what to do... tmp = SubStr(in, 0, 1); // pos 0 packetType = StrToNum(tmp); // main packet handler! switch (packetType) { case PROTO_CONTROLLED_MOTORCMD: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // parse... tmp = SubStr(in, 1, 1); // pos 1 port = StrToNum(tmp); tmp = SubStr(in, 2, 3); // pos 2 3 4 power = StrToNum(tmp); tmp = SubStr(in, 5, 6); // pos 5 6 7 8 9 10 angle = StrToNum(tmp); tmp = SubStr(in, 11, 1); // pos 11 modebits = StrToNum(tmp); // process... // power if(power > 100) { power = -(power - 100); }//end if // bitfield if (modebits & bit1) { holdbrake = true; } else { holdbrake = false; }//end if if (modebits & bit2) { speedreg = true; } else { speedreg = false; }//end if if (modebits & bit3) { smoothstart = true; } else { smoothstart = false; }//end if // finally, the command if (port == 0) { // OUT_A motorParamsA.power = power; motorParamsA.tacholimit = angle; motorParamsA.speedreg = speedreg; motorParamsA.holdbrake = holdbrake; motorParamsA.smoothstart = smoothstart; if (taskArunning == false) { start MoveA; } else { TaskBusySignal(); } // end if } else if (port == 1) { // OUT_B motorParamsB.power = power; motorParamsB.tacholimit = angle; motorParamsB.speedreg = speedreg; motorParamsB.holdbrake = holdbrake; motorParamsB.smoothstart = smoothstart; if (taskBrunning == false) { #ifdef ENABLEDEBUGGING_OLDLCDTIMING parsedTime = CurrentTick() - receivedMsgTime; // NEW DEBUG #endif start MoveB; } else { TaskBusySignal(); } // end if } else if (port == 2) { // OUT_C motorParamsC.power = power; motorParamsC.tacholimit = angle; motorParamsC.speedreg = speedreg; motorParamsC.holdbrake = holdbrake; motorParamsC.smoothstart = smoothstart; if (taskCrunning == false) { start MoveC; } else { TaskBusySignal(); }// end if } else { // Otherwise (OUT_AB, OUT_AC, OUT_BC, OUT_ABC?) SyncPorts = port; motorParamsSync.power = power; motorParamsSync.tacholimit = angle; motorParamsSync.turnratio = 0; //turnratio; motorParamsSync.speedreg = false; //always for sync! motorParamsSync.holdbrake = holdbrake; motorParamsSync.smoothstart = smoothstart; // this doesn't look elegant at all, but it works. if ( ((port == 3) && (taskArunning == false) && (taskBrunning == false)) || ((port == 4) && (taskArunning == false) && (taskCrunning == false)) || ((port == 5) && (taskBrunning == false) && (taskCrunning == false)) ) { start MoveSync; } else { TaskBusySignal(); }//end if }//end if //PlayTone(440, 50); break; case PROTO_RESET_ERROR_CORRECTION: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - tmp = SubStr(in, 1, 1); // pos 1 port = StrToNum(tmp); if (port <= 2) { ResetErrorCorrectionAndBlockCount(port); } else if (port == 3) { // OUT_AB ResetErrorCorrectionAndBlockCount(OUT_A); ResetErrorCorrectionAndBlockCount(OUT_B); } else if (port == 4) { // OUT_AC ResetErrorCorrectionAndBlockCount(OUT_A); ResetErrorCorrectionAndBlockCount(OUT_C); } else if (port == 5) { // OUT_BC ResetErrorCorrectionAndBlockCount(OUT_B); ResetErrorCorrectionAndBlockCount(OUT_C); } else if (port == 6) { // OUT_ABC ResetErrorCorrectionAndBlockCount(OUT_A); ResetErrorCorrectionAndBlockCount(OUT_B); ResetErrorCorrectionAndBlockCount(OUT_C); }//end if break; case PROTO_ISMOTORREADY: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - tmp = SubStr(in, 1, 1); // pos 1 port = StrToNum(tmp); // send answer string: portnum, then if ready or not if (IsMotorReady(port)) { tmp = StrCat(tmp, "1"); } else { tmp = StrCat(tmp, "0"); }//end if SendMessage(OUTBOX, tmp); break; case PROTO_CLASSIC_MOTORCMD: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // parse... tmp = SubStr(in, 1, 1); // pos 1 port = StrToNum(tmp); tmp = SubStr(in, 2, 3); // pos 2 3 4 power = StrToNum(tmp); tmp = SubStr(in, 5, 6); // pos 5 6 7 8 9 10 angle = StrToNum(tmp); tmp = SubStr(in, 11, 1); // pos 11 speedreg = StrToNum(tmp); // process... // power if(power > 100) { power = -(power - 100); }//end if //% OUT_A 0x00 //% OUT_B 0x01 //% OUT_C 0x02 //% OUT_AB 0x03 //% OUT_AC 0x04 //% OUT_BC 0x05 //% OUT_ABC 0x06 if (port <= 2) { // if no current tacholimit and no new one, allow speed change if ((MotorTachoLimit(port) == 0) && (angle == 0)) { MotorCmdSingleReset(port, power, angle, speedreg); } else { if (IsMotorReady(port)) { MotorCmdSingleReset(port, power, angle, speedreg); } else { TaskBusySignal(); }//end if }//end if } else { // Otherwise (OUT_AB, OUT_AC, OUT_BC, OUT_ABC?) switch(port) { case 3: // OUT_AB port1 = 0; port2 = 1; break; case 4: // OUT_AC port1 = 0; port2 = 2; break; case 5: // OUT_BC port1 = 1; port2 = 2; break; }//end switch // if no current tacholimit and no new one, allow speed change if ((MotorTachoLimit(port1) == 0) && (MotorTachoLimit(port2) == 0) && (angle == 0)) { //~~~~BEGIN COPY PASTE CODE~~~~~~~~~~~~~~~~~~ //avoid already synced motors (that doesn't work as we know...) until((MotorRegulation(port1) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) { // repeatedly setting this is not nice, but so // we don't need a timeout...! MotorOff2(port1, port2); // make sure VM applies our settings Wait(1); }//end until MotorCmdDoubleReset(port1, power, angle, port2); //~~~~END COPY PASTE CODE~~~~~~~~~~~~~~~~~~ } else { if (IsMotorReady(port1) && IsMotorReady(port2)) { //~~~~BEGIN COPY PASTE CODE~~~~~~~~~~~~~~~~~~ //avoid already synced motors (that doesn't work as we know...) until((MotorRegulation(port1) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) { // repeatedly setting this is not nice, but so // we don't need a timeout...! MotorOff2(port1, port2); // make sure VM applies our settings Wait(1); }//end until MotorCmdDoubleReset(port1, power, angle, port2); //~~~~END COPY PASTE CODE~~~~~~~~~~~~~~~~~~ } else { TaskBusySignal(); }//end if }//end if }//end if break; }//end switch - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // reset message !!!! in = ""; }//end if // check cancel button: if (ButtonPressed(BTNCENTER, false)) { // release all motors MotorOff(OUT_A); MotorOff(OUT_B); MotorOff(OUT_C); PlayTone(440, 500); Wait(100); // release all motors again MotorOff(OUT_A); MotorOff(OUT_B); MotorOff(OUT_C); TextOut(5,LCD_LINE7, "EMERGENCY STOP ", false); TextOut(2,LCD_LINE8, "Restart program! ", false); Wait(2500); StopAllTasks(); /* // keep while(ButtonPressed(BTNCENTER, false)) { MotorOff(OUT_A); MotorOff(OUT_B); MotorOff(OUT_C); PlayTone(440, 100); Wait(500); }//end while // purge incoming queue... in = "..."; while(StrLen(in) > 0) { ReceiveRemoteString(INBOX, true, in); }//end while in = ""; // manually reset task-semaphores // not very clean, but this button is for emergencies anyway... taskArunning = false; taskBrunning = false; taskCrunning = false; // reset counters for a nice clean start ResetErrorCorrectionAndBlockCount(OUT_A); ResetErrorCorrectionAndBlockCount(OUT_B); ResetErrorCorrectionAndBlockCount(OUT_C); */ }//end if // debugging #ifdef ENABLEDEBUGGING_SHOWTASKS string taskMsg = ""; string tmpA = "_ "; string tmpB = "_ "; string tmpC = "_ "; string tmpSync = "____ "; if (taskArunning) tmpA = "A "; if (taskBrunning) tmpB = "B "; if (taskCrunning) tmpC = "C "; if (taskSyncRunning) tmpSync = "Sync "; taskMsg = StrCat(tmpA, tmpB, tmpC, tmpSync); TextOut(0, LCD_LINE7, taskMsg); #endif }//end while TextOut(0,LCD_LINE1, "MainLoop exit", true); PlayTone(500, 1000); Wait(5000); }//end task nxt-python-2.2.2/motcont/MotorControl22.rxe0000644000175000001440000011123211662270234021331 0ustar marcususers00000000000000MindstormsNXT:Ð÷Á6&€%üB    $( ,048 <@ DHLP TX\ `dhlp tx|€„ ˆŒ ”˜œ ¤¨¬°´¸¼ÀÄÈÌÐÔØÜàäèìðôø ü   $(,048<@DHL PTX\`dhlptx|€„ˆŒ”˜œ ¤¨¬°´¸¼À ÄÈÌ ÐÔØ Üàäèìðôø ü   $(,048<@D HLPTX\`dhlptx|€„ˆŒ”˜œ ¤¨¬°´¸¼ÀÄÈÌÐÔØÜàäèìðôøü    $( , 048<@DHLPTX\`dhlptx|€„ˆŒ”˜œ ¤¨¬°´¸¼ÀÄÈÌÐÔØÜàäèìðôøü  $(,048<@DHLPTVXZ\^`bdfhjlnprtvxz|~€‚„†ˆŠŒŽ’”–˜šœž ¢¤¥¦§¨©ª«¬­®¯°±²³´µ¶·¸¹º»¼½¾¿ÀÁÂÃÄÅÆÇÈÉÊËÌÍÎÏÐÑÒÓÔÕÖ×ØÙÚÛÜÝÞßàáâãäåæçèéêëìíîïðñòóôõö÷øùúûüýþÿ      $&(,0123489:<=>@DHLMNPTX\]^`dfhjklptuvx|€„…†ˆŠŒŽ’”–˜šœž ¢¤¦¨ª¬®°²´¶¸º¼¾ÀÂÄÆÈÊÌÎÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿ-  !"#$%&...stop all motorsRestart program! ÿEMERGENCY STOP ÿMainLoop exitÿÿ0ÿÿ1ÿÿMotorControl ÿÿfor FW 1.28-1.29ÿÿÿ2.2(C) RWTH AachenPress ORANGE toUniversity, LfBÿÿÿ€ 'ÿÿÿÿ(ÿÿ:ÿÿ@ÿÿŠÐŒÿÿŽ Ôä’ ø” ÿÿ– ˜ÿÿš ÿÿœÿÿž  ¢ÿÿ¤ÿÿ¦ÿÿ¨$ª4¬H®ÿÿ°ÿÿ²ÿÿ´ÿÿ¶ÿÿ¸Lºÿÿ¼\¾#ÿÿÀ ÿÿÂ!ÿÿÄ"ÿÿÆ%lÈ$|ÊÿÿÌ&ÿÿÎÿÿÿ² Ø " H ’ è > N l Š ¢ ¬ ÷"Éô•¾_ˆ) œ(—)ñ)–1[9AâBäBçBéBìBîBñBóBöB ".`$Z€…"H4úH6`Ù`HÙ`ÙEHÙ`ÙäHÙ`Ù=HÙ`ÙyHÙ`ÙxHÙ`Ù`^Ù`Ù`\Ù`ÙgHÙ`ÙfHÙ`ÙpHÙH4ÂH6`Ù`™Ù`Ù`¦Ù`Ù`¥ÙT).`¨T*$).` ¶$*@HöúH6!à60`Ù6€ÙÙ†9HÙ`ÙÙ`Ù'd,@ËHH†IH†(`Ì`HÐ`ÙÍ-@Ë%@Öÿ@HöúH6!à60`Ù6€ÙÙ†9HÙ`ÙÙ`Ù'd,@ËIH…IH†(`Ì`HÐ`ÙÍ-@Ë%@ÖÿH4úH6`ÙeÙ'd‹ ,@ËHH†IH†(`Ì`HÐ`ÙÍ-@Ë!à60`Ù6€ÙÙ†9HÙ`ÙÙ`Ù'dv !à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿvHÙ‹HO&„†Ú&„‡Ú&„„Ú&„õ‰Ú%@< !à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿ}HÙ!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿEHÙ!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿäHÙ!à60`Ù 9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿyHÙ»H9HÙ`ÙdÙ`Ù'd`Ù9HÙ»H:HÙ`Ùd€ÙÙ€ÙÙEHÙˆHRHÚšH@€ÚìÚeÚ'd `Ù`^Ù%@`Ù`^ÙˆHRHÚ›H?€ÚìÚeÚ'd `ÙxHÙ%@`ÙxHÙˆHRHÚ‘HI€ÚìÚeÚ'd `Ù`\Ù%@`Ù`\Ù„HVHÚ`Ù„ìÙ`Ù'd<»H`ÚÙH½`ÛÙ‰HQ`ÜÚ`Ú^`ÝÚ`Ú\`ÞÚ`Ú™HÚ`Ù„ìÙ`Ù'd.`H%@q).`§q*%@ÄHVHÚ`Ù„ìÙ`Ù'd<»H`ÓÙH½`ÔÙ‰HQ`ÕÚ`Ú^`ÖÚ`Ú\`×Ú`Ú¦HÚ`Ù„ìÙ`Ù'd.`J%@q).`§q*%@y„HVHÚ`Ù„ìÙ`Ù'd<»H`îÙH½`ïÙ‰HQ`ðÚ`Ú^`ñÚ`Ú\`òÚ`Ú¥HÚ`Ù„ìÙ`Ù'd.`!K%@q).`§q*%@/„HV`šÚ»H`çÙH½`èÙ`Ù`ìÙ`Ù`éÙ`Ú^`êÚ`Ú\`ëÚ„HVHÚ`Ù„ìÙ`Ù'd69HÙ`Ú™HÚ`Ù„éÙ`Ù€ÙÙeÙ`Ù'd9HÙ`Ú¦HÚ`Ù„éÙ`Ù€ÙÙeÙ`Ù'e¬9HÙ„HVHÚ`Ù„éÙ`Ù'd6:HÙ`Ú™HÚ`Ù„êÙ`Ù€ÙÙeÙ`Ù'd:HÙ`Ú¥HÚ`Ù„êÙ`Ù€ÙÙeÙ`Ù€ÙÙeÙ`Ù'eS9HÙ„HVHÚ`Ù„éÙ`Ù'd6:HÙ`Ú¦HÚ`Ù„êÙ`Ù€ÙÙeÙ`Ù'd:HÙ`Ú¥HÚ`Ù„êÙ`Ù€ÙÙeÙ`Ù€ÙÙeÙ'd.`#C%@q).`§q*%@,!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿ}HÙ„HVHÚ`Ù‚ìÙ`Ù'd)„HV`rÚ.` ª*%@¯„HVHÚ`Ù„ìÙ`Ù'd)`Ù`rÙ.` ª*)`Ù`rÙ.` ª*%@†„HVHÚ`Ù„ìÙ`Ù'd)`Ù`rÙ.` ª*)`Ù`rÙ.` ª*%@]„HVHÚ`Ù„ìÙ`Ù'd)`Ù`rÙ.` ª*)`Ù`rÙ.` ª*%@4„HVHÚ`Ù„ìÙ`Ù'd$)`Ù`rÙ.` ª*)`Ù`rÙ.` ª*)`Ù`rÙ.` ª*%@?!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿ}HÙ„HVhHÚ3€ B`= ` …= €H'd<`= 3€ B`@ ` …@ €H€ = e €H'd`= 3€ B`@ ` € @ e €H€ = e 'd ` þH %@2%@0ÉHB&„… &„† &„‡ %@ ` ™ `  %@%@` ¦ `  %@%@ ` ¥ `  %@%@ÏH eÚ'd!à6øöH6!à6 øH6!à 8,.þH8ÂH6%@!à6øöH6!à6 øH6!à 8,.þH8ÂH6,@ÆDH…ÒHø(`Ç`ÙÈ-@Æ%@M!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿ}HÙ!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿEHÙ!à60`Ù9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿäHÙ!à60`Ù 9HÙ`Ù" 86ÙþH8ÂH6!à6ø ÀÙ6ÿÿÿÿxHÙ»H9HÙ`ÙdÙ`Ù'd`Ù9HÙ»H:HÙ`Ùd€ÙÙ€ÙÙEHÙ„HVHÚ`Ù‚ìÙ`Ù'd3€ÙV9HÙ`Ù„Ù`Ù'd9HÙH½:HÙ`Ù„Ù`Ù€ÙÙeÙ'd)„HV`bÚ»HZHÙH½` Ù‰HQ`gÚ.`f*%@¶„HVhHÚ3€ B`= ` …= €H'd<`= 3€ B`@ ` …@ €H€ = e €H'd`= 3€ B`@ ` € @ e €H€ = e 'd ` þH %@2%@0ÉHB&„… &„† &„‡ %@ ` ™ `  %@%@` ¦ `  %@%@ ` ¥ `  %@%@ÏH eÚ'd)„HV`bÚ»HZHÙH½` Ù‰HQ`gÚ.`f*%@q).`§q*%@’„HV&„„Ú&„‰Ú&„ ˆÚ%@)`Ù`[Ù`ÙtHÙ%@`Ù`[Ù`ÙtHÙ%@`Ù`[Ù`ÙtHÙ%@3€Ù[9HÙ`Ù„Ù`Ù'd69HÙ3€ÙM:HÙ`Ù„Ù`Ù€ÙÙeÙ`Ù'd9HÙH½:HÙ`Ù„Ù`Ù€ÙÙeÙ'dj3€Ù[9HÙ`Ù„Ù`Ù'd9HÙ3€ÙM:HÙ`Ù„Ù`Ù€ÙÙ `ÙÙeÙ'd)`Ú[`hÚHM`dÚ.`c*`ÙîHÙ4`ÿÿÇ%@¶ÿ)`Ú[`wÚ»HAHÙH½`ÙHM`vÚ.`{*%@¨`Ú[hHÚ3€ B`= ` …= €H'd<`= 3€ B`@ ` …@ €H€ = e €H'd`= 3€ B`@ ` € @ e €H€ = e 'd ` þH %@2%@0ÉHB&„… &„† &„‡ %@ ` ™ `  %@%@` ¦ `  %@%@ ` ¥ `  %@%@ÏH eÚ`Ù'dž9HÙHMhHÚ3€ B`= ` …= €H'd<`= 3€ B`@ ` …@ €H€ = e €H'd`= 3€ B`@ ` € @ e €H€ = e 'd ` þH %@2%@0ÉHB&„… &„† &„‡ %@ ` ™ `  %@%@` ¦ `  %@%@ ` ¥ `  %@%@ÏH eÚ`Ù€ÙÙeÙ'dj3€Ù[9HÙ`Ù„Ù`Ù'd9HÙ3€ÙM:HÙ`Ù„Ù`Ù€ÙÙ `ÙÙeÙ'd)`Ú[`hÚHM`dÚ.`c*`ÙîHÙ4`ÿÿÇ%@¶ÿ)`Ú[`wÚ»HAHÙH½`ÙHM`vÚ.`{*%@q).`§q*%@H4úH6`Ù9HÙ`Ù,@Å`Ã`ÆÙ(`Á`ÙÄ-@ÅeÙ'dÎ )`Ù`mÙ.`l * )`Ù`mÙ.`l * )`Ù`mÙ.`l *`Ù¸9HÙ`Ùô:HÙ`ÙHÙ`Ù,@Ì`â`ã`åë`äÙ(` à`Ùá-@Ì`ÙdîHÙ4`ÿÿÇ )`Ù`mÙ.`l * )`Ù`mÙ.`l * )`Ù`mÙ.`l *`Ù9HÙ`Ù:HÙ!à6þ`Ù,@È`¼`½`ÀÙˆH6(` ¹`Ùº-@È`Ù9HÙ`Ù:HÙ!à6ü`Ù,@È`¼`½`ÀÙˆH6(` ¹`Ùº-@È`ÙÄ îHÙ4`ÿÿÇ`·)@·%@qõ`Ù9HÙ`Ù8:HÙ!à6`Ù,@È`¼`½`ÀÙˆH6(` ¹`Ùº-@È`Ùô9HÙ`Ùè:HÙ`ÙHÙ`Ù,@Ì`â`ã`åë`äÙ(` à`Ùá-@Ì`ÙˆîHÙ4`ÿÿÇ*`ÿÿÿÿ`×H×`×H×`×H×`× H×`×1à|íðñî×/@k`ØHØ`ØHØ`Ø HØ`Ø HØ`Ø1àjïèáâØ`ØHØ`ØHØ`Ø HØ`Ø HØ`Ø1àiïèáâØ/@n`Ü$HÜ`Ü'HÜ`Ü(HÜ`Ü%HÜ`Ü1àmÜ/@l`Ö,HÖ`Ö%HÖ`ÖHÖ`ÖHÖ`Ö1àhûôõÖ`Ö,HÖ`Ö%HÖ`ÖHÖ`ÖHÖ`Ö1àdûôõÖ/@c`Þ`eÞ`Þ`xÞ`Ûge~Û'd~`Ûe$HÛ`Þ€ÞÿÞ`eÞ`Þ`xÞ«H3HÞ`Þ HÞ`Ûe!HÛ`Ûx"HÛ`Þ HÞ`Þ1àbäåüýþÞ/@fÃHHÝ`ÝHÝ`ÝHÝ`Ý HÝ`Ý HÝ`Ý/1àwßàãæçÝÃHHÝ`ÝHÝ`ÝHÝ`Ý HÝ`Ý HÝ`Ý/1àvßàãæçÝ/@{‚H'‹H©`©1àz4©/@yH%‘H¦`¦1àu7¦H%‘H¦`¦1à7¦/@q‡H ‘H§`§ŽH§`§H§`§1àp856§/@o`¿OH¿`¿KH¿`¿1àt ¿/@s`¥(1à r¥/@ª`ÍGHÍ`Í' Í`ÍGHÍ`Íÿ& Í`ÍGHÍ`Íï& Í`ÍGHÍ`ÍÞ& Í`ÍGHÍ`ÍÍ& Í`ÍGHÍ`ͼ& Í`ÍGHÍ`Í«& Í`ÍGHÍ`Í›& Í`ÍGHÍ`ÍŠ& Í`Í GHÍ`Íy& Í`Í GHÍ`Íh& Í`Í GHÍ`ÍW& Í`Í GHÍ`ÍF& Í`Í GHÍ`Í5& Í`ÍGHÍ`Í$& Í`ÍGHÍ`Í& Í`ÍGHÍ`Í& Í`ÍGHÍ`Íñ% Í`ÍGHÍ`Íß% Í`ÍGHÍ`ÍÎ% Í`ÍGHÍ`ͽ% Í`ÍGHÍ`ͬ% Í`ÍGHÍ`Íš% Í`ÍGHÍ`͉% Í`ÍGHÍ`Íx% Í`ÍGHÍ`Íf% Í`ÍGHÍ`ÍU% Í`ÍGHÍ`ÍC% Í`ÍGHÍ`Í2% Í`ÍGHÍ`Í % Í`ÍGHÍ`Í% Í`ÍGHÍ`Íý$ Í`Í GHÍ`Íì$ Í`Í!GHÍ`ÍÚ$ Í`Í"GHÍ`ÍÈ$ Í`Í#GHÍ`Í·$ Í`Í$GHÍ`Í¥$ Í`Í%GHÍ`Í“$ Í`Í&GHÍ`Í$ Í`Í'GHÍ`Ío$ Í`Í(GHÍ`Í]$ Í`Í)GHÍ`ÍL$ Í`Í*GHÍ`Í:$ Í`Í+GHÍ`Í($ Í`Í,GHÍ`Í$ Í`Í-GHÍ`Í$ Í`Í.GHÍ`Íñ# Í`Í/GHÍ`Íß# Í`Í0GHÍ`ÍÍ# Í`Í1GHÍ`Í»# Í`Í2GHÍ`Í©# Í`Í3GHÍ`Í–# Í`Í4GHÍ`Í„# Í`Í5GHÍ`Ír# Í`Í6GHÍ`Í_# Í`Í7GHÍ`ÍM# Í`Í8GHÍ`Í:# Í`Í9GHÍ`Í(# Í`Í:GHÍ`Í# Í`Í;GHÍ`Í# Í`Í<GHÍ`Íð" Í`Í=GHÍ`ÍÞ" Í`Í>GHÍ`ÍË" Í`Í?GHÍ`͸" Í`Í@GHÍ`Í¥" Í`ÍAGHÍ`Í“" Í`ÍBGHÍ`Í€" Í`ÍCGHÍ`Ím" Í`ÍDGHÍ`ÍZ" Í`ÍEGHÍ`ÍG" Í`ÍFGHÍ`Í4" Í`ÍGGHÍ`Í!" Í`ÍHGHÍ`Í" Í`ÍIGHÍ`Íû! Í`ÍJGHÍ`Íç! Í`ÍKGHÍ`ÍÔ! Í`ÍLGHÍ`ÍÁ! Í`ÍMGHÍ`Í®! Í`ÍNGHÍ`Íš! Í`ÍOGHÍ`͇! Í`ÍPGHÍ`Ís! Í`ÍQGHÍ`Í`! Í`ÍRGHÍ`ÍL! Í`ÍSGHÍ`Í9! Í`ÍTGHÍ`Í%! Í`ÍUGHÍ`Í! Í`ÍVGHÍ`Íþ  Í`ÍWGHÍ`Íê  Í`ÍXGHÍ`ÍÖ  Í`ÍYGHÍ`Í  Í`ÍZGHÍ`ͯ  Í`Í[GHÍ`Í›  Í`Í\GHÍ`͇  Í`Í]GHÍ`Ís  Í`Í^GHÍ`Í_  Í`Í_GHÍ`ÍJ  Í`Í`GHÍ`Í6  Í`ÍaGHÍ`Í"  Í`ÍbGHÍ`Í  Í`ÍcGHÍ`Íù Í`ÍdGHÍ`Íå Í`ÍeGHÍ`ÍÑ Í`ÍfGHÍ`ͼ Í`ÍgGHÍ`ͧ Í`ÍhGHÍ`Í“ Í`ÍiGHÍ`Í~ Í`ÍjGHÍ`Íj Í`ÍkGHÍ`ÍU Í`ÍlGHÍ`Í@ Í`ÍmGHÍ`Í+ Í`ÍnGHÍ`Í Í`ÍoGHÍ`Í Í`ÍpGHÍ`Íì Í`ÍqGHÍ`Í× Í`ÍrGHÍ`Í Í`ÍsGHÍ`Í­ Í`ÍtGHÍ`͘ Í`ÍuGHÍ`Í‚ Í`ÍvGHÍ`Ím Í`ÍwGHÍ`ÍW Í`ÍxGHÍ`ÍB Í`ÍyGHÍ`Í, Í`ÍzGHÍ`Í Í`Í{GHÍ`Í Í`Í|GHÍ`Íë Í`Í}GHÍ`ÍÖ Í`Í~GHÍ`ÍÀ Í`ÍGHÍ`ͪ Í`Í€GHÍ`Í” Í`ÍGHÍ`Í~ Í`Í‚GHÍ`Íh Í`̓GHÍ`ÍR Í`Í„GHÍ`Í; Í`Í…GHÍ`Í% Í`͆GHÍ`Í Í`͇GHÍ`Íø Í`͈GHÍ`Íâ Í`͉GHÍ`ÍË Í`ÍŠGHÍ`Í´ Í`Í‹GHÍ`Íž Í`ÍŒGHÍ`͇ Í`ÍGHÍ`Íp Í`ÍŽGHÍ`ÍY Í`ÍGHÍ`ÍB Í`ÍGHÍ`Í+ Í`Í‘GHÍ`Í Í`Í’GHÍ`Íý Í`Í“GHÍ`Íå Í`Í”GHÍ`ÍΠÍ`Í•GHÍ`Í· Í`Í–GHÍ`ÍŸ Í`Í—GHÍ`͇ Í`͘GHÍ`Íp Í`Í™GHÍ`ÍX Í`ÍšGHÍ`Í@ Í`Í›GHÍ`Í( Í`ÍœGHÍ`Í Í`ÍGHÍ`Íø Í`ÍžGHÍ`Íà Í`ÍŸGHÍ`ÍÈ Í`Í GHÍ`ͯ Í`Í¡GHÍ`Í— Í`Í¢GHÍ`Í~ Í`Í£GHÍ`Íf Í`ͤGHÍ`ÍM Í`Í¥GHÍ`Í4 Í`ͦGHÍ`Í Í`ͧGHÍ`Í Í`ͨGHÍ`Íé Í`Í©GHÍ`ÍРÍ`ͪGHÍ`Í· Í`Í«GHÍ`Í Í`ͬGHÍ`Í„ Í`Í­GHÍ`Íj Í`Í®GHÍ`ÍQ Í`ͯGHÍ`Í7 Í`Í°GHÍ`Í Í`ͱGHÍ`Í Í`ͲGHÍ`Íé Í`ͳGHÍ`ÍÏ Í`Í´GHÍ`͵ Í`͵GHÍ`Íš Í`ͶGHÍ`Í€ Í`Í·GHÍ`Íe Í`͸GHÍ`ÍJ Í`͹GHÍ`Í/ Í`ͺGHÍ`Í Í`Í»GHÍ`Íù Í`ͼGHÍ`ÍÞ Í`ͽGHÍ`ÍàÍ`;GHÍ`ͧ Í`Í¿GHÍ`ÍŒ Í`ÍÀGHÍ`Íp Í`ÍÁGHÍ`ÍT Í`ÍÂGHÍ`Í8 Í`ÍÃGHÍ`Í Í`ÍÄGHÍ`Í Í`ÍÅGHÍ`Íã Í`ÍÆGHÍ`ÍÇ Í`ÍÇGHÍ`ͪ Í`ÍÈGHÍ`ÍŽ Í`ÍÉGHÍ`Íq Í`ÍÊGHÍ`ÍS Í`ÍËGHÍ`Í6 Í`ÍÌGHÍ`Í Í`ÍÍGHÍ`Íû Í`ÍÎGHÍ`ÍÞ Í`ÍÏGHÍ`ÍÀ Í`ÍÐGHÍ`Í¢ Í`ÍÑGHÍ`Í„ Í`ÍÒGHÍ`Íe Í`ÍÓGHÍ`ÍG Í`ÍÔGHÍ`Í( Í`ÍÕGHÍ`Í  Í`ÍÖGHÍ`Íê Í`Í×GHÍ`ÍË Í`ÍØGHÍ`ͬ Í`ÍÙGHÍ`ÍŒ Í`ÍÚGHÍ`Íl Í`ÍÛGHÍ`ÍL Í`ÍÜGHÍ`Í, Í`ÍÝGHÍ`Í  Í`ÍÞGHÍ`Íë Í`ÍßGHÍ`ÍÊ Í`ÍàGHÍ`Í© Í`ÍáGHÍ`͈ Í`ÍâGHÍ`Íg Í`ÍãGHÍ`ÍE Í`ÍäGHÍ`Í# Í`ÍåGHÍ`Í Í`ÍæGHÍ`ÍÞ Í`ÍçGHÍ`ͼ Í`ÍèGHÍ`Í™ Í`ÍéGHÍ`Ív Í`ÍêGHÍ`ÍR Í`ÍëGHÍ`Í/ Í`ÍìGHÍ`Í  Í`ÍíGHÍ`Íç Í`ÍîGHÍ`Í Í`ÍïGHÍ`Í Í`ÍðGHÍ`Íx Í`ÍñGHÍ`ÍS Í`ÍòGHÍ`Í- Í`ÍóGHÍ`Í Í`ÍôGHÍ`Íà Í`ÍõGHÍ`ͺ Í`ÍöGHÍ`Í“ Í`Í÷GHÍ`Ík Í`ÍøGHÍ`ÍC Í`ÍùGHÍ`Í Í`ÍúGHÍ`Íò Í`ÍûGHÍ`ÍÉ Í`ÍüGHÍ`Í  Í`ÍýGHÍ`Ív Í`ÍþGHÍ`ÍL Í`ÍÿGHÍ`Í! Í`ÍGHÍ`Íö Í`ÍGHÍ`ÍÊ Í`ÍGHÍ`Íž Í`ÍGHÍ`Íq Í`ÍGHÍ`ÍC Í`ÍGHÍ`Í Í`ÍGHÍ`Íç  Í`ÍGHÍ`͸  Í`ÍGHÍ`͈  Í`Í GHÍ`ÍX  Í`Í GHÍ`Í'  Í`Í GHÍ`Íõ  Í`Í GHÍ`Í  Í`Í GHÍ`Í  Í`ÍGHÍ`ÍZ  Í`ÍGHÍ`Í%  Í`ÍGHÍ`Íï  Í`ÍGHÍ`͸  Í`ÍGHÍ`Í€  Í`ÍGHÍ`ÍG  Í`ÍGHÍ`Í  Í`ÍGHÍ`ÍÑ  Í`ÍGHÍ`Í”  Í`ÍGHÍ`ÍV  Í`ÍGHÍ`Í  Í`ÍGHÍ`ÍÕ  Í`ÍGHÍ`Í‘  Í`ÍGHÍ`ÍL  Í`ÍGHÍ`Í  Í`ÍGHÍ`ͼ Í`ÍGHÍ`Íp Í`ÍGHÍ`Í" Í`Í GHÍ`ÍРÍ`Í!GHÍ`Í{ Í`Í"GHÍ`Í" Í`Í#GHÍ`ÍÄ Í`Í$GHÍ`Ía Í`Í%GHÍ`Íø Í`Í&GHÍ`͆ Í`Í'GHÍ`Í  Í`Í(GHÍ`̓ Í`Í)GHÍ`Íè Í`Í*GHÍ`Í0 Í`Í+GHÍ`ÍA Í`Í,GHÍ`Í, Í/@¶`Ã#EHÃ`Ã#€ÃÃEHÃ`ÃŒ €ÃÃEHÃ`ÀÃÃEHÃ`ÀÃÃ/@«/@«3€™¯`™5@™H™5@™H™`™¼€™)™‚H™HH™`™ú€™)™H™5@™‘)™`™‘'dp‘3€™¯`™`™H™`™…‘)™`™‘'d‘5@™H™`™`™3€™¯H™`™„‘)™`™‘'d‘H™3€™¯H™`™„‘&™`™‘€™)™e‘™'d ‘`™H™/@®5@™H™~Hƒ‘)™`™‘'d‘%@%@|ÿ`™H™/@®/@®`ÀQHÀ`À€ÀÀQHÀ`ÀŒ €ÀÀQHÀ`À€ÀÀQHÀ`À€ÀÀ/@±/@±3€ °àH 5@ äH 5@ ‡H ` ¼€ ' áH H„‡H ` ú€ ' ‡H 5@ –' ` –'dk–3€ °ãH  H€‡H Hƒ…–' ` –'d–5@ äH HƒàH 3€ °‡H ` „–' ` –'d–‡H 3€ °ŠH ` „–* ` –€ ' e– 'd –` ýH /@­5@ ‡H Hƒ–' ` –'d–%@%@ÿ` ýH /@­/@­FH{OHÁFH{€ÁÁOHÁ`ÁŒ €ÁÁOHÁ`Á€ÁÁOHÁ`Á€ÁÁ/@¬/@¬3€ª¸ÉHª5@ªÏHª5@ªƒHª`ª¼€ª-ªÐHª1HyƒHª`ªú€ª-ªƒHª5@ª•-ª`ª•'dk•3€ª¸ÌHª7HsƒHª4Hv…•-ª`ª•'d•5@ªÏHª4HvÉHª3€ª¸ƒHª`ª„•-ª`ª•'d•ƒHª3€ª¸„Hª`ª„•.ª`ª•€ª-ªe•ª'd •`ª Hª/@µ5@ªƒHª0Hzƒ•-ª`ª•'d•%@%@ÿ`ª Hª/@µ/@µJHxGHÂJHx€Â ÂGHÂ`˜€Â ÂGHÂ`€ ÂGHÂ`€ Â/@´/@´3€¡³òH¡5@¡öH¡5@¡ŽH¡`¡¼€¡/¡óH¡ H—ŽH¡`¡ú€¡/¡ŽH¡5@¡’/¡`¡’'dk’3€¡³õH¡H“ŽH¡ H–…’/¡`¡’'d’5@¡öH¡ H–òH¡3€¡³ŽH¡`¡„’/¡`¡’'d’ŽH¡3€¡³‡H¡`¡„’(¡`¡’€¡/¡e’¡'d ’`¡úH¡/@²5@¡ŽH¡ H”ƒ’/¡`¡’'d’%@%@ÿ`¡úH¡/@²/@²`¢<`›'d ¢`›ÿÿ'`¢`›`¢›`4¢`¢4UH¢ H•€¢÷¢äH¢3€¢·UH¢`¢„“÷¢`¢“'d“UH¢3€¢ŸVH¢`¢„“ø¢`¢“€¢÷¢ `¢¢e“¢'d“)`£·`h£`£Ÿ`d£.`c*`¢çH¢4`ÿÿ‰%@µÿ)`£·`w£`¢4xH¢ H•`¢`£Ÿ`v£.`{*`¢`,¢`¢<7`¢¢`;¢`¢;`:¢`£©e“£'d"“`¢€::¢`¢:UH¢`¢d“÷¢`¢“'d “`¢d`:¢`¢`˜¢`¢·H¢`¢µH¢`¢¸H¢`¢;UH¢`¢'€¢÷¢»H¢`¢vH¢€…„€…„`¢æH¢HˆUH¢`¢€“÷¢`¢“'d$“%@€ˆˆ†%@éÿHˆUH¢`¢ ÷¢HˆUH¢5@¢ ÷¢%@ãÿ`¢e“¢'dø“5@¢éH¢3€¢·UH¢`¢„“÷¢`¢“'e“UH¢3€¢·VH¢`¢„“ø¢`¢“€¢÷¢e“¢'d “`¢H¢/@ž3€¢·UH¢ŠHVH¢€¾øäH¾€¢÷¢UH¢`¢è€¢÷¢UH¢5@¢VH¢ŠHöH¢€¾˜äH¾€¢ø¢€¢÷¢ºH¢ŠHUH¢`¢€¢÷¢æH¢HˆUH¢`¢€“÷¢`¢“'d“`¢æH¢HˆUH¢3€¢· ÷¢HˆUH¢5@¢ ÷¢€†ŠHUH¢`¢“÷¢`¢“'d“`¢vH¢`¢,UH¢`¢„“÷¢`¢“'d“`£œe“£'dÑ“€YY†IHYUH¢`¢ƒ“÷¢`¢“'d<“`£©e“£'d“)`£·`p£`¢<~H¢.` o*%@)`£·`u£`¢<`%¢`£ŸvH£.`q*`¢`,¢%@€IHYUH¢`¢d€¢÷¢UH¢`¢€¢÷¢´H¢LHVUH¢`¢;“÷¢`¢“'d<“`£©e“£'d“)`£·`p£`¢<~H¢.` o*%@)`£·`u£`¢<`%¢`£ŸvH£.`q*`¢`,¢%@ )`£·`u£LHVUH¢`¢4€¢÷¢`%¢`£ŸvH£.`q*%@9`£©e“£'d“)`£·`p£`¢<~H¢.` o*%@)`£·`u£`¢<`%¢`£ŸvH£.`q*`¢`,¢`¢,UH¢`¢‚“÷¢`¢“'d“w)FH\7`¢¢ÖH¢.`´àHÂw*êH¢`¢4UH¢`¢“÷¢`¢“'d}“H†UH¢3€¢·€¢÷¢UH¢HŒ‚“÷¢`¢“'e “UH¢H†VH¢3€¢Ÿ€¢ø¢VH¢HŒ‚“ø¢`¢“€¢÷¢e“¢'d>“`¢`,¢FH\ëH¢`¢4UH¢ H•VH¢HŒ€¢ø¢€¢÷¢èH¢`£©e“£'d“)`£·`t£.` s*`¢çH¢4`ÿÿ‰%@z3€¢·UH¢H†€¢÷¢UH¢HŒ‚“÷¢`¢“'e “UH¢3€¢ŸVH¢H†€¢ø¢VH¢HŒ‚“ø¢`¢“€¢÷¢e“¢'d>“`¢`,¢FH\ëH¢`¢4UH¢ H•VH¢HŒ€¢ø¢€¢÷¢èH¢`£©e“£'d“)`£·`t£.` s*`¢çH¢4`ÿÿ‰`¢,UH¢`¢„“÷¢`¢“'d¿“3€¢·UH¢HŠ€¢÷¢7`¢¢³H¢H¬H¢HŒ­H¢_HU`+´`´-€´+´`+´eHO€´+´cH´H`+´`´€”+´`´”'d ”`´cH´%@H`+´`´,”+´`´”'d”`´,cH´H`+´€&+ŽH&`+´fHN€´+´`+´`´'€´+´˜H´hHL7`´´`+´`´ƒ”+´`´”'d ”hHL%@$%@"`´`+´fHN`›'d ´`›ÿÿ'`´`›`´›€´+´%@îH´¹H¢KHW¸H¢FH\UH¢GH[€¢÷¢µH¢`¢4UH¢`¢“÷¢`¢“'dE“EH]UH¢`¢VH¢`¢öH¢KHWH¢HHZ€¢2¢€¢˜¢€¢ø¢€¢÷¢UH¢`¢VH¢`¢ÈöH¢HHZ€¢˜¢€¢ø¢€¢÷¢»H¢%@BEH]UH¢`¢VH¢`¢öH¢KHWH¢HHZ€¢2¢€¢˜¢€¢ø¢€¢÷¢UH¢`¢VH¢`¢ÈöH¢HHZ€¢˜¢€¢ø¢€¢÷¢»H¢EH]UH¢`¢'€¢÷¢¼H¢DH^UH¢`¢:“÷¢`¢“'d “`¢:¼H¢%@DH^UH¢`¢€“÷¢`¢“'d“`¢¼H¢)`£·`u£`¢4UH¢DH^€¢÷¢`%¢`£ŸvH£.`q*H‹UH¢`¢€¢÷¢¶H¢5@¢UH¢JHX€“÷¢`¢“'d÷“`¢4UH¢`¢“÷¢`¢“'dL“H†UH¢3€¢·€¢÷¢UH¢`¢‚“÷¢`¢“'e!“UH¢H†VH¢3€¢Ÿ€¢ø¢VH¢`¢‚“ø¢`¢“€¢÷¢e“¢'d “`¢`,¢%@%@I3€¢·UH¢H†€¢÷¢UH¢`¢‚“÷¢`¢“'e!“UH¢3€¢ŸVH¢H†€¢ø¢VH¢`¢‚“ø¢`¢“€¢÷¢e“¢'d “`¢`,¢%@T3€¢·UH¢`¢„“÷¢`¢“'e“UH¢3€¢·VH¢`¢„“ø¢`¢“€¢÷¢e“¢'d “`¢H¢/@ž`¢,UH¢`¢‚“÷¢`¢“'d “`¢çH¢4`ÿÿ‰%@þþ`¢,UH¢`¢„“÷¢`¢“'dù“5@¢UH¢`¢^€¢÷¢¶H¢`¢4UH¢`¢“÷¢`¢“'df“5@¢UH¢JHX€“÷¢`¢“'dT“3€¢·UH¢H†ƒ“÷¢`¢“'e“UH¢3€¢ŸVH¢H†ƒ“ø¢`¢“€¢÷¢e“¢'d “`¢`˜¢%@3€¢·UH¢`¢„“÷¢`¢“'d “`¢H¢/@ž%@¡ÿ%@c5@¢UH¢JHX€“÷¢`¢“'dT“3€¢·UH¢H†‚“÷¢`¢“'e“UH¢3€¢ŸVH¢H†‚“ø¢`¢“€¢÷¢e“¢'d “`¢`˜¢%@3€¢·UH¢`¢„“÷¢`¢“'d “`¢H¢/@ž%@¡ÿ?)`£·`j£`£Ÿ`i£.`n?*%@%@ù`¢`—¢,@’`£·`³£.`²H›-@’`—£`£ `££e“£'d“)`£·`h£`£Ÿ`d£.`c*`¢çH¢4`ÿÿ‰`£—/@ž/@ž!à*ÊH*!à*äH*!à &ôH&öH*`¼IH¼`¼8KH¼!à* `¼,@È`¼`½`À¼”H*(` ¹`¼º-@È`¼IH¼`¼0KH¼!à*`¼,@È`¼`½`À¼”H*(` ¹`¼º-@È`¼IH¼`¼ KH¼!à*$`¼,@È`¼`½`À¼”H*(` ¹`¼º-@È`¼IH¼`¼KH¼!à*2`¼,@È`¼`½`À¼”H*(` ¹`¼º-@È`¼IH¼`¼KH¼!à*(`¼,@È`¼`½`À¼”H*(` ¹`¼º-@È`¼IH¼`¼KH¼!à*ú`¼,@È`¼`½`À¼”H*(` ¹`¼º-@È/@¨`¨Ð‹H¨`¨2ˆH¨`¨‰H¨`¨,@Ì`â3`ã0`å1`ä¨(` à`¨á-@Ì`¨2ÇH¨4`ÿÿo`¨Ü‹H¨`¨2ˆH¨`¨‰H¨`¨,@Ì`â3`ã0`å1`ä¨(` à`¨á-@Ì/@§`Ð`™Ð`Ð`¡Ð`ÐÚTHÐ`ÐÛ‘HÐ`ÑÜ` Ñ`ÑÝ`£Ñ`ÑÞ`¢ÑŠH$`a'd ®`aÿÿ'`®`a`®auH®‹H#•H®MHa€®C®¸H®3€®¡•H®`®„ŠC®`®Š `®®eŠ®'dŠ )`«¡`m«.`l *`®¹H®4`ÿÿg%@Ôÿ)`«¡`b«‹H#`3®MHa` ®`« `g«.`f*`®sH®ŠH$7`®®|H®„H*gH®`« eŠ«'d Š`®€®™H•H®`®dŠC®`®Š'dŠ`®dgH®`®`A®`®ºH®`®ÂH®`®¿H®„H*•H®`®'€®C®¼H®`®}H®€…„€…„`®µH®KHc•H®`®€ŠC®`®Š'd$Š%@€cc†%@éÿKHc•H®`® C®KHc•H®5@® C®%@ãÿ`®eŠ®'dôŠ5@®±H®3€®¡•H®`®„ŠC®`®Š'eŠ•H®3€®¡–H®`®„ŠD®`®Š€®C®eŠ®'d Š`®ýH®%@ó3€®¡•H®ƒH+–H®€ÉDåHÉ€®C®•H®`®è€®C®•H®5@®–H®ƒH+™H®€ÉGåHÉ€®D®€®C®½H®ƒH+•H®`®€®C®µH®KHc•H®`®€ŠC®`®Š'dŠ`®µH®KHc•H®3€®¡ C®KHc•H®5@® C®€++†ƒH+•H®`®ŠC®`®Š'dŠ`®}H®H!•H®`®„ŠC®`®Š'dîŠ`«¢eŠ«'d·Š€hh†FHh•H®`®ƒŠC®`®Š'd3Š`« eŠ«'dŠ)`«¡`p«ŠH$rH®.` o*%@)`«¡`z«ŠH$yH®.`y*`®sH®%@oFHh•H®`®d€®C®•H®`®€®C®ÀH®@Hn•H®„H*ŠC®`®Š'd3Š`« eŠ«'dŠ)`«¡`p«ŠH$rH®.` o*%@)`«¡`z«ŠH$yH®.`y*`®sH®%@)`«¡`z«@Hn•H®‹H#€®C®yH®.`y*%@0`« eŠ«'dŠ)`«¡`p«ŠH$rH®.` o*%@)`«¡`z«ŠH$yH®.`y*`®sH®H!•H®`®‚ŠC®`®Š'dÍŠ")CHk7`®®`#®.` «ëHÃ"*¶H®‹H#•H®`®ŠC®`®Š'dXŠHHf•H®3€®¡€®C®•H®JHd‚ŠC®`®Š'd<Š`®sH®CHk·H®‹H#•H®MHa–H®JHd€®D®€®C®²H®`« eŠ«'dŠ)`«¡`t«.` s*`®¹H®4`ÿÿg%@U3€®¡•H®HHf€®C®•H®JHd‚ŠC®`®Š'd<Š`®sH®CHk·H®‹H#•H®MHa–H®JHd€®D®€®C®²H®`« eŠ«'dŠ)`«¡`t«.` s*`®¹H®4`ÿÿgH!•H®`®„ŠC®`®Š'dµŠ3€®¡•H®NH`€®C®7`®®¤H®IHe£H®JHd¥H®hHR`,º`º-€º,º`,ºgHS€º,ºnHº’H(`,º`º€,º`º'd `ºnHº%@’H(`,º`º,,º`º'd`º,nHº’H(`,º€&,”H&`,ºiHQ€º,º`,º`º'€º,º–HºjHP7`ºº`,º`ºƒ,º`º'd jHP%@$%@"`º`,ºiHQ`a'd º`aÿÿ'`º`a`ºa€º,º%@ôHº¾H®>Hp¿H®CHk•H®BHl€®C®ÂH®‹H#•H®`®ŠC®`®Š'dEŠDHj•H®`®–H®`®Ä ™H®>HpœH®AHm€®J®€®G®€®D®€®C®•H®`®–H®`®Š™H®AHm€®G®€®D®€®C®¼H®%@BDHj•H®`®–H®`®Ä ™H®>HpœH®AHm€®J®€®G®€®D®€®C®•H®`®–H®`®Š™H®AHm€®G®€®D®€®C®¼H®DHj•H®`®'€®C®»H®EHi•H®™HŠC®`®Š'd Š™H»H®%@EHi•H®`®€ŠC®`®Š'dŠ`®»H®)`«¡`z«‹H#•H®EHi€®C®yH®.`y*OH_•H®`®€®C®´H®5@®•H®LHb€ŠC®`®Š'd«Š‹H#•H®`®ŠC®`®Š'd'ŠHHf•H®3€®¡€®C®•H®`®‚ŠC®`®Š'd Š`®sH®%@w%@$3€®¡•H®HHf€®C®•H®`®‚ŠC®`®Š'd Š`®sH®%@S3€®¡•H®`®„ŠC®`®Š'eŠ•H®3€®¡–H®`®„ŠD®`®Š€®C®eŠ®'d Š`®ýH®%@#H!•H®`®‚ŠC®`®Š'd Š`®¹H®4`ÿÿg%@JÿH!•H®`®„ŠC®`®Š'd¼Š5@®•H®`®^€®C®´H®‹H#•H®`®ŠC®`®Š'dKŠ5@®•H®LHb€ŠC®`®Š'd9Š3€®¡•H®HHfƒŠC®`®Š'd Š`®`A®%@3€®¡•H®`®„ŠC®`®Š'd Š`®ýH®%@•%@¼ÿ%@H5@®•H®LHb€ŠC®`®Š'd9Š3€®¡•H®HHf‚ŠC®`®Š'd Š`®`A®%@3€®¡•H®`®„ŠC®`®Š'd Š`®ýH®%@M%@¼ÿA)`«¡`|«.`kA*%@%@ú`®``®)`«¡`¯«.`®Hš*``«`«£ `««eŠ«'dŠ )`«¡`m«.`l *`®¹H®4`ÿÿg`«`%@&H«`¤Ñ`Ѥe}Ñ'd.}3€Ð…"HÐ`Є}òÐ`Ð}'d}A)`Ð`|Ð.`kA*%@ )`Ð`mÐ.`l *`Ð`™Ð*`ÿÿÿÿ`Î`¦Î`Î`]Î`ÎÓQHÎ`ÎÔ`MÎ`ÏÕ`PÏ`ÏÖHÏ`Ï×`TÏ’H`>'d ±`>ÿÿ'`±`>`±>`1±€H1šH±dHM€±K±ÖH±3€±]šH±`±„K±`± `±±e±'d )`²]`m².`l *`±ÝH±4`ÿÿŽ%@Ôÿ)`²]`b²€H1`3±dHM` ±`²P`g².`f*`±~H±’H7`±±}H±ƒH.`5±`²Pe²'d"`±€55±`±5šH±`±dK±`±'d `±d`5±`±`E±`±ÌH±`±ÍH±`±`±ƒH.šH±`±'€±K±`±`±kH±€…„€…„`±ÔH±,H…šH±`±€K±`±'d$%@€……†%@éÿ,H…šH±`± K±,H…šH±5@± K±%@ãÿ`±e±'d5@±ÁH±3€±]šH±`±„K±`±'ešH±3€±]—H±`±„H±`±€±K±e±'d `±H±%@ 3€±]šH±•H—H±€ÊHçHÊ€±K±šH±`±è€±K±šH±5@±—H±•H˜H±€ÊIçHÊ€±H±€±K±`!±•HšH±`±€±K±ÔH±,H…šH±`±€K±`±'d`±ÔH±,H…šH±3€±] K±,H…šH±5@± K±€†•HšH±`±K±`±'d`±kH±‚H/šH±`±„K±`±'dî`²Te²'d·€}}†4H}šH±`±ƒK±`±'d3`²Pe²'d)`²]`p²’HoH±.` o*%@)`²]`z²’HvH±.`y*`±~H±%@o4H}šH±`±d€±K±šH±`±€±K±ËH±5H|šH±ƒH.K±`±'d3`²Pe²'d)`²]`p²’HoH±.` o*%@)`²]`z²’HvH±.`y*`±~H±%@)`²]`z²5H|šH±€H1€±K±vH±.`y*%@0`²Pe²'d)`²]`p²’HoH±.` o*%@)`²]`z²’HvH±.`y*`±~H±‚H/šH±`±‚K±`±'dÐ)`±!7`±±`±.`±ñHÀ*ßH±€H1šH±`±K±`±'dY*H‡šH±3€±]€±K±šH±!H‚K±`±'d=`±~H±`±!ÞH±€H1šH±dHM—H±!H€±H±€±K±àH±`²Pe²'d)`²]`t².` s*`±ÝH±4`ÿÿŽ%@V3€±]šH±*H‡€±K±šH±!H‚K±`±'d=`±~H±`±!ÞH±€H1šH±dHM—H±!H€±H±€±K±àH±`²Pe²'d)`²]`t².` s*`±ÝH±4`ÿÿŽ‚H/šH±`±„K±`±'dË3€±]šH± H‘€±K±7`±±`±"H` ±!H`±`¸H¸`¸-€¸E¸H¸`¸€¸E¸qH¸H)H¸`¸€ŒE¸`¸Œ'd Œ`¸qH¸%@H)H¸`¸,ŒE¸`¸Œ'dŒ`¸,qH¸H)H¸€&E’H&H¸`¸ €¸E¸H¸`¸'€¸E¸`¸`¸7`¸¸H¸`¸ƒŒE¸`¸Œ'd Œ`¸%@$%@"`¸H¸`¸ `>'d ¸`>ÿÿ'`¸`>`¸>€¸E¸%@ùH¸`%±3H~`±`±!šH±`±%€±K±ÍH±€H1šH±`±K±`±'dI`±šH±`±—H±`±Ä ˜H±3H~‘H±`±€±B±€±I±€±H±€±K±šH±`±—H±`±Š˜H±`±€±I±€±H±€±K±`±%@F`±šH±`±—H±`±Ä ˜H±3H~‘H±`±€±B±€±I±€±H±€±K±šH±`±—H±`±Š˜H±`±€±I±€±H±€±K±`±`±šH±`±'€±K±`±`±šH±`±5K±`±'d `±5`±%@`±šH±`±€K±`±'d `±`±)`²]`z²€H1šH±`±€±K±vH±.`y*?HršH±`±€±K±ÄH±5@±šH±€Ž;°`°Ž'd±Ž€H0‹H°`°Ž;°`°Ž'd)Ž`°‹H°3€°Y€°;°‹H°`°‚Ž;°`°Ž'd Ž`°`8°%@{%@&3€°Y‹H°`°€°;°‹H°`°‚Ž;°`°Ž'd Ž`°`8°%@U3€°Y‹H°`°„Ž;°`°Ž'eŽ‹H°3€°YŒH°`°„Ž<°`°Ž€°;°eŽ°'d Ž`°VH°%@!`°8‹H°`°‚Ž;°`°Ž'd Ž`°`°4`ÿÿ%@Dÿ`°8‹H°`°„Ž;°`°Ž'd¼Ž5@°‹H°`°^€°;°ŽH°€H0‹H°`°Ž;°`°Ž'dLŽ5@°‹H°rH>€Ž;°`°Ž'd:Ž3€°Y‹H°`°ƒŽ;°`°Ž'd Ž`°`F°%@3€°Y‹H°`°„Ž;°`°Ž'd Ž`°VH°%@%@»ÿ%@I5@°‹H°rH>€Ž;°`°Ž'd:Ž3€°Y‹H°`°‚Ž;°`°Ž'd Ž`°`F°%@3€°Y‹H°`°„Ž;°`°Ž'd Ž`°VH°%@F%@»ÿA)­HYvH.`kA*%@%@ùù`°`D°t)­HY`¸.`µPH¶t*>H±HU `eŽ'd Ž )­HYgH.`l *`°`°4`ÿÿÂHD%@ÏH`XÕ`ÕXeÕ'd.3€Ô‡"HÔ`Ô„öÔ`Ô'dA)`Ô`|Ô.`kA*%@ )`Ô`mÔ.`l *`Ô`¥Ô*`ÿÿÿÿ`Óš'HÓ`Ò„€úÒ`Ò€'d‹€`Ò`™Ò`Ò`¦Ò`Ò`·Ò`ÒçjHÒ`ÒèÃHÒ`Ò`©Ò`Óê`Ó`Óë`œÓ`Ò`ŸÒ.`ž0H£jHÓ–H=e€Ó'd:€3€Ò…'HÒ`Ò„€ùÒ`Ò€'d€?)`Ò`jÒ`Ò`iÒ.`n?*%@)`Ò`hÒ`Ò`dÒ.`c*`Ò`™Ò`Ò`¦Ò%@4`Óš'HÓ`Ò„€úÒ`Ò€'d‹€`Ò`™Ò`Ò`¥Ò`Ò`·Ò`ÒçjHÒ`ÒèÃHÒ`Ò`©Ò`Óê`Ó`Óë`œÓ`Ò`ŸÒ.`ž0H£jHÓ–H=e€Ó'd:€3€Ò…'HÒ`Ò„€ùÒ`Ò€'d€?)`Ò`jÒ`Ò`iÒ.`n?*%@)`Ò`hÒ`Ò`dÒ.`c*`Ò`™Ò`Ò`¥Ò%@š`Óš'HÓ`Ò„€úÒ`Ò€'d‰€`Ò`¦Ò`Ò`¥Ò`Ò`·Ò`ÒçjHÒ`ÒèÃHÒ`Ò`©Ò`Óê`Ó`Óë`œÓ`Ò`ŸÒ.`ž0H£jHÓ–H=e€Ó'd:€3€Ò†'HÒ`Ò„€ùÒ`Ò€'d€?)`Ò`jÒ`Ò`iÒ.`n?*%@)`Ò`hÒ`Ò`dÒ.`c*`Ò`¦Ò`Ò`¥Ò*`ÿÿÿÿ/@H*`/@J*`/@K*`/@C*`€…/@Znxt-python-2.2.2/motcont/CompileAndTransferMotorControlToNXT.bat0000644000175000001440000000254211565504122025474 0ustar marcususers00000000000000@echo off REM * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * REM * The program nbc.exe must exist in the same folder as this file or be on the path! * REM * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * if exist nbc.exe goto download else goto nbcmissing :nbcmissing echo. echo The NBC/NCC compiler executable, nbc.exe, is missing. echo This file has to be in the same directory as this script echo or must reside in the Windows search path. echo. echo Go to http://bricxcc.sourceforge.net/nbc echo and download the latest (beta) binary files, echo then place nbc.exe in this folder: echo. cd echo. echo If you get compiler errors, try the very latest echo NBC version, the file should be included in echo test_release.zip from http://bricxcc.sourceforge.net echo. goto end :download echo. echo This program compiles and downloads MotorControl22.nxc echo to the NXT brick. Please connect a single NXT brick echo via a USB cable and turn it on. echo Make sure Firmware 1.28 (or higher) is installed on the NXT. echo After a successful download the NXT will beep once... echo. pause echo. echo Compiling and downloading... echo. nbc -d -S=usb -v=128 MotorControl22.nxc :end echo. echo Script finished. pausenxt-python-2.2.2/motcont/MotorFunctions.nxc0000644000175000001440000003117511565504122021514 0ustar marcususers00000000000000//#!C /* % % This component of MotorControl provides different low level helper functions % and wrappers for various NXC motor commands etc... % % Signature % Author: Linus Atorf (see AUTHORS) % Date: 2009/04/02 % Copyright: 2007-2009, RWTH Aachen University % % % *********************************************************************************************** % * This file is part of the RWTH - Mindstorms NXT Toolbox. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * You should have received a copy of the GNU General Public License along with the * % * RWTH - Mindstorms NXT Toolbox. If not, see . * % *********************************************************************************************** */ #ifndef __MOTORFUNCTIONS_NXC__ #define __MOTORFUNCTIONS_NXC__ //#define MAXTOTALBRAKINGTIME 1000 /* //TODO lower this value a bit for faster responseness? #define MOVEMENTCHECKTIME 120 //TODO set this to 0 for better accuracy? #define MAXMOVEMENTPERCHECK 1 */ #define MOTORSTOPPED_TIMEOUT 700 //in ms //TODO this can / should be lowered! very important to reduce // total execution time of motor controlling task! // this period is basically "wasted" at the end of EACH AND EVERY SINGLE // movement, so it really is a sort of "full blackout" for a motor... #define MOTORSTOPPED_RESTINGPERIOD 250 //in ms // former inline function as macro #define MotorIsRunning(_port) ((MotorRunState(_port) != OUT_RUNSTATE_IDLE) && (MotorPower(_port) != 0) && (MotorMode(_port) & OUT_MODE_MOTORON)) /* inline bool MotorIsRunning(const byte &port) { if ((MotorRunState(port) != OUT_RUNSTATE_IDLE) && (MotorPower(port) != 0) && (MotorMode(port) & OUT_MODE_MOTORON)) { return true; } else { return false; }//end if }//end function */ safecall void MotorBrake(const byte &port) { // Similar to "StopMotor(port, 'brake')", enables active braking for a motor... //MotorCmd(port, 0, 0, 0, true, false); /* byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED; // + UF_UPDATE_RESET_COUNT; byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED; byte reg = OUT_REGMODE_SPEED; byte state = OUT_RUNSTATE_RUNNING; */ SetOutput(port, Power, 0, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SPEED, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED); // + UF_UPDATE_TACHO_LIMIT); }//end MotorBrake safecall void MotorBrake2(const byte &port, const byte &port2) { SetOutput(port, Power, 0, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SPEED, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED); // + UF_UPDATE_TACHO_LIMIT); SetOutput(port2, Power, 0, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SPEED, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED); // + UF_UPDATE_TACHO_LIMIT); }//end MotorBrake2 /* safecall void MotorBrakeTest(byte port) { // Similar to "StopMotor(port, 'brake')", enables active braking for a motor... //MotorCmd(port, 0, 0, 0, true, false); byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT; // + UF_UPDATE_RESET_COUNT; byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED; byte reg = OUT_REGMODE_SPEED; byte state = OUT_RUNSTATE_RUNNING; SetOutput(port, Power, 0, TachoLimit, 0, OutputMode, mode, RegMode, reg, RunState, state, UpdateFlags, flags); }//end MotorBrake safecall void MotorBrakeSoft(byte port) { // Similar to "StopMotor(port, 'brake')", enables active braking for a motor... //MotorCmd(port, 0, 0, 0, true, false); byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED; // + UF_UPDATE_RESET_COUNT; byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; byte reg = OUT_REGMODE_IDLE; byte state = OUT_RUNSTATE_RUNNING; SetOutput(port, Power, 0, OutputMode, mode, RegMode, reg, RunState, state, UpdateFlags, flags); }//end MotorBrake */ safecall void MotorOff(const byte &port) { // Similar to "StopMotor(port, 'off')", turns off power to a motor, enabling COAST mode... //MotorCmd(port, 0, 0, 0, true, false); /* byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED; // + UF_UPDATE_RESET_COUNT; byte mode = OUT_MODE_COAST; byte reg = OUT_REGMODE_IDLE; byte state = OUT_RUNSTATE_IDLE; */ SetOutput(port, Power, 0, OutputMode, OUT_MODE_COAST, RegMode, OUT_REGMODE_IDLE, RunState, OUT_RUNSTATE_IDLE, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED); }//end MotorOff safecall void MotorOff2(const byte &port, const byte &port2) { SetOutput(port, Power, 0, OutputMode, OUT_MODE_COAST, RegMode, OUT_REGMODE_IDLE, RunState, OUT_RUNSTATE_IDLE, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED); SetOutput(port2, Power, 0, OutputMode, OUT_MODE_COAST, RegMode, OUT_REGMODE_IDLE, RunState, OUT_RUNSTATE_IDLE, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED); }//end MotorOff safecall void MotorCmdSingle(const byte &port, const int &pwr, const long &tacholimit, const bool &speedreg) { byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; byte reg = OUT_REGMODE_IDLE; if (speedreg) { mode = mode + OUT_MODE_REGULATED; reg = OUT_REGMODE_SPEED; }//end if SetOutput(port, Power, pwr, TachoLimit, tacholimit, OutputMode, mode, RegMode, reg, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT); }//end MotorCmdSingle safecall void MotorCmdSingleReset(const byte &port, const int &pwr, const long &tacholimit, const bool &speedreg) { byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; byte reg = OUT_REGMODE_IDLE; if (speedreg) { mode = mode + OUT_MODE_REGULATED; reg = OUT_REGMODE_SPEED; }//end if SetOutput(port, Power, pwr, TachoLimit, tacholimit, OutputMode, mode, RegMode, reg, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT + UF_UPDATE_RESET_COUNT); }//end MotorCmdSingleReset safecall void MotorCmdDouble(const byte &port, const int &pwr, const long &tacholimit, const byte &port2) { SetOutput( port, Power, pwr, TachoLimit, tacholimit, OutputMode, OUT_MODE_MOTORON + OUT_MODE_BRAKE + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SYNC, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT); SetOutput( port2, Power, pwr, TachoLimit, tacholimit, OutputMode, OUT_MODE_MOTORON + OUT_MODE_BRAKE + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SYNC, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT); }//end MotorCmd safecall void MotorCmdDoubleReset(const byte &port, const int &pwr, const long &tacholimit, const byte &port2) { SetOutput( port, Power, pwr, TachoLimit, tacholimit, OutputMode, OUT_MODE_MOTORON + OUT_MODE_BRAKE + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SYNC, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT + UF_UPDATE_RESET_COUNT + UF_UPDATE_RESET_BLOCK_COUNT); SetOutput( port2, Power, pwr, TachoLimit, tacholimit, OutputMode, OUT_MODE_MOTORON + OUT_MODE_BRAKE + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SYNC, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT + UF_UPDATE_RESET_COUNT + UF_UPDATE_RESET_BLOCK_COUNT); }//end MotorCmd safecall void UpdatePower(const byte &port, const int &pwr) { SetOutput(port, Power, pwr, UpdateFlags, UF_UPDATE_SPEED); }//end UpdatePower safecall void UpdatePower2(const byte &port, const int &pwr, const int &port2) { SetOutput(port, Power, pwr, UpdateFlags, UF_UPDATE_SPEED); SetOutput(port2, Power, pwr, UpdateFlags, UF_UPDATE_SPEED); }//end UpdatePower2 safecall void UpdatePowerAndEnableSpeedReg(const byte &port, const int &pwr) { SetOutput(port, Power, pwr, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SPEED, UpdateFlags, UF_UPDATE_SPEED + UF_UPDATE_MODE); }//end UpdatePowerAndEnableSpeedReg safecall void DisableSpeedRegWhithMotorOn(const byte &port) { SetOutput(port, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON, RegMode, OUT_REGMODE_IDLE, UpdateFlags, UF_UPDATE_MODE); }//end DisableSpeedRegWhithMotorOn safecall void EnableSpeedRegWhithMotorOn(const byte &port) { SetOutput(port, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SPEED, UpdateFlags, UF_UPDATE_MODE); }//end DisableSpeedRegWhithMotorOn safecall void ResetErrorCorrection(const byte &port) { // Resets TachoCount of a motor, which also resets internal error correction memory. (But not the other 2 counters) SetOutput(port, UpdateFlags, UF_UPDATE_RESET_COUNT); }//end ResetErrorCorrection safecall void ResetErrorCorrectionAndBlockCount(const byte &port) { // Resets TachoCount of a motor, which also resets internal error correction memory. // also resets BlockTachoCount with just one single call. Needed for sync driving. If not // reset, bot will "re-align" itself to last turnratio, which is usually 0, so in this // case, bot will turn to drive straight again. Nice feature if wanted, but usually // unexpected and then looks like "dancing" or totally chaotic movements SetOutput(port, UpdateFlags, UF_UPDATE_RESET_COUNT + UF_UPDATE_RESET_BLOCK_COUNT); }//end ResetErrorCorrection safecall void DebugPrintPower(byte port) { string tmp; tmp = NumToStr(MotorPower(port)); tmp = StrCat("Power=", tmp); TextOut(0,LCD_LINE2, tmp); }//end /* inline void MotorCmd(byte port, int pwr, long tacholimit, int turnratio, bool speedreg, bool sync) { byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; byte reg = OUT_REGMODE_IDLE; if (speedreg) { mode = mode + OUT_MODE_REGULATED; reg = OUT_REGMODE_SPEED; }//end if if (sync) { mode = mode + OUT_MODE_REGULATED; reg = OUT_REGMODE_SYNC; }//end if SetOutput(port, Power, pwr, TachoLimit, tacholimit, TurnRatio, turnratio, OutputMode, mode, RegMode, reg, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT); }//end MotorCmd safecall void MotorCmdEx(byte port, int pwr, long tacholimit, int turnratio, bool speedreg, bool sync, byte runstate) { byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT; // + UF_UPDATE_RESET_COUNT; byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; byte reg = OUT_REGMODE_IDLE; byte state = runstate; if (speedreg) { mode = mode + OUT_MODE_REGULATED; reg = OUT_REGMODE_SPEED; }//end if if (sync) { mode = mode + OUT_MODE_REGULATED; reg = OUT_REGMODE_SYNC; }//end if SetOutput(port, Power, pwr, TachoLimit, tacholimit, TurnRatio, turnratio, OutputMode, mode, RegMode, reg, RunState, state, UpdateFlags, flags); }//end MotorCmdEx */ #endif nxt-python-2.2.2/motcont/ControllerCore.nxc0000644000175000001440000011134711565504122021457 0ustar marcususers00000000000000//#!C /* % Part of MotorControl containing the main PD-controller function, to be % included multiple times via preprocessor tricks :-) % % Signature % Author: Linus Atorf (see AUTHORS) % Date: 2009/06/28 % Copyright: 2007-2009, RWTH Aachen University % % % *********************************************************************************************** % * This file is part of the RWTH - Mindstorms NXT Toolbox. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * The RWTH - Mindstorms NXT Toolbox 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. * % * * % * You should have received a copy of the GNU General Public License along with the * % * RWTH - Mindstorms NXT Toolbox. If not, see . * % *********************************************************************************************** */ // this function CAN and HAS TO BE INCLUDED MULTIPLE TIMES!!! // Macro which adds A, B, or C to the end of a function/taskname or constant // useful when including the file multiple times to create mutliple identical // functions with different names for thread-safety (making them inline didn't // work somehow). #define _ADD_MOTORNAME(funcname) funcname##RUNMOTOR_CURRENTPORTNAME #ifdef RUNMOTOR2_SYNCMODE safecall long GetAbsBrakingDistFromAbsSpeed2(const long &absSpeed) { // absolute max realistic speed with full batteries (not akkus) is 1100 deg per sec //TODO maybe the plus-term below is not necessary after all... // to avoid braking mode being skipped... return ((absSpeed * absSpeed) / 2200 ) + ENDGAMEBRAKINGSTARTDIST + 5; }//end function #else safecall long _ADD_MOTORNAME(GetAbsBrakingDistFromAbsSpeed) (const long &absSpeed) { // absolute max realistic speed with full batteries (not akkus) is 1100 deg per sec //return (absSpeed / 4) + 30; //TODO maybe the plus-term below is not necessary after all... // to avoid braking mode being skipped... return ((absSpeed * absSpeed) / 2700 ) + ENDGAMEBRAKINGSTARTDIST + 1; }//end function #endif safecall bool _ADD_MOTORNAME(WaitUntilMotorStopped)(const byte &port) { /* RETURNS true if stopped by direct command This function waits until a braked motor has come to a full stop. The "trick" is that it's not only beeing waited until the motor has stopped for a very short time, but for certain period. This even works with oscillating movements as seen when jamming in the brake at full speed. Only monitoring if the motor "is at the same position as last time" wouldn't work because the oscillation could come accross the same position twice... This loop should work with different execution speeds, too. Actually I don't see quite why this function is this complex, but I'm pretty sure I had my reasons when MotorControl 1.0 and 1.1 were developed... */ //much simpler approach now... long lastPos = MotorTachoCount(port); long curPos; long lastChangedTick = CurrentTick(); long timeoutTick = CurrentTick() + MOTORSTOPPED_TIMEOUT; long tmp; // loop while in motion... while ((lastChangedTick + MOTORSTOPPED_RESTINGPERIOD) > CurrentTick()) { curPos = MotorTachoCount(port); // reset "last motion" timepoint if (lastPos != curPos) { lastChangedTick = CurrentTick(); }//end if lastPos = curPos; // check if direct command stopped us (indicated by TachoLimit...) if ((MotorTachoLimit(port) == 0) && (MotorPower(port) == 0)) { //direct command must have stopped us! #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "ABORTED(wait4Stop) "); Wait(300); #endif return true; }//end if if (CurrentTick() >= timeoutTick) { break; }//end if }//end while return false; }//end void // To get functionality for TWO SYNCED MOTORS, do the following: // Always take RunMotor, copy-paste it, call it RunMotor2, add another // argument port2 to it, and done. BEFORE the function, define RUNMOTOR2_SYNCMODE // note how RunMotor2 doesn't need to be inline and takes 1 more argument... // RETURN is wether a direct command stopped us!!!! #ifdef RUNMOTOR2_SYNCMODE bool RunMotor2(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart, const byte &port2) { #else inline bool _ADD_MOTORNAME(RunMotor) (const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart) { #endif /* The whole concept works like this: There is a loop with static execution time for worst case -- if it's faster, there's busy waiting at the end. Durinc decelaration / braking, we control the motor with a PD-controller algorithm. The constant loop execution time is important, as a varying delta T for the controller is really bad. It would require a different set of KP and KD parameters, and the speed calculation the way it's currently implemented would also change its sliding integration window, so we stick with constant loop timing. Execution speed can vary since in the worst case, all 3 motors are controlled by this function, running in 3 threads at once. There is a 4th thread to listen for incoming messages, so we only have 1 quarter of the NXT's original CPU power left per thread. We don't want to program statically, since than we'd allways have the worst case. Hence the dynamic busy waiting at the end, while still intelligently checking if the desired motor position has been reached (so the waiting is still useful in some way). So we've got a loop with constant execution time, and a PD-controller during deceleration. The motor-power will constantly be set down, linearly, dependent on its current position. This way, if it get's stuck (i.e. during braking the load somehow increases or the motor blocks), the controller adjusts the power accordingly (the controller has a given set-point for power, depending where in relation to our position target we stand). Since the loop is fairly slow in the worst case, we've got time to spare in the best case (2 threads running, 1 motor + 1 message listener). During the waiting time, we check if the motors get's close to its target -- this only matters during BRAKE stage. Yes, inside the big outer loop, there are different stages. During RAMPUP we can implement a manual rampup (power constantly increasing) for a smooth start. During driving, nothing happens but measuring speed and deciding when the whole braking/deceleration should begin. Then there's the BRAKING stage, with the already mentioned PD controller in action. This is what's actually important. To get precision, we have to very closely monitor when our desired target is reached. In order to achieve this, we've got the ENDGAME mode. ENDGAME phase will be entered a couple of degrees before the motor reaches its goal. The PD controller should have succeded by now to enforce a really low turning speed of the motor (just as we targeted/specified in our desired "speed at motor position" deceleration curve lookup table). The most important and most critical thing of the whole function is now to come to a stop at exactly the right position. All CPU time we have is needed to monitor the motor as often as possible and enable the brake at the target position. This is whats called the endgame inner loop. To avoid the very unlikely event of a lock-up (the motor gets blocked just a tiny degree or two befor its final goal), there's also a timeout. The idea of "stages" (or phases) during a main loop was chosen so that short movements are possible with the same code/function. If we had different loops after each other, we'd have to check for abort conditions all the time, and include speed monitoring in each loop. This way with stages in one loop, the algorithm (or command flow) can proceed from stage to stage, even if the current stage has to be ended premature (i.e. a motor is not fully ramped up yet, but already has to start braking in order to reach its target safely). */ // **** THINGS NOT IN THIS FUNCTION // - decision if NO CONTROLLED LOOP is needed (classic way?!) // - defer as much decision etc as possible to MATLAB, see below // **** More lookups needed: // - Brakingdist from Speed // - Absolute acceleration power during rampup (in discrete steps like 10, 20, etc) // --- Some DECLARATIONS -- not all of them, this is timecritical long i; int powerSgn = sign(power); long tachoTarget = powerSgn * tacholimit; // --- Reset counters etc // * See below, now happens implicitely in MotorCmd*Reset // --- Decide if SIMPLE FUNCTION needed? // * For small powers! // * For (very?) short distances??? // --- Start driving (important that its EARLY in this function -- for direct commands) // * Runstate RUNNING // * power = +-1 // * tacholimit already... //TODO why exactly do we only power up to +/-1 here? couldn't it be full speed already? // maybe it's because for speed monitor initialization, we use TachoCount and need it to remain // constant etc? //avoid already synced motors (that doesn't work as we know...) //turning off motors is in general not a bad idea to make sure everything works :-) #ifdef RUNMOTOR2_SYNCMODE until((MotorRegulation(port) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) { #else until(MotorRegulation(port) == OUT_REGMODE_IDLE) { #endif // repeatedly setting this is not nice, but so // we don't need a timeout...! #ifndef RUNMOTOR2_SYNCMODE MotorOff(port); #else MotorOff2(port, port2); #endif // make sure VM applies our settings Wait(1); }//end until #ifdef RUNMOTOR2_SYNCMODE MotorCmdDoubleReset(port, powerSgn, tacholimit, port2); #else MotorCmdSingleReset(port, powerSgn, tacholimit, speedreg); #endif #ifdef ENABLEDEBUGGING_OLDLCDTIMING if (port == OUT_B) { // works only for motor B, by design motorStartedTime = CurrentTick() - receivedMsgTime; }//end if #endif // --- Declare and Initialize various variables // * STAGE is RAMPUP int curStage = STAGE_RAMPUP; int absPower = abs(power); int maxAbsBrakingPower; maxAbsBrakingPower = absPower; if (speedreg) { maxAbsBrakingPower += ADDITIONALMAXSPEEDREGBRAKINGPOWER; if (maxAbsBrakingPower > 100) { maxAbsBrakingPower = 100; }//end if }//end if long absBrakingDist; long brakingStartSpeed; long brakingStartTacho; long loopStartTick; long waitEndTick; bool endgameSuccessful = false; long rampupLoopCount = 0; long newPower; //PID stuff long err_0 = 0; // current error (t = 0) long err_1 = 0; // prev. error (t = - T) long curPIDPowerUpscaled = absPower * PIDUPSCALING; long newAbsPower; long desiredSpeed; // speed stuff long curSpeed; int SpeedLogIndex = 0; long TachoCountLog[SPEEDHISTORY]; long TickCountLog[SPEEDHISTORY]; // init speedlog for(i = 0; i < SPEEDHISTORY; i++) { TachoCountLog[i] = 0; // should still be 0, was reset just a moment ago TickCountLog[i] = CurrentTick(); //careful, check if this works... }//end for #ifdef ENABLEDEBUGGING_LCD long loopCount = 0; long lastTick = CurrentTick(); long lastLoopTime; long totalStartTick = CurrentTick(); long totalEndTick; long totalControlTime; long endgameLoopCount = 0; string lcdTmp; string lcdTmp1; string lcdTmp2; #endif// - - - - - - - - - - #ifdef ENABLEDEBUGGING_REMOTE string tmp; string tmp1; string tmp2; string tmp3; bool brakingStartMarkerSent = false; #endif// - - - - - - - - - - #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG string lcdTmp; int DebugLinePos[3]; DebugLinePos[0] = LCD_LINE1; DebugLinePos[1] = LCD_LINE3; DebugLinePos[2] = LCD_LINE5; int DebugLinePos2[3]; DebugLinePos2[0] = LCD_LINE2; DebugLinePos2[1] = LCD_LINE4; DebugLinePos2[2] = LCD_LINE6; TextOut(0, LCD_LINE8, "SLOW ANTIBUG ON ", true); #endif// - - - - - - - #ifdef ENABLEDEBUGGING_ACOUSTIC PlayTone(300,100); #endif #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Entering main loop"); #endif // --- MAIN LOOP --------------------- while(true) { // * record time loopStartTick = CurrentTick(); #ifdef ENABLEDEBUGGING_LCD loopCount++; lastLoopTime = CurrentTick() - lastTick; lastTick = CurrentTick(); #endif// - - - - - - - - - - // * check STOP CONDITIONS? // - already there? // - direct command stopped us? // this is like an emergency-check to see if endgame should be enabled? //TODO do we need this below? /* if ( abs(tachoTarget - MotorTachoCount(port)) >= 2 ) { // we are almost already there :-) // or even too far :-/ }//end if */ if ((MotorPower(port) == 0) || (MotorTachoLimit(port) == 0)) { //direct command must have stopped us! #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "ABORTED(start) "); Wait(300); #endif return true; }//end if // * RECORD SPEED // - with this long loop execution time, use speed history of 2 // - 2 vars, toggle between history... curSpeed = ((MotorTachoCount(port) - TachoCountLog[SpeedLogIndex]) * 1000) / (CurrentTick() - TickCountLog[SpeedLogIndex]); //TODO optimize speed logging by using only 2 history values and toggling between them // record speedlog data to previous index... i = SpeedLogIndex - 1; if (i < 0) i = SPEEDHISTORY-1; TachoCountLog[i] = MotorTachoCount(port); TickCountLog[i] = CurrentTick(); // move current index for next reading SpeedLogIndex++; if (SpeedLogIndex > SPEEDHISTORY-1) SpeedLogIndex = 0; // * if RAMPUP STAGE: // - increase power (set according value from lookup / count loop-passes) // - enter NEXT STAGE when power is reached... // - maybe enable SPEED REG if not already done? if (curStage == STAGE_RAMPUP) { #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "RAMPUP "); #endif // do we need rampup at all? if (smoothstart) { rampupLoopCount++; // when rampup is done if (rampupLoopCount >= SMOOTHSTARTSTEPS) { if (speedreg) { //there's no speedreg for synced driving... UpdatePowerAndEnableSpeedReg(port, power); } else { #ifndef RUNMOTOR2_SYNCMODE UpdatePower(port, power); #else UpdatePower2(port, power, port2); #endif }//end if curStage = STAGE_DRIVING; } else { // simple linear interpolation between 0->100 newPower = ((rampupLoopCount * 100)/SMOOTHSTARTSTEPS); // if power < 100 we might be done sooner: if (newPower > absPower) { if (speedreg) { //there's no speedreg for synced driving... UpdatePowerAndEnableSpeedReg(port, power); } else { #ifndef RUNMOTOR2_SYNCMODE UpdatePower(port, power); #else UpdatePower2(port, power, port2); #endif }//end if curStage = STAGE_DRIVING; } else { // here's the actual rampup step! #ifndef RUNMOTOR2_SYNCMODE UpdatePower(port, newPower * powerSgn); #else UpdatePower2(port, newPower * powerSgn, port2); #endif }//end if }//end if } else { // no rampup // full power right away if (speedreg) { //there's no speedreg for synced driving... UpdatePowerAndEnableSpeedReg(port, power); } else { #ifndef RUNMOTOR2_SYNCMODE UpdatePower(port, power); #else UpdatePower2(port, power, port2); #endif }//end if curStage = STAGE_DRIVING; }//end if }//end if // * if DRIVING STAGE: // - look up current braking distance from current speed // - decide: stage change? time to brake? // - enter braking stage, set braking dist! // - disable speed reg if necessary... // Actually do this during either rampup or driving // (that's the whole idea: braking even if already necessary during rampup) //optimized, OLD: if ((curStage == STAGE_DRIVING) || (curStage == STAGE_RAMPUP)) { if (curStage <= STAGE_DRIVING) { #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG if (curStage == STAGE_DRIVING) { TextOut(0, DebugLinePos[port], "DRIVING "); }//end if #endif // we've got two different Bremsweg-Algos (sync driving needs a bit more way ahead) #ifdef RUNMOTOR2_SYNCMODE absBrakingDist = GetAbsBrakingDistFromAbsSpeed2(abs(curSpeed)); #else absBrakingDist = _ADD_MOTORNAME(GetAbsBrakingDistFromAbsSpeed)(abs(curSpeed)); #endif // make the following safe for both power signs! if (powerSgn > 0) { // a little dirty preprocessor hack... #ifdef RUNMOTOR2_SYNCMODE if ((tachoTarget - MotorTachoCount(port)) <= absBrakingDist || (tachoTarget - MotorTachoCount(port2)) <= absBrakingDist ) { #else if ((tachoTarget - MotorTachoCount(port)) <= absBrakingDist ) { #endif //~~~~~~~~~~~~~~~~~~ same code from here // ok, we're close enough, start braking! curStage = STAGE_BRAKING; // record this for later down (for speed lookup) brakingStartSpeed = curSpeed; brakingStartTacho = powerSgn * (tacholimit - absBrakingDist); // disable SPEED REG if needed if (speedreg) { // again no need to care about synced driving DisableSpeedRegWhithMotorOn(port); Wait(1); // for FW to process }//end if //~~~~~~~~~~~~~~~~~~ same code until here }//end if } else { #ifdef RUNMOTOR2_SYNCMODE if (((MotorTachoCount(port) - tachoTarget) <= absBrakingDist) || ((MotorTachoCount(port2) - tachoTarget) <= absBrakingDist)) { #else if ((MotorTachoCount(port) - tachoTarget) <= absBrakingDist ) { #endif //~~~~~~~~~~~~~~~~~~ same code from here // ok, we're close enough, start braking! curStage = STAGE_BRAKING; // record this for later down (for speed lookup) brakingStartSpeed = curSpeed; brakingStartTacho = powerSgn * (tacholimit - absBrakingDist); // disable SPEED REG if needed if (speedreg) { // again no need to care about synced driving DisableSpeedRegWhithMotorOn(port); Wait(1); // for FW to process }//end if //~~~~~~~~~~~~~~~~~~ same code until here }//end if }//end if }//end if // * if BRAKING STAGE: // ( - check endbraking special mode conditions ) // ( . maybe skip PID if time for endgame? ) // - PID control // . calc current error // . calc new setpoint // . use KP and KD constants as MACROS (#define), not VARS! // . clip power and set if (curStage == STAGE_BRAKING) { #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "BRAKING:top "); #endif #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Braking: Beginning"); #endif #ifdef ENABLEDEBUGGING_REMOTE if(!brakingStartMarkerSent) { SendMessage(OUTBOX, "1050|0|0"); brakingStartMarkerSent = true; }//end if #endif// - - - - - - - - - - // TODO maybe replace this variable (dont use it) and do it inline? desiredSpeed = GetIdealSpeedFromPos(abs(MotorTachoCount(port) - brakingStartTacho), brakingStartSpeed, absBrakingDist); // shift/calculate all errors for current cycle err_1 = err_0; err_0 = curSpeed - desiredSpeed; //replace desiredSpeed with inline? #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "BRAKING:speed/err "); #endif //#ifdef ENABLEDEBUGGING_REMOTELOGGING // SendDebugMessage(port, "Braking: IdealSpeed / error calced"); //#endif // use different KD, KP consts for sync: #ifdef RUNMOTOR2_SYNCMODE // Our PD-Controller is still dependent on turning direction... if (powerSgn > 0) { curPIDPowerUpscaled = curPIDPowerUpscaled + (HARDCODED_KD_SYNC * (err_0 - err_1)) + (HARDCODED_KP_SYNC * err_1); } else { curPIDPowerUpscaled = curPIDPowerUpscaled - (HARDCODED_KD_SYNC * (err_0 - err_1)) - (HARDCODED_KP_SYNC * err_1); }//end if #else // Our PD-Controller is still dependent on turning direction... if (powerSgn > 0) { curPIDPowerUpscaled = curPIDPowerUpscaled + (HARDCODED_KD * (err_0 - err_1)) + (HARDCODED_KP * err_1); } else { curPIDPowerUpscaled = curPIDPowerUpscaled - (HARDCODED_KD * (err_0 - err_1)) - (HARDCODED_KP * err_1); }//end if #endif #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "BRAKING:PIDdone "); #endif //#ifdef ENABLEDEBUGGING_REMOTELOGGING // SendDebugMessage(port, "Braking: PID calc done, clipping power"); //#endif // downscaling newAbsPower = curPIDPowerUpscaled / PIDUPSCALING; // clip these values! if (newAbsPower > maxAbsBrakingPower) { newAbsPower = maxAbsBrakingPower; } else if (newAbsPower < 1) { newAbsPower = 1; }//end if #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "BRAKING:PwrCalced "); #endif #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Braking: Setting new power"); #endif //finally set new power! #ifndef RUNMOTOR2_SYNCMODE UpdatePower(port, powerSgn * newAbsPower); #else UpdatePower2(port, powerSgn * newAbsPower, port2); #endif #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG lcdTmp = NumToStr(newAbsPower); lcdTmp = StrCat("BRAKING:Pwr=", lcdTmp, " "); TextOut(0, DebugLinePos[port], lcdTmp); #endif #ifdef ENABLEDEBUGGING_REMOTE tmp1 = NumToStr(err_0); tmp2 = NumToStr(desiredSpeed); tmp3 = NumToStr(curSpeed); tmp = StrCat(tmp1, "|", tmp2, "|", tmp3); SendMessage(OUTBOX, tmp); #endif// - - - - - - - - - - }//end if // * WAIT // - burn constant CPU time by waiting for max delay... // - keep on checking tachocount for ENDGAME MODE? // - (MAYBE keep on checking direct command cancellation?) #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos2[port], " waitloop "); #endif waitEndTick = loopStartTick + LOOP_DURATION; //precalc loop-end while (CurrentTick() < waitEndTick) { //already there? check for endgame mode if (powerSgn > 0) { // this nice little line also detects if the tachotarget is way // out of reach already (not a nice case though) #ifdef RUNMOTOR2_SYNCMODE if (((tachoTarget - MotorTachoCount(port)) <= ENDGAMEBRAKINGSTARTDIST) || ((tachoTarget - MotorTachoCount(port2)) <= ENDGAMEBRAKINGSTARTDIST)) { #else if ((tachoTarget - MotorTachoCount(port)) <= ENDGAMEBRAKINGSTARTDIST) { #endif curStage = STAGE_ENDGAME; #ifdef ENABLEDEBUGGING_ACOUSTIC PlayTone(1000,100); #endif break; }//end if } else { // modified to work for negative stuff... #ifdef RUNMOTOR2_SYNCMODE if (((MotorTachoCount(port) - tachoTarget) <= ENDGAMEBRAKINGSTARTDIST) || ((MotorTachoCount(port2) - tachoTarget) <= ENDGAMEBRAKINGSTARTDIST)) { #else if ((MotorTachoCount(port) - tachoTarget) <= ENDGAMEBRAKINGSTARTDIST) { #endif curStage = STAGE_ENDGAME; #ifdef ENABLEDEBUGGING_ACOUSTIC PlayTone(1000,100); #endif break; }//end if }//end if //direct command stopped us? if ((MotorPower(port) == 0) || (MotorTachoLimit(port) == 0)) { #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "ABORTED(wait) "); Wait(300); #endif return true; }//end if #ifdef SLEEP_DURING_WAIT_WHEN_DRIVING if (curStage <= STAGE_DRIVING) { // free some CPU time... Wait(1); }//end if #endif }//end while (times out when static worst case looptime is over (~10 to 20ms) #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos2[port], " "); #endif #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Wait-loop done"); #endif // * if SPECIAL ENDGAME BRAKING // - FAST & SIMPLE INNER LOOP!!! // . check position // . enable HARD BRAKE if necessary, jump out of loop if (curStage == STAGE_ENDGAME) { #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "ENDGAME "); #endif #ifdef ENABLEDEBUGGING_REMOTE SendMessage(OUTBOX, "-300|0|0"); #endif// - - - - - - - - - - #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Endgame begins"); #endif // fast inner loop now! // don't forget TIMEOUT or something, avoid possible lock up, // unfortunately it is possible... waitEndTick = CurrentTick() + ENDGAME_INNERLOOP_TIMEOUT; //precalc loop-end // make two loops to save execution time! if (powerSgn > 0) { while (CurrentTick() < waitEndTick) { #ifdef RUNMOTOR2_SYNCMODE if ((MotorTachoCount(port) >= tachoTarget) || (MotorTachoCount(port2) >= tachoTarget)) { #else if (MotorTachoCount(port) >= tachoTarget) { #endif //WE'RE THERE!!! endgameSuccessful = true; break; }//end if // even for syncmode, only check first motor... if (MotorTachoLimit(port) == 0) { //direct command must have stopped us! #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "ABORTED(endgame) "); Wait(300); #endif return true; }//end if #ifdef ENABLEDEBUGGING_LCD endgameLoopCount++; #endif }//end while } else { // negative power sign while (CurrentTick() < waitEndTick) { #ifdef RUNMOTOR2_SYNCMODE if ((MotorTachoCount(port) <= tachoTarget) || (MotorTachoCount(port2) <= tachoTarget)) { #else if (MotorTachoCount(port) <= tachoTarget) { #endif //WE'RE THERE!!! endgameSuccessful = true; break; }//end if // even for syncmode, only check first motor... if (MotorTachoLimit(port) == 0) { //direct command must have stopped us! #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG TextOut(0, DebugLinePos[port], "ABORTED(endgame) "); Wait(300); #endif return true; }//end if #ifdef ENABLEDEBUGGING_LCD endgameLoopCount++; #endif }//end while }//end if //if succesful or not, BRAKE (before debug)! #ifndef RUNMOTOR2_SYNCMODE MotorBrake(port); #else MotorBrake2(port, port2); #endif //now there's a little time for debug... #ifdef ENABLEDEBUGGING_ACOUSTIC if (!endgameSuccessful) { PlayTone(4000,100); }//end if #endif #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG if (endgameSuccessful) { TextOut(0, DebugLinePos[port], "ENDGAME SUCCESS "); } else { TextOut(0, DebugLinePos[port], "ENDGAME TIMEOUT "); }//end if #endif #ifdef ENABLEDEBUGGING_REMOTELOGGING if (endgameSuccessful) { SendDebugMessage(port, "Endgame successful"); } else { SendDebugMessage(port, "Endgame timed out"); }//end if #endif // and this control process is finally over! break; }//end if (stage ENDGAME) }//end while // --- END MAIN LOOP --------------------- #ifdef ENABLEDEBUGGING_LCD totalEndTick = CurrentTick(); totalControlTime = CurrentTick() - totalStartTick; #endif // --- seems obsolete: if loop exited manually by direct command, exit void! // (because clean exit and then return should all be above inside loop) // --- monitor braking (as in old MotorControl) // * release when necessary at full stop //TODO what happens if during these final and "save" (esasy) stopping/braking // phase, a new direct command arrives and does weired stuff? it will definitely // mess up our current motor state, maybe that's not too bad... #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Waiting for Motorstop"); #endif bool stoppedByDirectCmd = false; stoppedByDirectCmd = _ADD_MOTORNAME(WaitUntilMotorStopped)(port); // --- Motor off / idle, over & out? if (!holdbrake) { #ifndef RUNMOTOR2_SYNCMODE MotorOff(port); #else MotorOff2(port, port2); #endif }//end if // tiny wait for motorstop..., sometimes it really is better... Wait(3); #ifdef ENABLEDEBUGGING_REMOTELOGGING SendDebugMessage(port, "Finished!"); #endif #ifdef ENABLEDEBUGGING_REMOTE SendMessage(OUTBOX, "-480|0|0"); #endif// - - - - - - - - - - // --- Display Debug Info if necessary #ifdef ENABLEDEBUGGING_LCD lcdTmp = NumToStr(loopCount); lcdTmp = StrCat("loops=", lcdTmp); TextOut(0,LCD_LINE1, lcdTmp, true); lcdTmp = NumToStr(totalControlTime / loopCount); lcdTmp = StrCat("t/loop=", lcdTmp); TextOut(0,LCD_LINE2, lcdTmp); lcdTmp = NumToStr((loopCount * 1000) / totalControlTime); lcdTmp = StrCat("loops/s=", lcdTmp); TextOut(0,LCD_LINE3, lcdTmp); lcdTmp = NumToStr(lastLoopTime); lcdTmp = StrCat("lastloop=", lcdTmp); TextOut(0,LCD_LINE4, lcdTmp); lcdTmp1 = NumToStr(MotorTachoCount(port)); lcdTmp2 = NumToStr(tacholimit); lcdTmp = StrCat("tacho=", lcdTmp1, "(", lcdTmp2, ")"); TextOut(0,LCD_LINE5, lcdTmp); // endgame duration & count lcdTmp1 = NumToStr(endgameLoopCount); lcdTmp2 = NumToStr(totalEndTick - (waitEndTick - ENDGAME_INNERLOOP_TIMEOUT)); lcdTmp = StrCat("endg=", lcdTmp1, " in ", lcdTmp2, "ms"); TextOut(0,LCD_LINE6, lcdTmp); #ifdef ENABLEDEBUGGING_WAITAFTERLCD Wait(5000); //until (ButtonPressed(BTNCENTER, false)); #endif #endif #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG Wait(200); TextOut(0, DebugLinePos[port], "FINISHED "); #endif // return wether a direct command stopped us... return stoppedByDirectCmd; }//end void // the following function must be an excact copy-paste-replica of the RunMotor // function from above, but with the name RunMotor2, the additional param port2, // RUNMOTOR2_SYNCMODE defined, and always speedreg=false. It also doesn't need // to be inline (to save space)... nxt-python-2.2.2/motcont/CompileMotorControlHere.bat0000644000175000001440000000140011565504122023243 0ustar marcususers00000000000000@echo off if exist nbc.exe goto compile else goto nbcmissing :nbcmissing echo. echo The NBC/NCC compiler executable, nbc.exe, is missing. echo This file has to be in the same directory as this script echo or must reside in the Windows search path. echo. echo Go to http://bricxcc.sourceforge.net/nbc echo and download the latest (beta) binary files, echo then place nbc.exe in this folder: echo. cd echo. echo If you get compiler errors, try the very latest echo NBC version, the file should be included in echo test_release.zip from http://bricxcc.sourceforge.net echo. goto end :compile echo. echo Compiling... nbc -v=128 -O=MotorControl22.rxe MotorControl22.nxc :end echo. echo Done. echo. pausenxt-python-2.2.2/nxt/0000755000175000001440000000000011760013147015127 5ustar marcususers00000000000000nxt-python-2.2.2/nxt/error.py0000644000175000001440000000572711565504062016651 0ustar marcususers00000000000000# nxt.error module -- LEGO Mindstorms NXT error handling # Copyright (C) 2006, 2007 Douglas P Lau # Copyright (C) 2009 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. 'Declarations for the errors' class ProtocolError(Exception): pass class SysProtError(ProtocolError): pass class FileExistsError(SysProtError): pass class FileNotFound(SysProtError): pass class ModuleNotFound(SysProtError): pass class DirProtError(ProtocolError): pass class I2CError(DirProtError): pass class I2CPendingError(I2CError): pass CODES = { 0x00: None, 0x20: I2CPendingError('Pending communication transaction in progress'), 0x40: DirProtError('Specified mailbox queue is empty'), 0x81: SysProtError('No more handles'), 0x82: SysProtError('No space'), 0x83: SysProtError('No more files'), 0x84: SysProtError('End of file expected'), 0x85: SysProtError('End of file'), 0x86: SysProtError('Not a linear file'), 0x87: FileNotFound('File not found'), 0x88: SysProtError('Handle already closed'), 0x89: SysProtError('No linear space'), 0x8A: SysProtError('Undefined error'), 0x8B: SysProtError('File is busy'), 0x8C: SysProtError('No write buffers'), 0x8D: SysProtError('Append not possible'), 0x8E: SysProtError('File is full'), 0x8F: FileExistsError('File exists'), 0x90: ModuleNotFound('Module not found'), 0x91: SysProtError('Out of bounds'), 0x92: SysProtError('Illegal file name'), 0x93: SysProtError('Illegal handle'), 0xBD: DirProtError('Request failed (i.e. specified file not found)'), 0xBE: DirProtError('Unknown command opcode'), 0xBF: DirProtError('Insane packet'), 0xC0: DirProtError('Data contains out-of-range values'), 0xDD: DirProtError('Communication bus error'), 0xDE: DirProtError('No free memory in communication buffer'), 0xDF: DirProtError('Specified channel/connection is not valid'), 0xE0: I2CError('Specified channel/connection not configured or busy'), 0xEC: DirProtError('No active program'), 0xED: DirProtError('Illegal size specified'), 0xEE: DirProtError('Illegal mailbox queue ID specified'), 0xEF: DirProtError('Attempted to access invalid field of a structure'), 0xF0: DirProtError('Bad input or output specified'), 0xFB: DirProtError('Insufficient memory available'), 0xFF: DirProtError('Bad arguments'), } def check_status(status): if status: ex = CODES.get(status) if ex: raise ex else: raise ProtocolError, status nxt-python-2.2.2/nxt/__init__.py0000644000175000001440000000132311571005210017227 0ustar marcususers00000000000000# nxt.__init__ module -- LEGO Mindstorms NXT python package # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2009 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. from nxt.locator import find_one_brick, Method from nxt.motor import * from nxt.sensor import * nxt-python-2.2.2/nxt/locator.py0000644000175000001440000001720311603070740017145 0ustar marcususers00000000000000# nxt.locator module -- Locate LEGO Minstorms NXT bricks via USB or Bluetooth # Copyright (C) 2006, 2007 Douglas P Lau # Copyright (C) 2009 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 traceback, ConfigParser, os class BrickNotFoundError(Exception): pass class NoBackendError(Exception): pass class Method(): """Used to indicate which comm backends should be tried by find_bricks/ find_one_brick. Any or all can be selected.""" def __init__(self, usb=True, bluetooth=True, fantomusb=False, fantombt=False): #new method options MUST default to False! self.usb = usb self.bluetooth = bluetooth self.fantom = fantomusb or fantombt self.fantomusb = fantomusb self.fantombt = fantombt def find_bricks(host=None, name=None, silent=False, method=Method()): """Used by find_one_brick to look for bricks ***ADVANCED USERS ONLY***""" methods_available = 0 if method.usb: try: import usbsock methods_available += 1 socks = usbsock.find_bricks(host, name) for s in socks: yield s except ImportError: import sys if not silent: print >>sys.stderr, "USB module unavailable, not searching there" if method.bluetooth: try: import bluesock methods_available += 1 try: socks = bluesock.find_bricks(host, name) for s in socks: yield s except (bluesock.bluetooth.BluetoothError, IOError): #for cases such as no adapter, bluetooth throws IOError, not BluetoothError pass except ImportError: import sys if not silent: print >>sys.stderr, "Bluetooth module unavailable, not searching there" if method.fantom: try: import fantomsock methods_available += 1 if method.fantomusb: usbsocks = fantomsock.find_bricks(host, name, False) for s in usbsocks: yield s if method.fantombt: btsocks = fantomsock.find_bricks(host, name, True) for s in btsocks: yield s except ImportError: import sys if not silent: print >>sys.stderr, "Fantom module unavailable, not searching there" if methods_available == 0: raise NoBackendError("No selected backends are available! Did you install the comm modules?") def find_one_brick(host=None, name=None, silent=False, strict=None, debug=False, method=None, confpath=None): """Use to find one brick. The host and name args limit the search to a given MAC or brick name. Set silent to True to stop nxt-python from printing anything during the search. This function by default automatically checks to see if the brick found has the correct host/name (if either are provided) and will not return a brick which doesn't match. This can be disabled (so the function returns any brick which can be connected to and provides a valid reply to get_device_info()) by passing strict=False. This will, however, still tell the comm backends to only look for devices which match the args provided. The confpath arg specifies the location of the configuration file which brick location information will be read from if no brick location directives (host, name, strict, or method) are provided.""" if debug and silent: silent=False print "silent and debug can't both be set; giving debug priority" conf = read_config(confpath, debug) if not (host or name or strict or method): host = conf.get('Brick', 'host') name = conf.get('Brick', 'name') strict = bool(int(conf.get('Brick', 'strict'))) method = eval('Method(%s)' % conf.get('Brick', 'method')) if not strict: strict = True if not method: method = Method() if debug: print "Host: %s Name: %s Strict: %s" % (host, name, str(strict)) print "USB: %s BT: %s Fantom: %s FUSB: %s FBT: %s" % (method.usb, method.bluetooth, method.fantom, method.fantombt, method.fantomusb) for s in find_bricks(host, name, silent, method): try: if host and 'host' in dir(s) and s.host != host: if debug: print "Warning: the brick found does not match the host provided (s.host)." if strict: continue b = s.connect() info = b.get_device_info() if host and info[1] != host: if debug: print "Warning: the brick found does not match the host provided (get_device_info)." if strict: s.close() continue if name and info[0].strip('\0') != name: if debug: print "Warning; the brick found does not match the name provided." if strict: s.close() continue return b except: if debug: traceback.print_exc() print "Failed to connect to possible brick" raise BrickNotFoundError def server_brick(host, port = 2727): import ipsock sock = ipsock.IpSock(host, port) return sock.connect() def read_config(confpath=None, debug=False): conf = ConfigParser.RawConfigParser({'host': None, 'name': None, 'strict': True, 'method': ''}) if not confpath: confpath = os.path.expanduser('~/.nxt-python') if conf.read([confpath]) == [] and debug: print "Warning: Config file (should be at %s) was not read. Use nxt.locator.make_config() to create a config file." % confpath if conf.has_section('Brick') == False: conf.add_section('Brick') return conf def make_config(confpath=None): conf = ConfigParser.RawConfigParser() if not confpath: confpath = os.path.expanduser('~/.nxt-python') print "Welcome to the nxt-python config file generator!" print "This function creates an example file which find_one_brick uses to find a brick." try: if os.path.exists(confpath): raw_input("File already exists at %s. Press Enter to overwrite or Ctrl+C to abort." % confpath) except KeyboardInterrupt: print "Not writing file." return conf.add_section('Brick') conf.set('Brick', 'name', 'MyNXT') conf.set('Brick', 'host', '54:32:59:92:F9:39') conf.set('Brick', 'strict', 0) conf.set('Brick', 'method', 'usb=True, bluetooth=False, fantomusb=True') conf.write(open(confpath, 'w')) print "The file has been written at %s" % confpath print "The file contains less-than-sane default values to get you started." print "You must now edit the file with a text editor and change the values to match what you would pass to find_one_brick" print "The fields for name, host, and strict correspond to the similar args accepted by find_one_brick" print "The method field contains the string which would be passed to Method()" print "Any field whose corresponding option does not need to be passed to find_one_brick should be commented out (using a # at the start of the line) or simply removed." print "If you have questions, check the wiki and then ask on the mailing list." nxt-python-2.2.2/nxt/motor.py0000644000175000001440000003703111565504062016651 0ustar marcususers00000000000000# nxt.motor module -- Class to control LEGO Mindstorms NXT motors # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, rhn # Copyright (C) 2010 rhn # # 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. """Use for motor control""" import time PORT_A = 0x00 PORT_B = 0x01 PORT_C = 0x02 PORT_ALL = 0xFF MODE_IDLE = 0x00 MODE_MOTOR_ON = 0x01 MODE_BRAKE = 0x02 MODE_REGULATED = 0x04 REGULATION_IDLE = 0x00 REGULATION_MOTOR_SPEED = 0x01 REGULATION_MOTOR_SYNC = 0x02 RUN_STATE_IDLE = 0x00 RUN_STATE_RAMP_UP = 0x10 RUN_STATE_RUNNING = 0x20 RUN_STATE_RAMP_DOWN = 0x40 LIMIT_RUN_FOREVER = 0 class BlockedException(Exception): pass class OutputState(object): """An object holding the internal state of a motor, not including rotation counters. """ def __init__(self, values): (self.power, self.mode, self.regulation, self.turn_ratio, self.run_state, self.tacho_limit) = values def to_list(self): """Returns a list of properties that can be used with set_output_state. """ return [self.power, self.mode, self.regulation, self.turn_ratio, self.run_state, self.tacho_limit] def __str__(self): modes = [] if self.mode & MODE_MOTOR_ON: modes.append('on') if self.mode & MODE_BRAKE: modes.append('brake') if self.mode & MODE_REGULATED: modes.append('regulated') if not modes: modes.append('idle') mode = '&'.join(modes) regulation = 'regulation: ' + \ ['idle', 'speed', 'sync'][self.regulation] run_state = 'run state: ' + {0: 'idle', 0x10: 'ramp_up', 0x20: 'running', 0x40: 'ramp_down'}[self.run_state] return ', '.join([mode, regulation, str(self.turn_ratio), run_state] + [str(self.tacho_limit)]) class TachoInfo: """An object containing the information about the rotation of a motor""" def __init__(self, values): self.tacho_count, self.block_tacho_count, self.rotation_count = values def get_target(self, tacho_limit, direction): """Returns a TachoInfo object which corresponds to tacho state after moving for tacho_limit ticks. Direction can be 1 (add) or -1 (subtract) """ # TODO: adjust other fields if abs(direction) != 1: raise ValueError('Invalid direction') new_tacho = self.tacho_count + direction * tacho_limit return TachoInfo([new_tacho, None, None]) def is_greater(self, target, direction): return direction * (self.tacho_count - target.tacho_count) > 0 def is_near(self, target, threshold): difference = abs(target.tacho_count - self.tacho_count) return difference < threshold def __str__(self): return str((self.tacho_count, self.block_tacho_count, self.rotation_count)) class SynchronizedTacho(object): def __init__(self, leader_tacho, follower_tacho): self.leader_tacho = leader_tacho self.follower_tacho = follower_tacho def get_target(self, tacho_limit, direction): """This method will leave follower's target as None""" leader_tacho = self.leader_tacho.get_target(tacho_limit, direction) return SynchronizedTacho(leader_tacho, None) def is_greater(self, other, direction): return self.leader_tacho.is_greater(other.leader_tacho, direction) def is_near(self, other, threshold): return self.leader_tacho.is_near(other.leader_tacho, threshold) def __str__(self): if self.follower_tacho is not None: t2 = str(self.follower_tacho.tacho_count) else: t2 = 'None' t1 = str(self.leader_tacho.tacho_count) return 'tacho: ' + t1 + ' ' + t2 def get_tacho_and_state(values): """A convenience function. values is the list of values from get_output_state. Returns both OutputState and TachoInfo. """ return OutputState(values[1:7]), TachoInfo(values[7:]) class BaseMotor(object): """Base class for motors""" debug = 0 def _debug_out(self, message): if self.debug: print message def turn(self, power, tacho_units, brake=True, timeout=1, emulate=True): """Use this to turn a motor. The motor will not stop until it turns the desired distance. Accuracy is much better over a USB connection than with bluetooth... power is a value between -127 and 128 (an absolute value greater than 64 is recommended) tacho_units is the number of degrees to turn the motor. values smaller than 50 are not recommended and may have strange results. brake is whether or not to hold the motor after the function exits (either by reaching the distance or throwing an exception). timeout is the number of seconds after which a BlockedException is raised if the motor doesn't turn emulate is a boolean value. If set to False, the motor is aware of the tacho limit. If True, a run() function equivalent is used. Warning: motors remember their positions and not using emulate may lead to strange behavior, especially with synced motors """ tacho_limit = tacho_units if tacho_limit < 0: raise ValueError, "tacho_units must be greater than 0!" #TODO Calibrate the new values for ip socket latency. if self.method == 'bluetooth': threshold = 70 elif self.method == 'usb': threshold = 5 elif self.method == 'ipbluetooth': threshold = 80 elif self.method == 'ipusb': threshold = 15 else: threshold = 30 #compromise tacho = self.get_tacho() state = self._get_new_state() # Update modifiers even if they aren't used, might have been changed state.power = power if not emulate: state.tacho_limit = tacho_limit self._debug_out('Updating motor information...') self._set_state(state) direction = 1 if power > 0 else -1 self._debug_out('tachocount: ' + str(tacho)) current_time = time.time() tacho_target = tacho.get_target(tacho_limit, direction) blocked = False try: while True: time.sleep(self._eta(tacho, tacho_target, power) / 2) if not blocked: # if still blocked, don't reset the counter last_tacho = tacho last_time = current_time tacho = self.get_tacho() current_time = time.time() blocked = self._is_blocked(tacho, last_tacho, direction) if blocked: self._debug_out(('not advancing', last_tacho, tacho)) # the motor can be up to 80+ degrees in either direction from target when using bluetooth if current_time - last_time > timeout: if tacho.is_near(tacho_target, threshold): break else: raise BlockedException("Blocked!") else: self._debug_out(('advancing', last_tacho, tacho)) if tacho.is_near(tacho_target, threshold) or tacho.is_greater(tacho_target, direction): break finally: if brake: self.brake() else: self.idle() class Motor(BaseMotor): def __init__(self, brick, port): self.brick = brick self.port = port self._read_state() self.sync = 0 self.turn_ratio = 0 try: self.method = brick.sock.type except: print "Warning: Socket did not report a type!" print "Please report this problem to the developers!" print "For now, turn() accuracy will not be optimal." print "Continuing happily..." self.method = None def _set_state(self, state): self._debug_out('Setting brick output state...') list_state = [self.port] + state.to_list() self.brick.set_output_state(*list_state) self._debug_out(state) self._state = state self._debug_out('State set.') def _read_state(self): self._debug_out('Getting brick output state...') values = self.brick.get_output_state(self.port) self._debug_out('State got.') self._state, tacho = get_tacho_and_state(values) return self._state, tacho #def get_tacho_and_state here would allow tacho manipulation def _get_state(self): """Returns a copy of the current motor state for manipulation.""" return OutputState(self._state.to_list()) def _get_new_state(self): state = self._get_state() if self.sync: state.mode = MODE_MOTOR_ON | MODE_REGULATED state.regulation = REGULATION_MOTOR_SYNC state.turn_ratio = self.turn_ratio else: state.mode = MODE_MOTOR_ON | MODE_REGULATED state.regulation = REGULATION_MOTOR_SPEED state.run_state = RUN_STATE_RUNNING state.tacho_limit = LIMIT_RUN_FOREVER return state def get_tacho(self): return self._read_state()[1] def reset_position(self, relative): """Resets the counters. Relative can be True or False""" self.brick.reset_motor_position(self.port, relative) def run(self, power=100, regulated=False): '''Tells the motor to run continuously. If regulated is True, then the synchronization starts working. ''' state = self._get_new_state() state.power = power if not regulated: state.mode = MODE_MOTOR_ON self._set_state(state) def brake(self): """Holds the motor in place""" state = self._get_new_state() state.power = 0 state.mode = MODE_MOTOR_ON | MODE_BRAKE | MODE_REGULATED self._set_state(state) def idle(self): '''Tells the motor to stop whatever it's doing. It also desyncs it''' state = self._get_new_state() state.power = 0 state.mode = MODE_IDLE state.regulation = REGULATION_IDLE state.run_state = RUN_STATE_IDLE self._set_state(state) def weak_turn(self, power, tacho_units): """Tries to turn a motor for the specified distance. This function returns immediately, and it's not guaranteed that the motor turns that distance. This is an interface to use tacho_limit without REGULATION_MODE_SPEED """ tacho_limit = tacho_units tacho = self.get_tacho() state = self._get_new_state() # Update modifiers even if they aren't used, might have been changed state.mode = MODE_MOTOR_ON state.regulation = REGULATION_IDLE state.power = power state.tacho_limit = tacho_limit self._debug_out('Updating motor information...') self._set_state(state) def _eta(self, current, target, power): """Returns time in seconds. Do not trust it too much""" tacho = abs(current.tacho_count - target.tacho_count) return (float(tacho) / abs(power)) / 5 def _is_blocked(self, tacho, last_tacho, direction): """Returns if any of the engines is blocked""" return direction * (last_tacho.tacho_count - tacho.tacho_count) >= 0 class SynchronizedMotors(BaseMotor): """The object used to make two motors run in sync. Many objects may be present at the same time but they can't be used at the same time. Warning! Movement methods reset tacho counter. THIS CODE IS EXPERIMENTAL!!! """ def __init__(self, leader, follower, turn_ratio): """Turn ratio can be >= 0 only! If you want to have it reversed, change motor order. """ if follower.brick != leader.brick: raise ValueError('motors belong to different bricks') self.leader = leader self.follower = follower self.method = self.leader.method #being from the same brick, they both have the same com method. if turn_ratio < 0: raise ValueError('Turn ratio <0. Change motor order instead!') if self.leader.port == self.follower.port: raise ValueError("The same motor passed twice") elif self.leader.port > self.follower.port: self.turn_ratio = turn_ratio else: self._debug_out('reversed') self.turn_ratio = -turn_ratio def _get_new_state(self): return self.leader._get_new_state() def _set_state(self, state): self.leader._set_state(state) self.follower._set_state(state) def get_tacho(self): leadertacho = self.leader.get_tacho() followertacho = self.follower.get_tacho() return SynchronizedTacho(leadertacho, followertacho) def reset_position(self, relative): """Resets the counters. Relative can be True or False""" self.leader.reset_position(relative) self.follower.reset_position(relative) def _enable(self): # This works as expected. I'm not sure why. #self._disable() self.reset_position(True) self.leader.sync = True self.follower.sync = True self.leader.turn_ratio = self.turn_ratio self.follower.turn_ratio = self.turn_ratio def _disable(self): # This works as expected. (tacho is reset ok) self.leader.sync = False self.follower.sync = False #self.reset_position(True) self.leader.idle() self.follower.idle() #self.reset_position(True) def run(self, power=100): """Warning! After calling this method, make sure to call idle. The motors are reported to behave wildly otherwise. """ self._enable() self.leader.run(power, True) self.follower.run(power, True) def brake(self): self._disable() # reset the counters self._enable() self.leader.brake() # brake both motors at the same time self.follower.brake() self._disable() # now brake as usual self.leader.brake() self.follower.brake() def idle(self): self._disable() def turn(self, power, tacho_units, brake=True, timeout=1): self._enable() # non-emulation is a nightmare, tacho is being counted differently try: if power < 0: self.leader, self.follower = self.follower, self.leader BaseMotor.turn(self, power, tacho_units, brake, timeout, emulate=True) finally: if power < 0: self.leader, self.follower = self.follower, self.leader def _eta(self, tacho, target, power): return self.leader._eta(tacho.leader_tacho, target.leader_tacho, power) def _is_blocked(self, tacho, last_tacho, direction): # no need to check both, they're synced return self.leader._is_blocked(tacho.leader_tacho, last_tacho.leader_tacho, direction) nxt-python-2.2.2/nxt/system.py0000644000175000001440000002070611565504062017036 0ustar marcususers00000000000000# nxt.system module -- LEGO Mindstorms NXT system telegrams # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2009 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. 'Use for communications regarding the NXT filesystem and such ***ADVANCED USERS ONLY***' def _create(opcode): 'Create a simple system telegram' from telegram import Telegram return Telegram(False, opcode) def _create_with_file(opcode, fname): tgram = _create(opcode) tgram.add_filename(fname) return tgram def _create_with_handle(opcode, handle): tgram = _create(opcode) tgram.add_u8(handle) return tgram def open_read(opcode, fname): return _create_with_file(opcode, fname) def _parse_open_read(tgram): tgram.check_status() handle = tgram.parse_u8() n_bytes = tgram.parse_u32() return (handle, n_bytes) def open_write(opcode, fname, n_bytes): tgram = _create_with_file(opcode, fname) tgram.add_u32(n_bytes) return tgram def _parse_open_write(tgram): tgram.check_status() handle = tgram.parse_u8() return handle def read(opcode, handle, n_bytes): tgram = _create_with_handle(opcode, handle) tgram.add_u16(n_bytes) return tgram def _parse_read(tgram): tgram.check_status() handle = tgram.parse_u8() n_bytes = tgram.parse_u16() data = tgram.parse_string() return (handle, n_bytes, data) def write(opcode, handle, data): tgram = _create_with_handle(opcode, handle) tgram.add_string(len(data), data) return tgram def _parse_write(tgram): tgram.check_status() handle = tgram.parse_u8() n_bytes = tgram.parse_u16() return (handle, n_bytes) def close(opcode, handle): return _create_with_handle(opcode, handle) def _parse_close(tgram): tgram.check_status() handle = tgram.parse_u8() return handle def delete(opcode, fname): return _create_with_file(opcode, fname) def _parse_delete(tgram): tgram.check_status() handle = tgram.parse_u8() fname = tgram.parse_string() return (handle, fname) def find_first(opcode, fname): return _create_with_file(opcode, fname) def _parse_find(tgram): tgram.check_status() handle = tgram.parse_u8() fname = tgram.parse_string(20) n_bytes = tgram.parse_u32() return (handle, fname, n_bytes) def find_next(opcode, handle): return _create_with_handle(opcode, handle) def get_firmware_version(opcode): return _create(opcode) def _parse_get_firmware_version(tgram): tgram.check_status() prot_minor = tgram.parse_u8() prot_major = tgram.parse_u8() prot_version = (prot_major, prot_minor) fw_minor = tgram.parse_u8() fw_major = tgram.parse_u8() fw_version = (fw_major, fw_minor) return (prot_version, fw_version) def open_write_linear(opcode, fname, n_bytes): tgram = _create_with_file(opcode, fname) tgram.add_u32(n_bytes) return tgram def open_read_linear(opcode, fname): return _create_with_file(opcode, fname) def _parse_open_read_linear(tgram): tgram.check_status() n_bytes = tgram.parse_u32() return n_bytes def open_write_data(opcode, fname, n_bytes): tgram = _create_with_file(opcode, fname) tgram.add_u32(n_bytes) return tgram def open_append_data(opcode, fname): return _create_with_file(opcode, fname) def _parse_open_append_data(tgram): tgram.check_status() handle = tgram.parse_u8() n_bytes = tgram.parse_u32() return (handle, n_bytes) def request_first_module(opcode, mname): return _create_with_file(opcode, mname) def _parse_request_module(tgram): tgram.check_status() handle = tgram.parse_u8() mname = tgram.parse_string(20) mod_id = tgram.parse_u32() mod_size = tgram.parse_u32() mod_iomap_size = tgram.parse_u16() return (handle, mname, mod_id, mod_size, mod_iomap_size) def request_next_module(opcode, handle): return _create_with_handle(opcode, handle) def close_module_handle(opcode, handle): return _create_with_handle(opcode, handle) def read_io_map(opcode, mod_id, offset, n_bytes): tgram = _create(opcode) tgram.add_u32(mod_id) tgram.add_u16(offset) tgram.add_u16(n_bytes) return tgram def _parse_read_io_map(tgram): tgram.check_status() mod_id = tgram.parse_u32() n_bytes = tgram.parse_u16() contents = tgram.parse_string() return (mod_id, n_bytes, contents) def write_io_map(opcode, mod_id, offset, content): tgram = _create(opcode) tgram.add_u32(mod_id) tgram.add_u16(offset) tgram.add_u16(len(content)) tgram.add_string(len(content), content) return tgram def _parse_write_io_map(tgram): tgram.check_status() mod_id = tgram.parse_u32() n_bytes = tgram.parse_u16() return (mod_id, n_bytes) def boot(opcode): # Note: this command is USB only (no Bluetooth) tgram = _create(opcode) tgram.add_string(19, "Let's dance: SAMBA\0") return tgram def _parse_boot(tgram): tgram.check_status() resp = tgram.parse_string() # Resp should be 'Yes\0' return resp def set_brick_name(opcode, bname): tgram = _create(opcode) if len(bname) > 15: print "Warning! Brick name %s will be truncated to %s!" % (bname, bname[0:15]) bname = bname[0:15] elif len(bname) < 15: bname += '\x00' * (15-len(bname)) #fill the extra chars with nulls tgram.add_string(len(bname), bname) return tgram def _parse_set_brick_name(tgram): tgram.check_status() def get_device_info(opcode): return _create(opcode) def _parse_get_device_info(tgram): tgram.check_status() name = tgram.parse_string(15) a0 = tgram.parse_u8() a1 = tgram.parse_u8() a2 = tgram.parse_u8() a3 = tgram.parse_u8() a4 = tgram.parse_u8() a5 = tgram.parse_u8() a6 = tgram.parse_u8() # FIXME: what is a6 for? address = '%02X:%02X:%02X:%02X:%02X:%02X' % (a0, a1, a2, a3, a4, a5) signal_strength = tgram.parse_u32() user_flash = tgram.parse_u32() return (name, address, signal_strength, user_flash) def delete_user_flash(opcode): return _create(opcode) def _parse_delete_user_flash(tgram): tgram.check_status() def poll_command_length(opcode, buf_num): tgram = _create(opcode) tgram.add_u8(buf_num) return tgram def _parse_poll_command_length(tgram): buf_num = tgram.parse_u8() tgram.check_status() n_bytes = tgram.parse_u8() return (buf_num, n_bytes) def poll_command(opcode, buf_num, n_bytes): tgram = _create(opcode) tgram.add_u8(buf_num) tgram.add_u8(n_bytes) return tgram def _parse_poll_command(tgram): buf_num = tgram.parse_u8() tgram.check_status() n_bytes = tgram.parse_u8() command = tgram.parse_string() return (buf_num, n_bytes, command) def bluetooth_factory_reset(opcode): # Note: this command is USB only (no Bluetooth) return _create(opcode) def _parse_bluetooth_factory_reset(tgram): tgram.check_status() #TODO Add docstrings to all methods OPCODES = { 0x80: (open_read, _parse_open_read), 0x81: (open_write, _parse_open_write), 0x82: (read, _parse_read), 0x83: (write, _parse_write), 0x84: (close, _parse_close), 0x85: (delete, _parse_delete), 0x86: (find_first, _parse_find), 0x87: (find_next, _parse_find), 0x88: (get_firmware_version, _parse_get_firmware_version), 0x89: (open_write_linear, _parse_open_write), 0x8A: (open_read_linear, _parse_open_read_linear), 0x8B: (open_write_data, _parse_open_write), 0x8C: (open_append_data, _parse_open_append_data), 0x90: (request_first_module, _parse_request_module), 0x91: (request_next_module, _parse_request_module), 0x92: (close_module_handle, _parse_close), 0x94: (read_io_map, _parse_read_io_map), 0x95: (write_io_map, _parse_write_io_map), 0x97: (boot, _parse_boot), 0x98: (set_brick_name, _parse_set_brick_name), 0x9B: (get_device_info, _parse_get_device_info), 0xA0: (delete_user_flash, _parse_delete_user_flash), 0xA1: (poll_command_length, _parse_poll_command_length), 0xA2: (poll_command, _parse_poll_command), 0xA4: (bluetooth_factory_reset, _parse_bluetooth_factory_reset), } nxt-python-2.2.2/nxt/usbsock.py0000644000175000001440000000577011662266606017176 0ustar marcususers00000000000000# nxt.usbsock module -- USB socket communication with LEGO Minstorms NXT # Copyright (C) 2006, 2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner # Copyright (C) 2011 Paul Hollensen, 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 usb, os from nxt.brick import Brick ID_VENDOR_LEGO = 0x0694 ID_PRODUCT_NXT = 0x0002 class USBSock(object): 'Object for USB connection to NXT' bsize = 60 # USB socket block size type = 'usb' def __init__(self, device): self.device = device self.handle = None self.debug = False def __str__(self): return 'USB (%s)' % (self.device.filename) def connect(self): 'Use to connect to NXT.' if self.debug: print 'Connecting via USB...' config = self.device.configurations[0] iface = config.interfaces[0][0] self.blk_out, self.blk_in = iface.endpoints self.handle = self.device.open() self.handle.setConfiguration(1) self.handle.claimInterface(0) if os.name != 'nt': # http://code.google.com/p/nxt-python/issues/detail?id=33 self.handle.reset() if self.debug: print 'Connected.' return Brick(self) def close(self): 'Use to close the connection.' if self.debug: print 'Closing USB connection...' self.device = None self.handle = None self.blk_out = None self.blk_in = None if self.debug: print 'USB connection closed.' def send(self, data): 'Use to send raw data over USB connection ***ADVANCED USERS ONLY***' if self.debug: print 'Send:', print ':'.join('%02x' % ord(c) for c in data) self.handle.bulkWrite(self.blk_out.address, data) def recv(self): 'Use to recieve raw data over USB connection ***ADVANCED USERS ONLY***' data = self.handle.bulkRead(self.blk_in.address, 64) if self.debug: print 'Recv:', print ':'.join('%02x' % (c & 0xFF) for c in data) # NOTE: bulkRead returns a tuple of ints ... make it sane return ''.join(chr(d & 0xFF) for d in data) def find_bricks(host=None, name=None): 'Use to look for NXTs connected by USB only. ***ADVANCED USERS ONLY***' # FIXME: probably should check host (MAC) # if anyone knows how to do this, please file a bug report for bus in usb.busses(): for device in bus.devices: if device.idVendor == ID_VENDOR_LEGO and device.idProduct == ID_PRODUCT_NXT: yield USBSock(device) nxt-python-2.2.2/nxt/sensor/0000755000175000001440000000000011760013147016440 5ustar marcususers00000000000000nxt-python-2.2.2/nxt/sensor/__init__.py0000644000175000001440000000350711565504062020562 0ustar marcususers00000000000000# nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors # Copyright (C) 2006,2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn # Copyright (C) 2010 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. from .common import * from .analog import BaseAnalogSensor from .digital import BaseDigitalSensor, find_class from .generic import Touch, Light, Sound, Ultrasonic, Color20 import mindsensors MSSumoEyes = mindsensors.SumoEyes MSCompassv2 = mindsensors.Compassv2 MSDIST = mindsensors.DIST MSRTC = mindsensors.RTC MSACCL = mindsensors.ACCL MSServo = mindsensors.Servo MSMTRMUX = mindsensors.MTRMUX MSLineLeader = mindsensors.LineLeader MSMMX = mindsensors.MMX MSPS2 = mindsensors.PS2 MSHID = mindsensors.HID import hitechnic HTCompass = hitechnic.Compass HTAccelerometer = hitechnic.Accelerometer HTGyro = hitechnic.Gyro HTColorv2 = hitechnic.Colorv2 HTEOPD = hitechnic.EOPD HTIRReceiver = hitechnic.IRReceiver HTIRSeekerv2 = hitechnic.IRSeekerv2 HTPrototype = hitechnic.Prototype def get_sensor(brick, port): """Tries to detect the sensor type and return the correct sensor object. Does not work for sensors with no identification information (such as all analog sensors or the MindSensors RTC. """ base_sensor = BaseDigitalSensor(brick, port, False) info = base_sensor.get_sensor_info() return find_class(info)(brick, port, check_compatible=False) nxt-python-2.2.2/nxt/sensor/digital.py0000644000175000001440000002171611760011361020432 0ustar marcususers00000000000000# nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors # Copyright (C) 2006,2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn # Copyright (C) 2010,2011,2012 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. from nxt.error import I2CError, I2CPendingError, DirProtError from common import * from time import sleep, time import struct class SensorInfo: def __init__(self, version, product_id, sensor_type): self.version = version self.product_id = product_id self.sensor_type = sensor_type def clarifybinary(self, instr, label): outstr = '' outstr += (label + ': `' + instr + '`\n') for char in instr: outstr += (hex(ord(char))+', ') outstr += ('\n') return outstr def __str__(self): outstr = '' outstr += (self.clarifybinary(str(self.version), 'Version')) outstr += (self.clarifybinary(str(self.product_id), 'Product ID')) outstr += (self.clarifybinary(str(self.sensor_type), 'Type')) return outstr class BaseDigitalSensor(Sensor): """Object for digital sensors. I2C_ADDRESS is the dictionary storing name to i2c address mappings. It should be updated in every subclass. When subclassing this class, make sure to call add_compatible_sensor to add compatible sensor data. """ I2C_DEV = 0x02 I2C_ADDRESS = {'version': (0x00, '8s'), 'product_id': (0x08, '8s'), 'sensor_type': (0x10, '8s'), # 'factory_zero': (0x11, 1), # is this really correct? 'factory_scale_factor': (0x12, 'B'), 'factory_scale_divisor': (0x13, 'B'), } def __init__(self, brick, port, check_compatible=True): """Creates a BaseDigitalSensor. If check_compatible is True, queries the sensor for its name, and if a wrong sensor class was used, prints a warning. """ super(BaseDigitalSensor, self).__init__(brick, port) self.set_input_mode(Type.LOW_SPEED_9V, Mode.RAW) self.last_poll = time() self.poll_delay = 0.01 sleep(0.1) # Give I2C time to initialize #Don't do type checking if this class has no compatible sensors listed. try: self.compatible_sensors except AttributeError: check_compatible = False if check_compatible: sensor = self.get_sensor_info() if not sensor in self.compatible_sensors: print ('WARNING: Wrong sensor class chosen for sensor ' + str(sensor.product_id) + ' on port ' + str(port) + '. ' + """ You may be using the wrong type of sensor or may have connected the cable incorrectly. If you are sure you're using the correct sensor class for the sensor, this message is likely in error and you should disregard it and file a bug report, including the output of get_sensor_info(). This message can be suppressed by passing "check_compatible=False" when creating the sensor object.""") def _ls_get_status(self, n_bytes): for n in range(30): #https://code.google.com/p/nxt-python/issues/detail?id=35 try: b = self.brick.ls_get_status(self.port) if b >= n_bytes: return b except I2CPendingError: pass raise I2CError, 'ls_get_status timeout' def _i2c_command(self, address, value, format): """Writes an i2c value to the given address. value must be a string. value is a tuple of values corresponding to the given format. """ value = struct.pack(format, *value) msg = chr(self.I2C_DEV) + chr(address) + value now = time() if self.last_poll+self.poll_delay > now: diff = now - self.last_poll sleep(self.poll_delay - diff) self.last_poll = time() self.brick.ls_write(self.port, msg, 0) def _i2c_query(self, address, format): """Reads an i2c value from given address, and returns a value unpacked according to the given format. Format is the same as in the struct module. See http://docs.python.org/library/struct.html#format-strings """ n_bytes = struct.calcsize(format) msg = chr(self.I2C_DEV) + chr(address) now = time() if self.last_poll+self.poll_delay > now: diff = now - self.last_poll sleep(self.poll_delay - diff) self.last_poll = time() self.brick.ls_write(self.port, msg, n_bytes) try: self._ls_get_status(n_bytes) finally: #we should clear the buffer no matter what happens data = self.brick.ls_read(self.port) if len(data) < n_bytes: raise I2CError, 'Read failure: Not enough bytes' data = struct.unpack(format, data[-n_bytes:]) return data def read_value(self, name): """Reads a value from the sensor. Name must be a string found in self.I2C_ADDRESS dictionary. Entries in self.I2C_ADDRESS are in the name: (address, format) form, with format as in the struct module. Be careful on unpacking single variables - struct module puts them in tuples containing only one element. """ address, fmt = self.I2C_ADDRESS[name] for n in range(3): try: return self._i2c_query(address, fmt) except DirProtError: pass raise I2CError, "read_value timeout" def write_value(self, name, value): """Writes value to the sensor. Name must be a string found in self.I2C_ADDRESS dictionary. Entries in self.I2C_ADDRESS are in the name: (address, format) form, with format as in the struct module. value is a tuple of values corresponding to the format from self.I2C_ADDRESS dictionary. """ address, fmt = self.I2C_ADDRESS[name] self._i2c_command(address, value, fmt) def get_sensor_info(self): version = self.read_value('version')[0].split('\0')[0] product_id = self.read_value('product_id')[0].split('\0')[0] sensor_type = self.read_value('sensor_type')[0].split('\0')[0] return SensorInfo(version, product_id, sensor_type) @classmethod def add_compatible_sensor(cls, version, product_id, sensor_type): """Adds an entry in the compatibility table for the sensor. If version is None, then it's the default class for this model. If product_id is None, then this is the default class for this vendor. """ try: cls.compatible_sensors except AttributeError: cls.compatible_sensors = [] finally: cls.compatible_sensors.append(SCompatibility(version, product_id, sensor_type)) add_mapping(cls, version, product_id, sensor_type) class SCompatibility(SensorInfo): """An object that helps manage the sensor mappings""" def __eq__(self, other): if self.product_id is None: return self.product_id == other.product_id elif self.version is None: return (self.product_id == other.product_id and self.sensor_type == other.sensor_type) else: return (self.version == other.version and self.product_id == other.product_id and self.sensor_type == other.sensor_type) sensor_mappings = {} def add_mapping(cls, version, product_id, sensor_type): "None means any other value" if product_id not in sensor_mappings: sensor_mappings[product_id] = {} models = sensor_mappings[product_id] if sensor_type is None: if sensor_type in models: raise ValueError('Already registered!') models[sensor_type] = cls return if sensor_type not in models: models[sensor_type] = {} versions = models[sensor_type] if version in versions: raise ValueError('Already registered!') else: versions[version] = cls class SearchError(Exception): pass def find_class(info): """Returns an appropriate class for the given SensorInfo""" dic = sensor_mappings for val, msg in zip((info.product_id, info.sensor_type, info.version), ('Vendor', 'Model', 'Version')): if val in dic: dic = dic[val] elif None in dic: dic = dic[None] else: raise SearchError(msg + ' not found') return dic[info.sensor_type][None] nxt-python-2.2.2/nxt/sensor/hitechnic.py0000644000175000001440000005270511760011427020760 0ustar marcususers00000000000000# nxt.sensor.hitechnic module -- Classes to read HiTechnic sensors # Copyright (C) 2006,2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn # Copyright (C) 2010 rhn, Marcus Wanner, melducky, Samuel Leeman-Munk # Copyright (C) 2011 jerradgenson, 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. from .common import * from .digital import BaseDigitalSensor from .analog import BaseAnalogSensor class Compass(BaseDigitalSensor): """Hitechnic compass sensor.""" I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({'mode': (0x41, 'B'), 'heading': (0x42, 'B'), 'adder' : (0x43, 'B'), }) class Modes: MEASUREMENT = 0x00 CALIBRATION = 0x43 CALIBRATION_FAILED = 0x02 def get_heading(self): """Returns heading from North in degrees.""" two_degree_heading = self.read_value('heading')[0] adder = self.read_value('adder')[0] heading = two_degree_heading * 2 + adder return heading get_sample = get_heading def get_relative_heading(self,target=0): rheading = self.get_sample()-target if rheading > 180: rheading -= 360 elif rheading < -180: rheading += 360 return rheading def is_in_range(self,minval,maxval): """This deserves a little explanation: if max > min, it's straightforward, but if min > max, it switches the values of max and min and returns true if heading is NOT between the new max and min """ if minval > maxval: (maxval,minval) = (minval,maxval) inverted = True else: inverted = False heading = self.get_sample() in_range = (heading > minval) and (heading < maxval) #an xor handles the reversal #a faster, more compact way of saying #if !reversed return in_range #if reversed return !in_range return bool(inverted) ^ bool(in_range) def get_mode(self): return self.read_value('mode')[0] def set_mode(self, mode): if mode != self.Modes.MEASUREMENT and \ mode != self.Modes.CALIBRATION: raise ValueError('Invalid mode specified: ' + str(mode)) self.write_value('mode', (mode, )) Compass.add_compatible_sensor(None, 'HiTechnc', 'Compass ') #Tested with version '\xfdV1.23 ' Compass.add_compatible_sensor(None, 'HITECHNC', 'Compass ') #Tested with version '\xfdV2.1 ' class Accelerometer(BaseDigitalSensor): 'Object for Accelerometer sensors. Thanks to Paulo Vieira.' I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({'x_axis_high': (0x42, 'b'), 'y_axis_high': (0x43, 'b'), 'z_axis_high': (0x44, 'b'), 'xyz_short': (0x42, '3b'), 'all_data': (0x42, '3b3B') }) class Acceleration: def __init__(self, x, y, z): self.x, self.y, self.z = x, y, z def __init__(self, brick, port, check_compatible=True): super(Accelerometer, self).__init__(brick, port, check_compatible) def get_acceleration(self): """Returns the acceleration along x, y, z axes. Units are unknown to me. """ xh, yh, zh, xl, yl, zl = self.read_value('all_data') x = xh << 2 + xl y = yh << 2 + yl z = zh << 2 + yl return self.Acceleration(x, y, z) get_sample = get_acceleration Accelerometer.add_compatible_sensor(None, 'HiTechnc', 'Accel. ') Accelerometer.add_compatible_sensor(None, 'HITECHNC', 'Accel. ') #Tested with version '\xfdV1.1 ' class IRReceiver(BaseDigitalSensor): """Object for HiTechnic IRReceiver sensors for use with LEGO Power Functions IR Remotes. Coded to HiTechnic's specs for the sensor but not tested. Please report whether this worked for you or not! """ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({ 'm1A': (0x42, 'b'), 'm1B': (0x43, 'b'), 'm2A': (0x44, 'b'), 'm2B': (0x45, 'b'), 'm3A': (0x46, 'b'), 'm3B': (0x47, 'b'), 'm4A': (0x48, 'b'), 'm4B': (0x49, 'b'), 'all_data': (0x42, '8b') }) class SpeedReading: def __init__(self, m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B): self.m1A, self.m1B, self.m2A, self.m2B, self.m3A, self.m3B, self.m4A, self.m4B = m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B self.channel_1 = (m1A, m1B) self.channel_2 = (m2A, m2B) self.channel_3 = (m3A, m3B) self.channel_4 = (m4A, m4B) def __init__(self, brick, port, check_compatible=True): super(IRReceiver, self).__init__(brick, port, check_compatible) def get_speeds(self): """Returns the motor speeds for motors A and B on channels 1-4. Values are -128, -100, -86, -72, -58, -44, -30, -16, 0, 16, 30, 44, 58, 72, 86 and 100. -128 specifies motor brake mode. Note that no motors are actually being controlled here! """ m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B = self.read_value('all_data') return self.SpeedReading(m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B) get_sample = get_speeds IRReceiver.add_compatible_sensor(None, 'HiTechnc', 'IRRecv ') IRReceiver.add_compatible_sensor(None, 'HITECHNC', 'IRRecv ') class IRSeekerv2(BaseDigitalSensor): """Object for HiTechnic IRSeeker sensors. Coded to HiTechnic's specs for the sensor but not tested. Please report whether this worked for you or not! """ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({ 'dspmode': (0x41, 'B'), 'DC_direction': (0x42, 'B'), 'DC_sensor_1': (0x43, 'B'), 'DC_sensor_2': (0x44, 'B'), 'DC_sensor_3': (0x45, 'B'), 'DC_sensor_4': (0x46, 'B'), 'DC_sensor_5': (0x47, 'B'), 'DC_sensor_mean': (0x48, 'B'), 'all_DC': (0x42, '7B'), 'AC_direction': (0x49, 'B'), 'AC_sensor_1': (0x4A, 'B'), 'AC_sensor_2': (0x4B, 'B'), 'AC_sensor_3': (0x4C, 'B'), 'AC_sensor_4': (0x4D, 'B'), 'AC_sensor_5': (0x4E, 'B'), 'all_AC': (0x49, '6B') }) I2C_DEV = 0x10 #different from standard 0x02 class DSPModes: #Modes for modulated (AC) data. AC_DSP_1200Hz = 0x00 AC_DSP_600Hz = 0x01 class _data: def get_dir_brightness(self, direction): "Gets the brightness of a given direction (1-9)." if direction%2 == 1: #if it's an odd number exec("val = self.sensor_%d" % ((direction-1)/2+1)) else: exec("val = (self.sensor_%d+self.sensor_%d)/2" % (direction/2, (direction/2)+1)) return val class DCData(_data): def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean): self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5, self.sensor_mean = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean class ACData(_data): def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5): self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5 = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5 def __init__(self, brick, port, check_compatible=True): super(IRSeekerv2, self).__init__(brick, port, check_compatible) def get_dc_values(self): """Returns the unmodulated (DC) values. """ direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean = self.read_value('all_DC') return self.DCData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean) def get_ac_values(self): """Returns the modulated (AC) values. 600Hz and 1200Hz modes can be selected between by using the set_dsp_mode() function. """ direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5 = self.read_value('all_AC') return self.ACData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5) def get_dsp_mode(self): return self.read_value('dspmode')[0] def set_dsp_mode(self, mode): self.write_value('dspmode', (mode, )) get_sample = get_ac_values IRSeekerv2.add_compatible_sensor(None, 'HiTechnc', 'NewIRDir') IRSeekerv2.add_compatible_sensor(None, 'HITECHNC', 'NewIRDir') class EOPD(BaseAnalogSensor): """Object for HiTechnic Electro-Optical Proximity Detection sensors. """ # To be divided by processed value. _SCALE_CONSTANT = 250 # Maximum distance the sensor can detect. _MAX_DISTANCE = 1023 def __init__(self, brick, port): super(EOPD, self).__init__(brick, port) from math import sqrt self.sqrt = sqrt def set_range_long(self): ''' Choose this mode to increase the sensitivity of the EOPD sensor by approximately 4x. May cause sensor overload. ''' self.set_input_mode(Type.LIGHT_ACTIVE, Mode.RAW) def set_range_short(self): ''' Choose this mode to prevent the EOPD sensor from being overloaded by white objects. ''' self.set_input_mode(Type.LIGHT_INACTIVE, Mode.RAW) def get_raw_value(self): '''Unscaled value read from sensor.''' return self._MAX_DISTANCE - self.get_input_values().raw_ad_value def get_processed_value(self): '''Derived from the square root of the raw value.''' return self.sqrt(self.get_raw_value()) def get_scaled_value(self): ''' Returns a value that will scale linearly as distance from target changes. This is the method that should generally be called to get EOPD sensor data. ''' try: result = self._SCALE_CONSTANT / self.get_processed_value() return result except ZeroDivisionError: return self._SCALE_CONSTANT get_sample = get_scaled_value class Colorv2(BaseDigitalSensor): """Object for HiTechnic Color v2 Sensors. Coded to HiTechnic's specs for the sensor but not tested. Please report whether this worked for you or not!""" I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({ 'mode': (0x41, 'B'), 'number': (0x42, 'B'), 'red': (0x43, 'B'), 'green': (0x44, 'B'), 'blue': (0x45, 'B'), 'white': (0x46, 'B'), 'index': (0x47, 'B'), 'normred': (0x48, 'B'), 'normgreen': (0x49, 'B'), 'normblue': (0x4A, 'B'), 'all_data': (0x42, '9B'), 'rawred': (0x42, 'l'), 'm1mode': (0x44, 'B'), 'm1power': (0x45, 'b'), 'm2power': (0x46, 'b'), 'm2mode': (0x47, 'B'), 'm2enctarget': (0x48, '>l'), 'm1enccurrent': (0x4c, '>l'), 'm2enccurrent': (0x50, '>l'), 'batteryvoltage': (0x54, '2B'), 'm1gearratio': (0x56, 'b'), 'm1pid': (0x57, '3B'), 'm2gearratio': (0x5a, 'b'), 'm2pid': (0x5b, '3B'), }) class PID_Data(): def __init__(self, p, i, d): self.p, self.i, self.d = p, i, d def __init__(self, brick, port, check_compatible=True): super(MotorCon, self).__init__(brick, port, check_compatible) def set_enc_target(self, mot, val): """Set the encoder target (-2147483648-2147483647) for a motor """ self.write_value('m%denctarget'%mot, (val, )) def get_enc_target(self, mot): """Get the encoder target for a motor """ return self.read_value('m%denctarget'%mot)[0] def get_enc_current(self, mot): """Get the current encoder value for a motor """ return self.read_value('m%denccurrent'%mot)[0] def set_mode(self, mot, mode): """Set the mode for a motor. This value is a bit mask and you can find details about it in the sensor's documentation. """ self.write_value('m%dmode'%mot, (mode, )) def get_mode(self, mot): """Get the mode for a motor. This value is a bit mask and you can find details about it in the sensor's documentation. """ return self.read_value('m%dmode'%mot)[0] def set_power(self, mot, power): """Set the power (-100-100) for a motor """ self.write_value('m%dpower'%mot, (power, )) def get_power(self, mot): """Get the power for a motor """ return self.read_value('m%dpower'%mot)[0] def set_gear_ratio(self, mot, ratio): """Set the gear ratio for a motor """ self.write_value('m%dgearratio'%mot, (ratio, )) def get_gear_ratio(self, mot): """Get the gear ratio for a motor """ return self.read_value('m%dgearratio'%mot)[0] def set_pid(self, mot, piddata): """Set the PID coefficients for a motor. Takes data in MotorCon.PID_Data(p, i, d) format. """ self.write_value('m%dpid'%mot, (piddata.p, piddata.i, piddata.d)) def get_pid(self, mot): """Get the PID coefficients for a motor. Returns a PID_Data() object. """ p, i, d = self.read_value('m%dpid'%mot) return self.PID_Data(p, i, d) def get_battery_voltage(self): """Gets the battery voltage (in millivolts/20) """ high, low = self.read_value('bateryvoltage')[0] return high << 2 + low MotorCon.add_compatible_sensor(None, 'HiTechnc', 'MotorCon') nxt-python-2.2.2/nxt/sensor/generic.py0000644000175000001440000001223611565504062020436 0ustar marcususers00000000000000# nxt.sensor.generic module -- Classes to read LEGO Mindstorms NXT sensors # Copyright (C) 2006,2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn # Copyright (C) 2010 melducky, 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. from .common import * from .digital import BaseDigitalSensor from .analog import BaseAnalogSensor class Touch(BaseAnalogSensor): """The LEGO touch sensor""" def __init__(self, brick, port): super(Touch, self).__init__(brick, port) self.set_input_mode(Type.SWITCH, Mode.BOOLEAN) def is_pressed(self): return bool(self.get_input_values().scaled_value) get_sample = is_pressed class Light(BaseAnalogSensor): """Object for light sensors. It automatically turns off light when it's not used. """ def __init__(self, brick, port, illuminated=True): super(Light, self).__init__(brick, port) def set_illuminated(self, active): if active: type_ = Type.LIGHT_ACTIVE else: type_ = Type.LIGHT_INACTIVE self.set_input_mode(type_, Mode.RAW) def get_lightness(self): return self.get_input_values().scaled_value get_sample = get_lightness class Sound(BaseAnalogSensor): 'Object for sound sensors' def __init__(self, brick, port, adjusted=True): super(Sound, self).__init__(brick, port) self.set_adjusted(adjusted) def set_adjusted(self, active): if active: type_ = Type.SOUND_DBA else: type_ = Type.SOUND_DB self.set_input_mode(type_, Mode.RAW) def get_loudness(self): return self.get_input_values().scaled_value get_sample = get_loudness class Ultrasonic(BaseDigitalSensor): """Object for ultrasonic sensors""" I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({'measurement_units': (0x14, '7s'), 'continuous_measurement_interval': (0x40, 'B'), 'command': (0x41, 'B'), 'measurement_byte_0': (0x42, 'B'), 'measurements': (0x42, '8B'), 'actual_scale_factor': (0x51, 'B'), 'actual_scale_divisor': (0x52, 'B'), }) class Commands: 'These are for passing to command()' OFF = 0x00 SINGLE_SHOT = 0x01 CONTINUOUS_MEASUREMENT = 0x02 EVENT_CAPTURE = 0x03 #Optimize results when other Ultrasonic sensors running REQUEST_WARM_RESET = 0x04 def __init__(self, brick, port, check_compatible=True): super(Ultrasonic, self).__init__(brick, port, check_compatible) self.set_input_mode(Type.LOW_SPEED_9V, Mode.RAW) def get_distance(self): 'Function to get data from the ultrasonic sensor' return self.read_value('measurement_byte_0')[0] get_sample = get_distance def get_measurement_units(self): return self.read_value('measurement_units')[0].split('\0')[0] def get_all_measurements(self): "Returns all the past readings in measurement_byte_0 through 7" return self.read_value('measurements') def get_measurement_no(self, number): "Returns measurement_byte_number" if not 0 <= number < 8: raise ValueError('Measurements are numbered 0 to 7, not ' + str(number)) base_address, format = self.I2C_ADDRESS['measurement_byte_0'] return self._i2c_query(base_address + number, format)[0] def command(self, command): self.write_value('command', (command, )) def get_interval(self): 'Get the sample interval for continuous measurement mode -- Unknown units' return self.read_value('continuous_measurement_interval') def set_interval(self, interval): """Set the sample interval for continuous measurement mode. Unknown units; default is 1""" self.write_value('continuous_measurement_interval', interval) Ultrasonic.add_compatible_sensor(None, 'LEGO', 'Sonar') #Tested with version 'V1.0' class Color20(BaseAnalogSensor): def __init__(self, brick, port): super(Color20, self).__init__(brick, port) self.set_light_color(Type.COLORFULL) def set_light_color(self, color): """color should be one of the COLOR* Type namespace values, e.g. Type.COLORBLUE""" self.set_input_mode(color, Mode.RAW) def get_light_color(self): """Returns one of the COLOR* Type namespace values, e.g. Type.COLORRED""" return self.get_input_values().sensor_type def get_reflected_light(self, color): self.set_light_color(color) return self.get_input_values().scaled_value def get_color(self): self.get_reflected_light(Type.COLORFULL) return self.get_input_values().scaled_value get_sample = get_color nxt-python-2.2.2/nxt/sensor/analog.py0000644000175000001440000000314711565504062020264 0ustar marcususers00000000000000# nxt.sensor.analog module -- submodule for use with analog sensors # Copyright (C) 2006,2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn # # 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. from common import * class RawReading: # can be converted to the old version """A pseudo-structure holding the raw sensor values as returned by the NXT brick. """ def __init__(self, values): (self.port, self.valid, self.calibrated, self.sensor_type, self.mode, self.raw_ad_value, self.normalized_ad_value, self.scaled_value, self.calibrated_value) = values def __repr__(self): return str((self.port, self.valid, self.calibrated, self.sensor_type, self.mode, self.raw_ad_value, self.normalized_ad_value, self.scaled_value, self.calibrated_value)) class BaseAnalogSensor(Sensor): """Object for analog sensors.""" def get_input_values(self): """Returns the raw sensor values as returned by the NXT brick.""" return RawReading(self.brick.get_input_values(self.port)) def reset_input_scaled_value(self): self.brick.reset_input_scaled_value() nxt-python-2.2.2/nxt/sensor/mindsensors.py0000644000175000001440000007360711565504062021377 0ustar marcususers00000000000000# nxt.sensor.mindsensors module -- Classes implementing Mindsensors sensors # Copyright (C) 2006,2007 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn # Copyright (C) 2010 Marcus Wanner, MindSensors # # 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. from .common import * from .digital import BaseDigitalSensor, SensorInfo from .analog import BaseAnalogSensor class SumoEyes(BaseAnalogSensor): """The class to control Mindsensors Sumo sensor. Warning: long range not working for my sensor. """ #range: 5-10cm class Reading: """Contains the reading of SumoEyes sensor. left and right can be True or False. If True, then there is something there, if False, then it's empty there. """ def __init__(self, raw_reading): self.raw = raw_reading val = raw_reading.normalized_ad_value # FIXME: make it rely on raw_ad_value right = 600 < val < 700 both = 700 <= val < 900 left = 300 < val < 400 self.left = left or both self.right = right or both def __str__(self): return '(left: ' + str(self.left) + ', right: ' + str(self.right) + ')' def __init__(self, brick, port, long_range=False): super(SumoEyes, self).__init__(brick, port) self.set_long_range(long_range) def set_long_range(self, val): """Sets if the sensor should operate in long range mode (12 inches) or the short range mode (6 in). val should be True or False. """ if val: type_ = Type.LIGHT_INACTIVE else: type_ = Type.LIGHT_ACTIVE self.set_input_mode(type_, Mode.RAW) def get_sample(self): """Returns the processed meaningful values of the sensor""" return self.Reading(self.get_input_values()) class Compassv2(BaseDigitalSensor): """Class for the now-discontinued CMPS-Nx sensor. Also works with v1.1 sensors. Note that when using a v1.x sensor, some of the commands are not supported! To determine your sensor's version, use get_sensor_info().version""" I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({'command': (0x41, '> 4 return str(gs3) + str(gs2) def get_minutes(self): gm = self.read_value('minutes')[0] gm2 = gm & 0xf gm3 = gm & 0x70 gm3 = gm3 >> 4 return str(gm3) + str(gm2) def get_hours(self): gh = self.read_value('hours')[0] gh2 = gh & 0xf gh3 = gh & 0x30 gh3 = gh3 >> 4 return str(gh3) + str(gh2) def get_day(self): gwd = self.read_value('day')[0] gwd = gwd & 0x07 return gwd def get_month(self): gmo = self.read_value('month')[0] gmo2 = gmo & 0xf gmo3 = gmo & 0x10 gmo3 = gmo3 >> 4 return str(gmo3) + str(gmo2) def get_year(self): """Last two digits (10 for 2010)""" gy = self.read_value('year')[0] gy2 = gy & 0xf gy3 = gy & 0xF0 gy3 = gy3 >> 4 return str(gy3) + str(gy2) def get_date(self): gd = self.read_value('date')[0] gd2 = gd & 0xf gd3 = gd & 0x60 gd3 = gd3 >> 4 return str(gd3) + str(gd2) def hour_mode(self, mode): """Writes mode bit and re-enters hours, which is required""" if mode == 12 or 24: hm = self.read_value('hours')[0] hm2 = hm & 0x40 hm2 = hm2 >> 6 if mode == 12 and hm2 == 0: #12_HOUR = 1 hm3 = hm + 64 self.write_value('hours', (hm3, )) elif mode == 24 and hm2 == 1: #24_HOUR = 0 hm3 = hm - 64 self.write_value('hours', (hm3, )) else: print 'That mode is already selected!' else: raise ValueError('Must be 12 or 24!') def get_mer(self): mer = self.read_value('hours')[0] mer2 = mer & 0x40 mer2 = mer2 >> 6 if mer2 == 1: mer3 = mer & 0x20 mer3 = mer3 >> 0x10 return mer3 else: print 'Cannot get mer! In 24-hour mode!' def get_sample(self): """Returns a struct_time() tuple which can be processed by the time module.""" import time return time.struct_time(( int(self.get_year())+2000, int(self.get_month()), int(self.get_date()), int(self.get_hours()), int(self.get_minutes()), int(self.get_seconds()), int(self.get_day()), 0, #Should be the Julian Day, but computing that is hard. 0 #No daylight savings time to worry about here. )) class ACCL(BaseDigitalSensor): """Class for Accelerometer sensor""" I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({'sensitivity': (0x19, 'B'), 'command': (0x41, 'B'), 'x_tilt': (0x42, 'b'), 'y_tilt': (0x43, 'b'), 'z_tilt': (0x44, 'b'), 'all_tilt': (0x42, '3b'), 'x_accel': (0x45, '> bit_num return value def get_tasks(self, motor_number): addressname = 'tasks_running_m' + str(motor_number) return self.read_value(addressname)[0] def set_pid(self, pid, target, value): addressname = str(pid) + '_' + str(target) self.write_value(addressname, (value, )) def set_pass_count(self, value): self.write_value('pass_count', (value, )) def set_tolerance(self, value): self.write_value('tolerance', (value, )) MMX.add_compatible_sensor(None, 'mndsnsrs', 'NxTMMX') #Tested with version 'V1.01' class HID(BaseDigitalSensor): """Class for Human Interface Device sensors. These are connected to a computer and look like a keyboard to it.""" I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy() I2C_ADDRESS.update({'command' : (0x41, '> 8) & 0xFF d = chr(l0) + chr(l1) + data self.sock.send(d) def recv(self): data = self.sock.recv(2) l0 = ord(data[0]) l1 = ord(data[1]) plen = l0 + (l1 << 8) data = self.sock.recv(plen) if self.debug: print 'Recv:', print ':'.join('%02x' % ord(c) for c in data) return data def _check_brick(arg, value): return arg is None or arg == value def find_bricks(host=None, name=None): for h, n in bluetooth.discover_devices(lookup_names=True): if _check_brick(host, h) and _check_brick(name, n): yield BlueSock(h) nxt-python-2.2.2/nxt/fantomsock.py0000644000175000001440000000702511760011360017644 0ustar marcususers00000000000000# fantomsock.py module -- NXT_Python socket wrapper for pyfantom (Mac) # # Copyright (C) 2011 Tat-Chee Wan, 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 pyfantom from nxt.brick import Brick USB_BUFSIZE = 64 BT_BUFSIZE = 1024 #"arbitrary" FANTOMSOCK_DEBUG = False class BluetoothSocket: bsize = BT_BUFSIZE - 4 # Bluetooth socket block size '''recv() method is currently reported broken''' def __init__(self, _sock=None): # We instantiate a NXT object only when we connect if none supplied self._sock = _sock self.debug = FANTOMSOCK_DEBUG def __str__(self): return 'FantomSock BT (%s)' % self.device_name() def device_name(self): devinfo = self._sock.get_device_info() return devinfo.name def connect(self, addrport=None): if self._sock is None: if self.debug: print "No NXT object assigned" assert addrport is not None # Port is ignored addr, port = addrport paired_addr = pyfantom.pair_bluetooth(addr) if self.debug: print "BT Paired Addr: ", paired_addr self._sock = pyfantom.NXT(paired_addr) else: if self.debug: print "Using existing NXT object" return Brick(self) def send(self, data): return self._sock.write(data) def recv(self, numbytes=BT_BUFSIZE): '''currently reported broken''' return self._sock.read(numbytes) def close(self): if self._sock is not None: self._sock.close() self._sock = None class USBSocket: bsize = USB_BUFSIZE - 4 # USB socket block size def __init__(self, device=None): self._sock = device self.debug = FANTOMSOCK_DEBUG def __str__(self): return 'FantomSock USB (%s)' % self.device_name() def device_name(self): devinfo = self._sock.get_device_info() return devinfo.name def connect(self, addrport=None): if self._sock is None: if self.debug: print "No NXT object assigned" assert addrport is not None # Port is ignored addr, port = addrport self._sock = pyfantom.NXT(addr) else: if self.debug: print "Using existing NXT object" return Brick(self) def send(self, data): return self._sock.write(data) def recv(self, numbytes=USB_BUFSIZE): return self._sock.read(numbytes) def close(self): if self._sock is not None: self._sock.close() self._sock = None def find_bricks(host=None, name=None, useBT=False): for d in pyfantom.NXTIterator(useBT): nxt = d.get_nxt() if host or name: info = nxt.get_device_info() if ((host and not info.bluetooth_address == host) or (name and not info.name == name)): #name or host doesn't match continue if useBT: yield BluetoothSocket(nxt) else: yield USBSocket(nxt) nxt-python-2.2.2/nxt/lightblueglue.py0000644000175000001440000000276011565504062020346 0ustar marcususers00000000000000# bluetooth.py module -- Glue code from NXT_Python to Lightblue, allowing # NXT_Python to run on Mac without modification. Supports subset of # PyBluez/bluetooth.py used by NXT_Python. # # Copyright (C) 2007 Simon D. Levy # # 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 lightblue RFCOMM=11 def discover_devices(lookup_names=False): # parameter is ignored pairs = [] d = lightblue.finddevices() for p in d: h = p[0] n = p[1] pairs.append((h, n)) return pairs class BluetoothSocket: def __init__(self, proto = RFCOMM, _sock=None): if _sock is None: _sock = lightblue.socket(proto) self._sock = _sock self._proto = proto def connect(self, addrport): addr, port = addrport self._sock.connect( (addr, port )) def send(self, data): return self._sock.send( data ) def recv(self, numbytes): return self._sock.recv( numbytes ) def close(self): return self._sock.close() class BluetoothError(IOError): pass nxt-python-2.2.2/nxt/brick.py0000644000175000001440000001544311565504062016606 0ustar marcususers00000000000000# nxt.brick module -- Classes to represent LEGO Mindstorms NXT bricks # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2009 Marcus Wanner, rhn # Copyright (C) 2010 rhn, Marcus Wanner, zonedabone # # 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. from time import sleep from threading import Lock from nxt.error import FileNotFound, ModuleNotFound from nxt.telegram import OPCODES, Telegram from nxt.sensor import get_sensor from nxt.motcont import MotCont def _make_poller(opcode, poll_func, parse_func): def poll(self, *args, **kwargs): ogram = poll_func(opcode, *args, **kwargs) with self.lock: self.sock.send(str(ogram)) if ogram.reply: igram = Telegram(opcode=opcode, pkt=self.sock.recv()) if ogram.reply: return parse_func(igram) else: return None return poll class _Meta(type): 'Metaclass which adds one method for each telegram opcode' def __init__(cls, name, bases, dict): super(_Meta, cls).__init__(name, bases, dict) for opcode in OPCODES: poll_func, parse_func = OPCODES[opcode][0:2] m = _make_poller(opcode, poll_func, parse_func) try: m.__doc__ = OPCODES[opcode][2] except: pass setattr(cls, poll_func.__name__, m) class FileFinder(object): 'A generator to find files on a NXT brick.' def __init__(self, brick, pattern): self.brick = brick self.pattern = pattern self.handle = None def _close(self): if self.handle is not None: self.brick.close(self.handle) self.handle = None def __del__(self): self._close() def __iter__(self): results = [] self.handle, fname, size = self.brick.find_first(self.pattern) results.append((fname, size)) while True: try: handle, fname, size = self.brick.find_next(self.handle) results.append((fname, size)) except FileNotFound: self._close() break for result in results: yield result def File(brick, name, mode='r', size=None): """Opens a file for reading/writing. Mode is 'r' or 'w'. If mode is 'w', size must be provided. """ if mode == 'w': if size is not None: return FileWriter(brick, name, size) else: return ValueError('Size not specified') elif mode == 'r': return FileReader(brick, name) else: return ValueError('Mode ' + str(mode) + ' not supported') class FileReader(object): """Context manager to read a file on a NXT brick. Do use the iterator or the read() method, but not both at the same time! The iterator returns strings of an arbitrary (short) length. """ def __init__(self, brick, fname): self.brick = brick self.handle, self.size = brick.open_read(fname) def read(self, bytes=None): if bytes is not None: remaining = bytes else: remaining = self.size bsize = self.brick.sock.bsize data = [] while remaining > 0: handle, bsize, buffer_ = self.brick.read(self.handle, min(bsize, remaining)) remaining -= len(buffer_) data.append(buffer_) return ''.join(data) def close(self): if self.handle is not None: self.brick.close(self.handle) self.handle = None def __del__(self): self.close() def __enter__(self): return self def __exit__(self, etp, value, tb): self.close() def __iter__(self): rem = self.size bsize = self.brick.sock.bsize while rem > 0: handle, bsize, data = self.brick.read(self.handle, min(bsize, rem)) yield data rem -= len(data) class FileWriter(object): "Object to write to a file on a NXT brick" def __init__(self, brick, fname, size): self.brick = brick self.handle = self.brick.open_write(fname, size) self._position = 0 self.size = size def __del__(self): self.close() def close(self): if self.handle is not None: self.brick.close(self.handle) self.handle = None def tell(self): return self._position def write(self, data): remaining = len(data) if remaining > self.size - self._position: raise ValueError('Data will not fit into remaining space') bsize = self.brick.sock.bsize data_position = 0 while remaining > 0: batch_size = min(bsize, remaining) next_data_position = data_position + batch_size buffer_ = data[data_position:next_data_position] handle, size = self.brick.write(self.handle, buffer_) self._position += batch_size data_position = next_data_position remaining -= batch_size class ModuleFinder(object): 'Iterator to lookup modules on a NXT brick' def __init__(self, brick, pattern): self.brick = brick self.pattern = pattern self.handle = None def _close(self): if self.handle: self.brick.close(self.handle) self.handle = None def __del__(self): self._close() def __iter__(self): self.handle, mname, mid, msize, miomap_size = \ self.brick.request_first_module(self.pattern) yield (mname, mid, msize, miomap_size) while True: try: handle, mname, mid, msize, miomap_size = \ self.brick.request_next_module( self.handle) yield (mname, mid, msize, miomap_size) except ModuleNotFound: self._close() break class Brick(object): #TODO: this begs to have explicit methods 'Main object for NXT Control' __metaclass__ = _Meta 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) sleep(duration / 1000.0) def __del__(self): self.sock.close() find_files = FileFinder find_modules = ModuleFinder open_file = File get_sensor = get_sensor nxt-python-2.2.2/nxt/motcont.py0000644000175000001440000001332711565504062017176 0ustar marcususers00000000000000# 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 import nxt.error import 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): now = time.time() if lastrun+delay > now: diff = now - 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 = {} 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 move_to(self, port, power, tachocount, speedreg=1, smoothstart=0, brake=0): ''' Same as cmd(), except that the tachocount is subtracted from the motor's current position and that value is used to turn the motor. Power is -100-100, but the sign is rewritten as needed.''' tacho = nxt.Motor(self.brick, port).get_tacho().block_tacho_count tacho = tachocount-tacho tsign = int(tacho >= 0) * 2 - 1 tacho = abs(tacho) power = abs(power)*tsign self.cmd(port, power, tacho, speedreg, smoothstart, brake) 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() nxt-python-2.2.2/nxt/ipsock.py0000644000175000001440000000364111602375211016773 0ustar marcususers00000000000000# nxt.ipsock module -- Server socket communication with LEGO Minstorms NXT # Copyright (C) 2011 zonedaobne, 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 socket from nxt.brick import Brick class IpSock(object): def __init__(self, host, port): self.host = host self.port = port self.sock = None self.debug = False def __str__(self): return 'Server (%s)' % self.host def connect(self): if self.debug: print 'Connecting via Server...' sock = socket.socket (socket.AF_INET, socket.SOCK_STREAM) sock.connect((self.host, self.port)) #TODO: sasl authentication here? self.sock = sock if self.debug: print 'Connected.' self.send('\x98') self.type = 'ip'+self.recv() return Brick(self) def close(self): if self.debug: print 'Closing Server connection...' self.sock.send('\x99') self.sock.close() if self.debug: print 'Server connection closed.' def send(self, data): if self.debug: print 'Send:', print ':'.join('%02x' % ord(c) for c in data) self.sock.send(data) def recv(self): data = self.sock.recv(1024) if self.debug: print 'Recv:', print ':'.join('%02x' % ord(c) for c in data) return data #TODO: add a find_bricks method and a corresponding broadcast system to nxt_server? nxt-python-2.2.2/nxt/direct.py0000644000175000001440000001617511565504062016771 0ustar marcususers00000000000000# nxt.direct module -- LEGO Mindstorms NXT direct telegrams # Copyright (C) 2006, 2007 Douglas P Lau # Copyright (C) 2009 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. 'Use for direct communication with the NXT ***EXTREMELY ADVANCED USERS ONLY***' def _create(opcode, need_reply = True): 'Create a simple direct telegram' from telegram import Telegram return Telegram(True, opcode, need_reply) def start_program(opcode, fname): tgram = _create(opcode) tgram.add_filename(fname) return tgram def _parse_simple(tgram): tgram.check_status() def stop_program(opcode): return _create(opcode) def play_sound_file(opcode, loop, fname): tgram = _create(opcode, False) tgram.add_u8(loop) tgram.add_filename(fname) return tgram def play_tone(opcode, frequency, duration): 'Play a tone at frequency (Hz) for duration (ms)' tgram = _create(opcode, False) tgram.add_u16(frequency) tgram.add_u16(duration) return tgram def set_output_state(opcode, port, power, mode, regulation, turn_ratio, run_state, tacho_limit): tgram = _create(opcode, False) tgram.add_u8(port) tgram.add_s8(power) tgram.add_u8(mode) tgram.add_u8(regulation) tgram.add_s8(turn_ratio) tgram.add_u8(run_state) tgram.add_u32(tacho_limit) return tgram def set_input_mode(opcode, port, sensor_type, sensor_mode): tgram = _create(opcode, False) tgram.add_u8(port) tgram.add_u8(sensor_type) tgram.add_u8(sensor_mode) return tgram def get_output_state(opcode, port): tgram = _create(opcode) tgram.add_u8(port) return tgram def _parse_get_output_state(tgram): tgram.check_status() port = tgram.parse_u8() power = tgram.parse_s8() mode = tgram.parse_u8() regulation = tgram.parse_u8() turn_ratio = tgram.parse_s8() run_state = tgram.parse_u8() tacho_limit = tgram.parse_u32() tacho_count = tgram.parse_s32() block_tacho_count = tgram.parse_s32() rotation_count = tgram.parse_s32() return (port, power, mode, regulation, turn_ratio, run_state, tacho_limit, tacho_count, block_tacho_count, rotation_count) def get_input_values(opcode, port): tgram = _create(opcode) tgram.add_u8(port) return tgram def _parse_get_input_values(tgram): tgram.check_status() port = tgram.parse_u8() valid = tgram.parse_u8() calibrated = tgram.parse_u8() sensor_type = tgram.parse_u8() sensor_mode = tgram.parse_u8() raw_ad_value = tgram.parse_u16() normalized_ad_value = tgram.parse_u16() scaled_value = tgram.parse_s16() calibrated_value = tgram.parse_s16() return (port, valid, calibrated, sensor_type, sensor_mode, raw_ad_value, normalized_ad_value, scaled_value, calibrated_value) def reset_input_scaled_value(opcode, port): tgram = _create(opcode) tgram.add_u8(port) return tgram def message_write(opcode, inbox, message): tgram = _create(opcode) tgram.add_u8(inbox) tgram.add_u8(len(message) + 1) tgram.add_string(len(message), message) tgram.add_u8(0) return tgram def reset_motor_position(opcode, port, relative): tgram = _create(opcode) tgram.add_u8(port) tgram.add_u8(relative) return tgram def get_battery_level(opcode): return _create(opcode) def _parse_get_battery_level(tgram): tgram.check_status() millivolts = tgram.parse_u16() return millivolts def stop_sound_playback(opcode): return _create(opcode) def keep_alive(opcode): return _create(opcode) def _parse_keep_alive(tgram): tgram.check_status() sleep_time = tgram.parse_u32() return sleep_time def ls_get_status(opcode, port): 'Get status of low-speed sensor (ultrasonic)' tgram = _create(opcode) tgram.add_u8(port) return tgram def _parse_ls_get_status(tgram): tgram.check_status() n_bytes = tgram.parse_u8() return n_bytes def ls_write(opcode, port, tx_data, rx_bytes): 'Write a low-speed command to a sensor (ultrasonic)' tgram = _create(opcode) tgram.add_u8(port) tgram.add_u8(len(tx_data)) tgram.add_u8(rx_bytes) tgram.add_string(len(tx_data), tx_data) return tgram def ls_read(opcode, port): 'Read a low-speed sensor value (ultrasonic)' tgram = _create(opcode) tgram.add_u8(port) return tgram def _parse_ls_read(tgram): tgram.check_status() n_bytes = tgram.parse_u8() contents = tgram.parse_string() return contents[:n_bytes] def get_current_program_name(opcode): return _create(opcode) def _parse_get_current_program_name(tgram): tgram.check_status() fname = tgram.parse_string() return fname def message_read(opcode, remote_inbox, local_inbox, remove): tgram = _create(opcode) tgram.add_u8(remote_inbox) tgram.add_u8(local_inbox) tgram.add_u8(remove) return tgram def _parse_message_read(tgram): tgram.check_status() local_inbox = tgram.parse_u8() n_bytes = tgram.parse_u8() message = tgram.parse_string() return (local_inbox, message[:n_bytes]) #TODO Add docstrings to all methods OPCODES = { 0x00: (start_program, _parse_simple,'Starts program execution on the NXT brick'), 0x01: (stop_program, _parse_simple, 'Stops program execution on the NXT brick'), 0x02: (play_sound_file, _parse_simple, 'Plays a sound file on the NXT brick.'), 0x03: (play_tone, _parse_simple, 'Plays a tone on the NXT brick'), 0x04: (set_output_state, _parse_simple, 'Sets the state of motor ports on the NXT brick'), 0x05: (set_input_mode, _parse_simple, 'Sets the state of sensor ports on the NXT brick'), 0x06: (get_output_state, _parse_get_output_state, 'Gets the state of motor ports on the NXT brick'), 0x07: (get_input_values, _parse_get_input_values, 'Gets the state of sensor ports on the NXT brick'), 0x08: (reset_input_scaled_value, _parse_simple), #TODO What does this method do? 0x09: (message_write, _parse_simple, 'Sends a message to the mailboxes on the NXT brick'), 0x0A: (reset_motor_position, _parse_simple, 'Resets tachometers on the NXT brick'), 0x0B: (get_battery_level, _parse_get_battery_level, 'Gets the voltage of the NXT battery'), 0x0C: (stop_sound_playback, _parse_simple, 'Stops the currently running sound file'), 0x0D: (keep_alive, _parse_keep_alive, 'Resets the standby timer on the NXT brick'), 0x0E: (ls_get_status, _parse_ls_get_status), #TODO What does this method do? 0x0F: (ls_write, _parse_simple), #TODO What does this method do? 0x10: (ls_read, _parse_ls_read), #TODO What does this method do? 0x11: (get_current_program_name, _parse_get_current_program_name, 'Gets the name of the currently running program'), 0x13: (message_read, _parse_message_read, 'Reads the next message sent from the NXT brick'), } nxt-python-2.2.2/nxt/telegram.py0000644000175000001440000000655311565504062017316 0ustar marcususers00000000000000# nxt.telegram module -- LEGO Mindstorms NXT telegram formatting and parsing # Copyright (C) 2006 Douglas P Lau # Copyright (C) 2009 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. 'Used by nxt.system for sending telegrams to the NXT' from cStringIO import StringIO from struct import pack, unpack import nxt.error class InvalidReplyError(Exception): pass class InvalidOpcodeError(Exception): pass class Telegram(object): TYPE = 0 # type byte offset CODE = 1 # code byte offset DATA = 2 # data byte offset TYPE_NOT_DIRECT = 0x01 # system vs. direct type TYPE_REPLY = 0x02 # reply type (from NXT brick) TYPE_REPLY_NOT_REQUIRED = 0x80 # reply not required flag def __init__(self, direct=False, opcode=0, reply_req=True, pkt=None): self.reply = True if pkt: self.pkt = StringIO(pkt) self.typ = self.parse_u8() self.opcode = self.parse_u8() if not self.is_reply(): raise InvalidReplyError if self.opcode != opcode: raise InvalidOpcodeError, self.opcode else: self.pkt = StringIO() typ = 0 if not direct: typ |= Telegram.TYPE_NOT_DIRECT if not reply_req: typ |= Telegram.TYPE_REPLY_NOT_REQUIRED self.reply = False self.add_u8(typ) self.add_u8(opcode) def __str__(self): return self.pkt.getvalue() def is_reply(self): return self.typ == Telegram.TYPE_REPLY def add_string(self, n_bytes, v): self.pkt.write(pack('%ds' % n_bytes, v)) def add_filename(self, fname): self.pkt.write(pack('20s', fname)) def add_s8(self, v): self.pkt.write(pack(' from time import sleep import nxt.locator FREQ_C = 523 FREQ_D = 587 FREQ_E = 659 FREQ_G = 784 b = nxt.locator.find_one_brick() b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_C, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_E, 500) sleep(0.5) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_D, 500) sleep(0.5) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_G, 500) b.play_tone_and_wait(FREQ_G, 500) sleep(0.5) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_C, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_E, 500) b.play_tone_and_wait(FREQ_D, 500) b.play_tone_and_wait(FREQ_C, 750) nxt-python-2.2.2/examples/cnc.py0000644000175000001440000000251111605101272017243 0ustar marcususers00000000000000#Script to control a NXT 2-axis CNC "Pancake maker" #Illustrates controlling more than one motor at the same time without trying to #sync them. Uses the thread module. #Written 2/3/11 by Marcus Wanner # #For more info and warnings see: #http://groups.google.com/group/nxt-python/browse_thread/thread/f6ef0865ae768ef import nxt, thread, time b = nxt.find_one_brick() mx = nxt.Motor(b, nxt.PORT_A) my = nxt.Motor(b, nxt.PORT_B) motors = [mx, my] def turnmotor(m, power, degrees): m.turn(power, degrees) #here are the instructions... #the first value is the time to start the instruction #the second is the axis (0 for x, 1 for y) #the third is the power #the fourth is the degrees #it's probably not a good idea to run simultaneous turn #functions on a single motor, so be careful with this instructions = ( [0, 0, 80, 180], [0, 1, -40, 1080], [1, 0, -80, 180], [2, 0, 80, 180], [3, 1, 100, 360], [3, 0, -100, 360], ) #how long from start until the last instruction is ended length = 5 def runinstruction(i): motorid, speed, degrees = i #THIS IS THE IMPORTANT PART! thread.start_new_thread( turnmotor, (motors[motorid], speed, degrees)) #main loop seconds = 0 while 1: print "Tick %d" % seconds for i in instructions: if i[0] == seconds: runinstruction(i[1:]) seconds = seconds + 1 if seconds >= length: break time.sleep(1) nxt-python-2.2.2/examples/latency.py0000644000175000001440000000103011565504062020143 0ustar marcususers00000000000000#!/usr/bin/env python import time import nxt.locator from nxt.sensor import * b = nxt.locator.find_one_brick() #Touch sensor latency test touch = Touch(b, PORT_1) start = time.time() for i in range(100): touch.get_sample() stop = time.time() print 'touch latency: %s ms' % (1000 * (stop - start) / 100.0) #Ultrasonic sensor latency test ultrasonic = Ultrasonic(b, PORT_4) start = time.time() for i in range(100): ultrasonic.get_sample() stop = time.time() print 'ultrasonic latency: %s ms' % (1000 * (stop - start) / 100.0) nxt-python-2.2.2/examples/alpharex.py0000644000175000001440000001072211565504062020320 0ustar marcususers00000000000000r'''Alpha Rex API A high-level, object-oriented programming interface to Lego MINDSTORMS NXT's "Alpha Rex" model (see [1] for assembling instructions), along with a small collection of functions demonstrating obvious usage scenarios. 1. http://www.active-robots.com/products/mindstorms4schools/building-instructions.shtml ''' from time import sleep from nxt.brick import Brick from nxt.locator import find_one_brick from nxt.motor import Motor, PORT_A, PORT_B, PORT_C from nxt.sensor import Light, Sound, Touch, Ultrasonic from nxt.sensor import PORT_1, PORT_2, PORT_3, PORT_4 FORTH = 100 BACK = -100 class AlphaRex(object): r'''A high-level controller for the Alpha Rex model. This class implements methods for the most obvious actions performable by Alpha Rex, such as walk, wave its arms, and retrieve sensor samples. Additionally, it also allows direct access to the robot's components through public attributes. ''' def __init__(self, brick='NXT'): r'''Creates a new Alpha Rex controller. brick Either an nxt.brick.Brick object, or an NXT brick's name as a string. If omitted, a Brick named 'NXT' is looked up. ''' if isinstance(brick, basestring): brick = find_one_brick(name=brick) self.brick = brick self.arms = Motor(brick, PORT_A) self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)] self.touch = Touch(brick, PORT_1) self.sound = Sound(brick, PORT_2) self.light = Light(brick, PORT_3) self.ultrasonic = Ultrasonic(brick, PORT_4) def echolocate(self): r'''Reads the Ultrasonic sensor's output. ''' return self.ultrasonic.get_sample() def feel(self): r'''Reads the Touch sensor's output. ''' return self.touch.get_sample() def hear(self): r'''Reads the Sound sensor's output. ''' return self.sound.get_sample() def say(self, line, times=1): r'''Plays a sound file named (line + '.rso'), which is expected to be stored in the brick. The file is played (times) times. line The name of a sound file stored in the brick. times How many times the sound file will be played before this method returns. ''' for i in range(0, times): self.brick.play_sound_file(False, line + '.rso') sleep(1) def see(self): r'''Reads the Light sensor's output. ''' return self.light.get_sample() def walk(self, secs, power=FORTH): r'''Simultaneously activates the leg motors, causing Alpha Rex to walk. secs How long the motors will rotate. power The strength effected by the motors. Positive values will cause Alpha Rex to walk forward, while negative values will cause it to walk backwards. If you are unsure about how much force to apply, the special values FORTH and BACK provide reasonable defaults. If omitted, FORTH is used. ''' for motor in self.legs: motor.run(power=power) sleep(secs) for motor in self.legs: motor.idle() def wave(self, secs, power=100): r'''Make Alpha Rex move its arms. secs How long the arms' motor will rotate. power The strength effected by the motor. If omitted, (100) is used. ''' self.arms.run(power=power) sleep(secs) self.arms.idle() def wave_and_talk(): r'''Connects to a nearby Alpha Rex, then commands it to wave its arms and play the sound file 'Object.rso'. ''' robot = AlphaRex() robot.wave(1) robot.say('Object') def walk_forth_and_back(): r'''Connects to a nearby Alpha Rex, then commands it to walk forward and then backwards. ''' robot = AlphaRex() robot.walk(10, FORTH) robot.walk(10, BACK) def walk_to_object(): r'''Connects to a nearby Alpha Rex, then commands it to walk until it approaches an obstacle, then stop and say 'Object' three times. ''' robot = AlphaRex() while robot.echolocate() > 10: robot.walk(1, FORTH) robot.say('Object', 3) if __name__ == '__main__': walk_to_object() nxt-python-2.2.2/examples/test_sensors.py0000644000175000001440000000045011565504062021244 0ustar marcususers00000000000000#!/usr/bin/env python import nxt.locator from nxt.sensor import * b = nxt.locator.find_one_brick() print 'Touch:', Touch(b, PORT_1).get_sample() print 'Sound:', Sound(b, PORT_2).get_sample() print 'Light:', Light(b, PORT_3).get_sample() print 'Ultrasonic:', Ultrasonic(b, PORT_4).get_sample() nxt-python-2.2.2/README0000644000175000001440000002264411760011324015201 0ustar marcususers00000000000000NXT-Python is a package for controlling a LEGO NXT robot using the Python programming language. It can communicate using either USB or Bluetooth. It is available under the Gnu GPL v3 license. It is based on NXT_Python, where releases halted in May 2007. Requirements: * Python 2.6 or greater, but not 3.x (http://www.python.org) And at least one comm library: * Bluetooth communications: Linux/Windows: PyBluez (http://code.google.com/p/pybluez/) (package python-bluez in deb-based linuxes) Mac: LightBlue (http://lightblue.sourceforge.net/) * USB communications: PyUSB (http://sourceforge.net/projects/pyusb/) * Fantom communications (tested on Mac OSX): Pyfantom (http://pyfantom.ni.fr.eu.org/) Installation (see http://code.google.com/p/nxt-python/wiki/Installation): * Untar/unzip source package. * In package directory, run "python setup.py install" (as root), or if under windows, double-click install.bat. * To use USB on Linux as non-superuser, at a root terminal type: groupadd lego usermod -a -G lego [username] echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="0694", GROUP="lego", MODE="0660"' > /etc/udev/rules.d/70-lego.rules Getting Started: Take a look at the examples directory. Feel free to copy that code into your scripts and don't be afraid to experiment! If you are having trouble with something, you may find the solution in the docstrings (for example, help('nxt.sensor.Ultrasonic')) or even in the source code (especially for digital sensors). Notes/FAQ: (I have tried to put the most important stuff first, but it would be a good idea to read the whole section. In any case, read it all the way through before asking for help. Thanks!) -=-=-About v2-=-=- This version is part of the 2.x series of releases. Programs designed for NXT_Python or for the 1.x series of nxt-python will not work with this version. If you are trying to get an old program to work, it most likely needs a 1.x series release, which can be downloaded from the nxt-python downloads page at googlecode. New projects should use a 2.x series release (hint: this is one!) due to the new features and API improvements. Converting old projects is somewhat difficult and not officially supported, though as always you're welcome to ask for help. -=-=-Problems and Their Solutions-=-=- Support for a number of sensors has not been tested at all, due to lack of hardware. I have started a project to test this code, but the going is slow and I still can't test everything. If you have a problem with a digital sensor, see the troubleshooting guide below and don't forget to report your trouble! The Synchronized Motor support has not been extensively tested for accuracy. It seems to mostly work well but the accuracy of the braking function and the closeness of the two motors to each other have not been given a formal scientific assessment. NXT-Python has not been tested and may not work with custom nxt firmware versions (if you don't know what that means, you don't need to worry about it). However, if the firmware supports the standard LEGO USB/BT communications protocol, everything should more or less work. NXT-Python has been tested with bricks using LEGO firmware version up to 1.29 and is compatible with protocol version 1.124 (used by most if not all of the official firmwares). It has also been reported working with LeJOS. -=-=-If you co'd the SVN...-=-=- The arduino directory of an svn checkout (not included with release packages) contains a digital sensor unit test system called nxtduemu. It is intended for developers and advanced users who are looking to experiment with sensors and the digital sensor framework and classes. See Arduino/README for more info and usage instructions. Specific Stability Status: nxt.brick, nxt.telegram, nxt.direct, and nxt.system: Have been redone somewhat as of v2.2.0 but appear to work well. USB Communication System (nxt.usbsock) On Linux: Very stable and extensively tested. On Windows: Somewhat tested; seems to work pretty well. On Mac: Some users having problems. BlueTooth Communication System (nxt.bluesock, nxt.lightblueglue) On Linux: Stable; well tested with both pybluez and lightblue. On Windows: Stable; working last I checked. On Mac: Some users having problems. Internet Communications System (nxt.ipsock) Seems to work for the most part. Occasionally has hiccups. Fantom Communications System (nxt.fantomsock) On Linux: N/A (Fantom driver not supported) On Windows: Not tested. On Mac: Tested, USB interface working, Bluetooth not working. nxt.locator: Tested working with revamped logic and new code in v2.2.0. nxt.motor: Stable except for Synchronized Motor support, which is experimental at this stage and has not been extensively tested. nxt.sensor: Code not specific to a particular sensor is well-tested and working great. More than half of the sensor classes were last reported working; the rest have not to my knowlege been tested and were written blindly from the manuacturers' specifications. nxt.error: If there's a problem with this one, I'm gonna cry. Contact: NXT-Python's Head Developer: Marcus Wanner (marcus@wanners.net) The support and development mailing list: http://groups.google.com/group/nxt-python Report bugs and suggest new features at: http://code.google.com/p/nxt-python/issues/list Thanks to: Doug Lau for writing NXT_Python, our starting point. rhn for creating what would become v2, making lots of smaller changes, and reviewing tons of code. mindsensors.com (esp. Ryan Kneip) for helping out with the code for a lot of their sensors, expanding the sensors covered by the type checking database, and providing hardware for testing. HiTechnic for providing identification information for their sensors. I note that they have now included this information in their website. ;) Linus Atorf, Samuel Leeman-Munk, melducky, Simon Levy, Steve Castellotti, Paulo Vieira, zonedabone, migpics, TC Wan, jerradgenson, henryacev, Paul Hollensen, and anyone else I forgot for various fixes and additions. All our users for their interest and support! Troubleshooting Digital Sensors (don't read unless you have problems): If you are getting errors, strange behavor, or incorrect values from a digital sensor, chances are that there is a bug in our code. Follow these instructions to try and find out what's wrong: 1. Test the sensor with a different access library to make sure it's working right. 2. Check your code again. There are some weird "features" in the interfaces of some of the sensors; make sure you are doing things right. 3. Locate the sensor class's source code in nxt-python. It should be somewhere in nxt/sensor/.py, under the heading "class SensorName( BaseDigitalSensor):". Read any comments for instructions on certain things. If you get to here and are still having a problem, you can either go ahead and report it now or continue to try and find and fix the problem and then report it (or not report it at all, but that wouldn't be very nice...). Python experience required beyond this point. 4. Get the sensor's specifications from the manufacturer's website. Make sure it includes a table of I2C registers and instructions for using them. 5. Pick one of the following depending on what the problem is: ####Errors: Cause: We screwed up. Solution: Check the line mentioned in the error for incorrect syntax or other problem. A bit of python experience and maybe some googling is needed here. ####Strange Behavior (in sensors with modes/commands): Cause: nxt-python's command enumerations are incorrect. Solution: Verify them using the sensor's specs, and correct any problems. See "Incorrect Values" for more. ####Incorrect Values: Cause: nxt-python is processing the value wrong. Solution: Check what goes on in the sampling method against what the spec says should be done. If there is an inconsistency, try to fix it. Cause: nxt-python has an incorrect register number or type in I2C_ADDRESS. Solution: Verify the address (the number) and the string (the struct format string). To verify the address, use the spec. To verify the struct format, you will need to read this: or have experience with struct. Read the spec for the sensor to determine how the given value should be read, then start at the sample method and read through it, checking for problems as you go. If it seems right, go back to the I2C_ADDRESS chunk (near the top of the class) and make sure that the correct struct format string is being used. The most common problem here is values that are off by plus or minus 128 or 32768 because of an incorrect signed/unsigned setting. This can be fixed by switching the case (as in upper or lower) of the letter in the string. Other problems could include the wrong size (B, H, or L) being used, or, in the two latter ones, the wrong byte order (< or >). As always, common sense required. nxt-python-2.2.2/LICENSE0000644000175000001440000010575511565504062015344 0ustar marcususers00000000000000 GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU 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 Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) 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. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . nxt-python-2.2.2/setup.py0000644000175000001440000000110211760011520016013 0ustar marcususers00000000000000#!/usr/bin/env python from distutils.core import setup try: readme = open('README', 'r') ldesc = readme.read(20000) readme.close() except: ldesc = "" setup( name='nxt-python', version='2.2.2', author='Marcus Wanner', author_email='marcus@wanners.net', description='LEGO Mindstorms NXT Control Package', url='http://code.google.com/p/nxt-python/', license='Gnu GPL v3', packages=['nxt', 'nxt.sensor'], scripts=['scripts/nxt_push', 'scripts/nxt_test', 'scripts/nxt_filer', 'scripts/nxt_server'], long_description=ldesc ) nxt-python-2.2.2/PKG-INFO0000644000175000001440000002607211760013147015422 0ustar marcususers00000000000000Metadata-Version: 1.0 Name: nxt-python Version: 2.2.2 Summary: LEGO Mindstorms NXT Control Package Home-page: http://code.google.com/p/nxt-python/ Author: Marcus Wanner Author-email: marcus@wanners.net License: Gnu GPL v3 Description: NXT-Python is a package for controlling a LEGO NXT robot using the Python programming language. It can communicate using either USB or Bluetooth. It is available under the Gnu GPL v3 license. It is based on NXT_Python, where releases halted in May 2007. Requirements: * Python 2.6 or greater, but not 3.x (http://www.python.org) And at least one comm library: * Bluetooth communications: Linux/Windows: PyBluez (http://code.google.com/p/pybluez/) (package python-bluez in deb-based linuxes) Mac: LightBlue (http://lightblue.sourceforge.net/) * USB communications: PyUSB (http://sourceforge.net/projects/pyusb/) * Fantom communications (tested on Mac OSX): Pyfantom (http://pyfantom.ni.fr.eu.org/) Installation (see http://code.google.com/p/nxt-python/wiki/Installation): * Untar/unzip source package. * In package directory, run "python setup.py install" (as root), or if under windows, double-click install.bat. * To use USB on Linux as non-superuser, at a root terminal type: groupadd lego usermod -a -G lego [username] echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="0694", GROUP="lego", MODE="0660"' > /etc/udev/rules.d/70-lego.rules Getting Started: Take a look at the examples directory. Feel free to copy that code into your scripts and don't be afraid to experiment! If you are having trouble with something, you may find the solution in the docstrings (for example, help('nxt.sensor.Ultrasonic')) or even in the source code (especially for digital sensors). Notes/FAQ: (I have tried to put the most important stuff first, but it would be a good idea to read the whole section. In any case, read it all the way through before asking for help. Thanks!) -=-=-About v2-=-=- This version is part of the 2.x series of releases. Programs designed for NXT_Python or for the 1.x series of nxt-python will not work with this version. If you are trying to get an old program to work, it most likely needs a 1.x series release, which can be downloaded from the nxt-python downloads page at googlecode. New projects should use a 2.x series release (hint: this is one!) due to the new features and API improvements. Converting old projects is somewhat difficult and not officially supported, though as always you're welcome to ask for help. -=-=-Problems and Their Solutions-=-=- Support for a number of sensors has not been tested at all, due to lack of hardware. I have started a project to test this code, but the going is slow and I still can't test everything. If you have a problem with a digital sensor, see the troubleshooting guide below and don't forget to report your trouble! The Synchronized Motor support has not been extensively tested for accuracy. It seems to mostly work well but the accuracy of the braking function and the closeness of the two motors to each other have not been given a formal scientific assessment. NXT-Python has not been tested and may not work with custom nxt firmware versions (if you don't know what that means, you don't need to worry about it). However, if the firmware supports the standard LEGO USB/BT communications protocol, everything should more or less work. NXT-Python has been tested with bricks using LEGO firmware version up to 1.29 and is compatible with protocol version 1.124 (used by most if not all of the official firmwares). It has also been reported working with LeJOS. -=-=-If you co'd the SVN...-=-=- The arduino directory of an svn checkout (not included with release packages) contains a digital sensor unit test system called nxtduemu. It is intended for developers and advanced users who are looking to experiment with sensors and the digital sensor framework and classes. See Arduino/README for more info and usage instructions. Specific Stability Status: nxt.brick, nxt.telegram, nxt.direct, and nxt.system: Have been redone somewhat as of v2.2.0 but appear to work well. USB Communication System (nxt.usbsock) On Linux: Very stable and extensively tested. On Windows: Somewhat tested; seems to work pretty well. On Mac: Some users having problems. BlueTooth Communication System (nxt.bluesock, nxt.lightblueglue) On Linux: Stable; well tested with both pybluez and lightblue. On Windows: Stable; working last I checked. On Mac: Some users having problems. Internet Communications System (nxt.ipsock) Seems to work for the most part. Occasionally has hiccups. Fantom Communications System (nxt.fantomsock) On Linux: N/A (Fantom driver not supported) On Windows: Not tested. On Mac: Tested, USB interface working, Bluetooth not working. nxt.locator: Tested working with revamped logic and new code in v2.2.0. nxt.motor: Stable except for Synchronized Motor support, which is experimental at this stage and has not been extensively tested. nxt.sensor: Code not specific to a particular sensor is well-tested and working great. More than half of the sensor classes were last reported working; the rest have not to my knowlege been tested and were written blindly from the manuacturers' specifications. nxt.error: If there's a problem with this one, I'm gonna cry. Contact: NXT-Python's Head Developer: Marcus Wanner (marcus@wanners.net) The support and development mailing list: http://groups.google.com/group/nxt-python Report bugs and suggest new features at: http://code.google.com/p/nxt-python/issues/list Thanks to: Doug Lau for writing NXT_Python, our starting point. rhn for creating what would become v2, making lots of smaller changes, and reviewing tons of code. mindsensors.com (esp. Ryan Kneip) for helping out with the code for a lot of their sensors, expanding the sensors covered by the type checking database, and providing hardware for testing. HiTechnic for providing identification information for their sensors. I note that they have now included this information in their website. ;) Linus Atorf, Samuel Leeman-Munk, melducky, Simon Levy, Steve Castellotti, Paulo Vieira, zonedabone, migpics, TC Wan, jerradgenson, henryacev, Paul Hollensen, and anyone else I forgot for various fixes and additions. All our users for their interest and support! Troubleshooting Digital Sensors (don't read unless you have problems): If you are getting errors, strange behavor, or incorrect values from a digital sensor, chances are that there is a bug in our code. Follow these instructions to try and find out what's wrong: 1. Test the sensor with a different access library to make sure it's working right. 2. Check your code again. There are some weird "features" in the interfaces of some of the sensors; make sure you are doing things right. 3. Locate the sensor class's source code in nxt-python. It should be somewhere in nxt/sensor/.py, under the heading "class SensorName( BaseDigitalSensor):". Read any comments for instructions on certain things. If you get to here and are still having a problem, you can either go ahead and report it now or continue to try and find and fix the problem and then report it (or not report it at all, but that wouldn't be very nice...). Python experience required beyond this point. 4. Get the sensor's specifications from the manufacturer's website. Make sure it includes a table of I2C registers and instructions for using them. 5. Pick one of the following depending on what the problem is: ####Errors: Cause: We screwed up. Solution: Check the line mentioned in the error for incorrect syntax or other problem. A bit of python experience and maybe some googling is needed here. ####Strange Behavior (in sensors with modes/commands): Cause: nxt-python's command enumerations are incorrect. Solution: Verify them using the sensor's specs, and correct any problems. See "Incorrect Values" for more. ####Incorrect Values: Cause: nxt-python is processing the value wrong. Solution: Check what goes on in the sampling method against what the spec says should be done. If there is an inconsistency, try to fix it. Cause: nxt-python has an incorrect register number or type in I2C_ADDRESS. Solution: Verify the address (the number) and the string (the struct format string). To verify the address, use the spec. To verify the struct format, you will need to read this: or have experience with struct. Read the spec for the sensor to determine how the given value should be read, then start at the sample method and read through it, checking for problems as you go. If it seems right, go back to the I2C_ADDRESS chunk (near the top of the class) and make sure that the correct struct format string is being used. The most common problem here is values that are off by plus or minus 128 or 32768 because of an incorrect signed/unsigned setting. This can be fixed by switching the case (as in upper or lower) of the letter in the string. Other problems could include the wrong size (B, H, or L) being used, or, in the two latter ones, the wrong byte order (< or >). As always, common sense required. Platform: UNKNOWN