(* ::Package:: *) (************************************************************************) (* This file was generated automatically by the Mathematica front end. *) (* It contains Initialization cells from a Notebook file, which *) (* typically will have the same name as this file except ending in *) (* ".nb" instead of ".m". *) (* *) (* This file is intended to be loaded into the Mathematica kernel using *) (* the package loading commands Get or Needs. Doing so is equivalent *) (* to using the Evaluate Initialization Cells menu command in the front *) (* end. *) (* *) (* DO NOT EDIT THIS FILE. This entire file is regenerated *) (* automatically each time the parent Notebook file is saved in the *) (* Mathematica front end. Any changes you make to this file will be *) (* overwritten. *) (************************************************************************) (*: =================================================================*) (* *) (*:NAME: Math4NXT` *) (*:Context: Math4NXT` *) (*:Title: NXT package for the NXT *) (*:Reference: Cousineau, D. (2011) Controling the NXT *) (*:Author:Denis Cousineau *) (* *) (*: =================================================================*) (* *) (*:Summary:This package provides NXT* functions that send "direct *) (* commands " to the NXT brick from Lego. Directs commands *) (* are documented in Lego (R) Mindstorms NXT Bluetooth (R) *) (* Developer kit ", available from the Mindstorms web site: *) (* http://mindstorms.lego.com go to Support, Files. *) (* *) (* This package also provides M4N* functions that are higher-*) (* level functions, making it easier to interact with the NXT*) (* *) (* This package comes with UsingMath4NXT.nb, containing *) (* example use of the commands in the package. *) (* *) (* This package comes with MotorControl22.rxe, *) (* (c) Rainer Schnitzler& Linus Atorf (2008). *) (* See www.mindstorms.rwth-aachen.de. *) (* Distributed under the GPL 3.0 licence. *) (* *) (* This package is compatible with Firmware 1.27 and over. *) (* *) (* This package requires the package SerialIO, obtaind from *) (* the Wolfram Library Archive site library.wolfram.com. *) (* This package works on most Linux, Mac, Windows 32 & 64 bit*) (* The SerialIO package is NOT provided with Math4NXT. *) (* *) (*: =================================================================*) (* *) (*:Limitations: There is no direct command to set the volume of the *) (* NXT. We add this function, created using IOMapWrite. *) (* Likewise, there is no direct command to set the sleep *) (* time. However, we were unable to program one. Use the *) (* brick directly. *) (* *) (*: =================================================================*) (* *) (*:Things to know: the file extensions used by the NXT are: *) (* firmware *.rfw *) (* modules *.mod *) (* programs *.rxe *) (* programs generated on brick *.rpg *) (* try-me programs *.rtm *) (* sounds *.rso *) (* graphics *.ric *) (* datalog files *.rdt *) (* *) (*: =================================================================*) (* *) (* The most common error messages returned by the NXT are: *) (* {"Success",0}, *) (* {"No more handles",129}, *) (* {"No space",130}, *) (* {"No more files",131}, *) (* {"End of file expected",132}, *) (* {"End of file",133}, *) (* {"Not a linear file",134}, *) (* {"File not found",135}, *) (* {"Handle allready closed",136}, *) (* {"No linear space",137}, *) (* {"Undefined error",138}, *) (* {"File is busy",139}, *) (* {"No write buffers",140}, *) (* {"Append not possible",141}, *) (* {"File is full",142}, *) (* {"File exists",143}, *) (* {"Module not found",144}, *) (* {"Out of boundary",145}, *) (* {"Illegal file name",146}, *) (* {"Illegal handle",147} *) (* *) (*: =================================================================*) (* *) (* The commands that were not programmed in this package but that *) (* are recognized byt NXt are: *) (* Open write linear *) (* Open read linear command (internal command) *) (* Boot *) (* Delete User Flash *) (* Poll Command Length *) (* Poll Command *) (* Bluetooth Factory Reset *) (* If there is a need for it, I will add them to this package. *) (* *) (*: =================================================================*) (* ::Input::Plain:: *) BeginPackage["Math4NXT`","SerialIO`"] (*Public definitions*) Options[NXT]={ResultFormat->Named,Echo->Off}; toSBYTE::usage="toSBYTE[value] converts value to a Signed byte."; fromSBYTE::usage="fromSBYTE[byte] converts a byte to a signed value."; toUWORD::usage="toUWORD[value] converts value to a Unsigned Word (2 bytes)."; fromUWORD::usage="fromUWORD[{LSByte,MSbyte}] converts 2 bytes to a value."; toSWORD::usage="toSWORD[value] converts value to a Signed Word (2 bytes)."; fromSWORD::usage="fromSWORD[{LSByte,MSbyte}] converts 2 bytes to a signed value."; toULONG::usage="toULONG[value] converts value to a Unsigned Long (4 bytes)."; fromULONG::usage="fromULONG[{LSByte,Byte, Byte, MSByte}] converts 4 bytes to a value."; toSLONG::usage="toSLONG[value] converts value to a Signed Long (4 bytes)."; fromSLONG::usage="fromSLONG[{LSByte,Byte, Byte, MSByte}] converts 4 bytes to a signed value."; (*Here is a list of a few constants, in case you prefer use them*) (* sensors*) SENSOR1=0; SENSOR2=1; SENSOR3=2; SENSOR4=3; SENSOR1::usage="SENSOR1 is an alias for the first sensor (sensor port 0)."; SENSOR2::usage="SENSOR2 is an alias for the first sensor (sensor port 1)."; SENSOR3::usage="SENSOR3 is an alias for the first sensor (sensor port 2)."; SENSOR4::usage="SENSOR4 is an alias for the first sensor (sensor port 3)."; (* sensor type*) NOSENSOR=0; SWITCH=1; TEMPERATURE=2; REFLECTION=3; ANGLE=4; LIGHTACTIVE=5; LIGHTINACTIVE=6; SOUNDDB=7; SOUNDDBA=8; CUSTOM=9; LOWSPEED=10; LOWSPEED9V=11; NOOFSENSORTYPES=12; NOSENSOR::usage=""; SWITCH::usage=""; TEMPERATURE::usage=""; REFLECTION::usage=""; ANGLE::usage=""; LIGHTACTIVE::usage=""; LIGHTINACTIVE::usage=""; SOUNDDB::usage=""; SOUNDDBA::usage=""; CUSTOM::usage=""; LOWSPEED::usage=""; LOWSPEED9V::usage=""; NOOFSENSORTYPES::usage=""; (* sensor mode*) RAWMODE=0; BOOLEANMODE=32; TRANSITIONCNTMODE=64; PERIODCOUNTERMODE=96; PCTFULLSCALEMODE=128; CELSIUSMODE=160; FAHRENHEITMODE=192; ANGLESTEPSMODE=224; SLOPEMASK=31; MODEMASK=224; RAWMODE::usage=""; BOOLEANMODE::usage=""; TRANSITIONCNTMODE::usage=""; PERIODCOUNTERMODE::usage=""; PCTFULLSCALEMODE::usage=""; CELSIUSMODE::usage=""; FAHRENHEITMODE::usage=""; ANGLESTEPSMODE::usage=""; SLOPEMASK::usage=""; MODEMASK::usage=""; (* motors*) MOTORA=0; MOTORB=1; MOTORC=2; MOTORALL=255; MOTORA::usage="MOTORA is an alias for the first motor port (motor port 0)."; MOTORB::usage="MOTORB is an alias for the second motor port (motor port 1)."; MOTORC::usage="MOTORC is an alias for the third motor port (motor port 2)."; MOTORALL::usage=""; (* output mode*) MOTORON=1; BRAKE=2; REGULATED=4; MOTORON::usage=""; BRAKE::usage=""; REGULATED::usage=""; (* output regulation mode*) REGULATIONMODEIDLE=0; REGULATIONMODEMOTORSPEED=1; (*les moteurs avec ce mode roulent \[AGrave] vitesse constante*) REGULATIONMODEMOTORSYNC=2; (*les moteurs qui ont ce mode roulent de facon synchrone*) REGULATIONMODEIDLE::usage=""; REGULATIONMODEMOTORSPEED::usage=""; REGULATIONMODEMOTORSYNC::usage=""; (* output run state*) MOTORRUNSTATEIDLE=0; MOTORRUNSTATERAMPUP=16; MOTORRUNSTATERUNNING=32; MOTORRUNSTATERAMPDOWN=64; MOTORRUNSTATEIDLE::usage=""; MOTORRUNSTATERAMPUP::usage=""; MOTORRUNSTATERUNNING::usage=""; MOTORRUNSTATERAMPDOWN::usage=""; SerialIO`SerialWrite[SerialIO`Private`port:SerialPort[_String,_Integer],SerialIO`Private`data_Integer]:= SerialIO`Private`serialWrite[ SerialIO`Private`serialPortNumber[SerialIO`Private`port], FromCharacterCode[SerialIO`Private`data] ]/;0<=SerialIO`Private`data<=255 SerialIO`SerialWrite[SerialIO`Private`port:SerialPort[_String,_Integer],SerialIO`Private`data_List]:= Map[ SerialIO`SerialWrite[SerialIO`Private`port,#]&, SerialIO`Private`data ] NXTStartProgram::usage="NXTStartProgram[port, filename] starts the program of that name on the NXT. Programs have extensions .RXE. Default option is Reply\[Rule]False."; NXTStopProgram::usage="NXTStopProgram[port] stops the currently running program. Default option is Reply\[Rule]False."; NXTPlaySoundFile::usage="NXTPlaySoundFile[port, filename] plays the sound file. Sound files have extensions .RSO. Default options are Reply\[Rule]False, Loop\[Rule]False."; NXTPlayTone::usage="NXTPlayTone[port,frequency, duration (ms)] plays the tone. Default option is Reply\[Rule]False."; NXTSetOutputState::usage="NXTSetOutputState[port,motor,power,mode, regulationmode,turnratio,runstate, tacholimit] sets the motor to these settings (see Lego documentation). Default option is Reply\[Rule]False."; NXTSetInputMode::usage="NXTSetInputMode[port,sensor,sensortype,sensormode] sets the sensors (see Lego documentation). Default option is Reply\[Rule]False."; NXTGetOutputState::usage="NXTGetOutputState[port,motor] reads the tacho counts of the motor. Default option is Reply\[Rule]True."; NXTGetInputValues::usage="NXTGetInputValues[port,sensor] reads the sensor. Default option is Reply\[Rule]True."; NXTResetInputScaledValue::usage="NXTResetInputScaledValue[port,sensor] resets the sensor to zero. Reply\[Rule]False."; NXTMessageWrite::usage="NXTMessageWrite[port,boxnumber,msg] write a message (given as a string) in the box number. Default option is Reply\[Rule]False."; NXTResetMotorPosition::usage="NXTResetMotorPosition[port,motor,mode] resets the tacho count. Default option is Reply\[Rule]False."; NXTGetBatteryLevel::usage="NXTGetBatteryLevel[port] returns the battery level (mV). Default option is Reply\[Rule]True."; NXTStopSound::usage="NXTStopSound[brick] stop the file sound if played in loop. Default option is Reply\[Rule]False."; NXTKeepAwake::usage="NXTKeepAwake[port] reset the timer before the NXT goes to sleep mode. Default option is Reply\[Rule]False."; NXTGetSleepTime::usage="NXTGetSleepTime[port] returns the amount of time (ms) after which the brick goes to sleep. Also resets the timer. Default option is Reply\[Rule]True."; NXTLSGetStatus::usage="NXTGetStatus[port,sensor] checks that the Low-Speed Sensor (I2C compatible) is ready. Default option is Reply\[Rule]True."; NXTLSWrite::usage="NXTLSWrite[port,sensor,NumberOfBytesExpectedInReturn, BytesToSend] sends bytes to the Low-Speed Sensor (I2C compatible) and informs it of the length of the response. Default option is Reply\[Rule]True."; NXTLSRead::usage="NXTLSREad[port,sensor] reads the Low-Speed Sensor (I2C compatible). Default option is Reply\[Rule]True."; NXTGetCurrentProgramName::usage="NXTGetCurrentProgramName[port] returns the name of the program currently running. Default option is Reply\[Rule]True."; NXTMessageRead::usage="NXTMessageRead[port,boxnumber,localboxno] reads a message from the NXT. Default options are Reply\[Rule]True, Remove\[Rule]True."; NXTOpenRead::usage="NXTOpenRead[port,filename] opens a file in Read mode and return a handle to the file. Default option is Reply\[Rule]True."; NXTOpenWrite::usage="NXTOpenWrite[port,filename,filesize] creates a file in Write mode and return a handle to the file. Default option is Reply\[Rule]True."; NXTRead::usage="NXTRead[port,handle,length] reads a number of bytes given by length from the handled file. Default option is Reply\[Rule]True."; NXTWrite::usage="NXTWrite[port,handle,data] write in the file handled the data, a list of bytes. Default option is Reply\[Rule]True."; NXTClose::usage="NXTClose[port,handle] close the file given by handle. Default option is Reply\[Rule]True."; NXTDelete::usage="NXTDelete[port,filename] delete the file named. Default option is Reply\[Rule]True."; NXTFindFirst::usage="NXTFindFirst[port,filename] locates the first file with the given filename and returns a handle. Wildcards can be used. Default option is Reply\[Rule]True."; NXTFindNext::usage="NXTFindNext[port,handle] locates the subsequent file from the NXTFindFirst command of the handle. Default option is Reply\[Rule]True."; NXTGetFirmwareVersion::usage="NXTGetFirmwareVersion[port] returns the protocol version and the firmware version."; NXTRequestFirstModule::usage="NXTRequestFirstModule[port,filename] locates the first module with the given filename and returns a handle and returns a moduleID (4 bytes). Modules are files with extension .mod. Wildcards can be used."; NXTRequestNextModule::usage="NXTRequestNextModule[port,handle] locates the subsequent module from the NXTFindFirstModule of the handle and returns a moduleID (4 bytes)."; NXTCloseModule::usage="NXTCloseModule[port,handle] closes the module handled."; NXTReadIOMap::usage="NXTReadIOMap[port,ModuleID,Offset,length] reads the memory bytes located at offset from the start of the module."; NXTWriteIOMap::usage="NXTWriteIOMap[port,ModuleID,Offset,length] writes the memory bytes, starting at offset from the start of the module."; NXTSetBrickName::usage="NXTSetBrickName[port,name] changes the name of the NXT displayed on the LCD screen."; NXTGetDeviceInfo::usage="NXTGetDeviceInfo[port] returns the NXT name, the Bluetooth address and the available memory."; NXTSetVolume::usage="NXTSetVolume[port,volume] sets the volume (0 to 4) on the NXT. Default option is Reply\[Rule]False."; NXTGetVolume::usage="NXTGetVolume[port] returns the volume of the NXT speaker. Default option is Reply\[Rule]False."; (*General usage defintions for NXT* *) Reply::usage="The option Reply\[Rule]True indicates whether a reply is expected from the NXT. It is used for most of the NXT* commands"; Loop::usage="Loop is an option for NXTPlaySoundFile which indicates whether the sound file will play in loop, forever, or not. Default is False."; Remove::usage="Remove is an option for NXTReadMessage which indicates whether the read message is removed or not. Default is True."; ResultFormat::usage="ResultFormat is an option of NXT which indicates the format of the Replies. Possible formats are Raw, Formatted and Named. Default is Formatted."; Raw::usage="Raw is one possible setting for the option ResultFormat; it is the most basic output format for the option ResultFormat, just raw bytes."; Formatted::usage="Formatted is one possible setting for the option ResultFormat in which bytes are converted into numbers."; Named::usage="Named is one possible setting for the option ResultFormat. This ouptut for ResultFormat uses names to identify the results. Names are really quoted strings."; NXT::usage="NXT is used to define global options with Options[NXT]. Default options are ResultFormat\[Rule]Names, Echo\[Rule]Off."; Echo::usage="Echo is an option of NXT used to generate Debug information in the Message Window (use menu Window: Message to see this window)."; (*General usage definitions for M4N* *) M4NInitialize::usage="M4NInitialize[port] is used to inform the computer of the architecture of the robot. It performs various checks, upload MotorControl22.rxe if necessary, and the sets the sensors and the motors. Default options are {MotorA\[Rule]None,MotorB\[Rule]None,MotorC\[Rule]None,Sensor1\[Rule]None,Sensor2\[Rule]None,Sensor3\[Rule]None,Sensor4\[Rule]None,SetVolume\[Rule]Automatic,SetName\[Rule]Automatic}"; M4NSetSensor::usage="M4NSetSensor[port, opt] is used to set the sensor types. Defaulf options are {Sensor1\[Rule]None,Sensor2\[Rule]None,Sensor3\[Rule]None,Sensor4\[Rule]None,ResetTacho\[Rule]False}. Possible sensor types are TouchSensor, LightSensor, LightActiveSensor, SoundSensor, \[IndentingNewLine]PulseSensor, RotationSensor, UltrasoundSensor and TemperatureSensor."; M4NReadSensor::usage="M4NReadSensor[port, sensornumber] reads the current state of the sensor."; M4NSetMotor::usage="M4NSetMotor[port] is used to inform the computer of the motor types. Default options are {MotorA\[Rule]None,MotorB\[Rule]None,MotorC\[Rule]None,ResetTacho\[Rule]False}. Possible motor types are RegularMotor and TachoMotor."; M4NRunMotor::usage="M4NRunMotor[port,motors] runs the specified motors (0, 1, or 2 or a list of those).Default options are {MotorPower\[Rule]100,TachoLimit\[Rule]0,SpeedRegulation\[Rule]True}."; M4NRunMotorFor::usage="M4NRunMotorFor[port,motors] runs the specified motors (0, 1, or 2 or a list of those) for a specified number of degrees provided by TachoLimit. Default options are {MotorPower\[Rule]100,TachoLimit\[Rule]360,HoldBrake\[Rule]False,SpeedRegulation\[Rule]True, SmoothStart\[Rule]True}."; M4NBrakeMotor::usage="M4NBrakeMotor[port,motors] breaks the specified motors (0, 1 or 2 or a list of those). Default option is Duration\[Rule]1 the duration (sec) of the breaking (its takes too much power)."; M4NStopMotor::usage="M4NStopMotor[port,motors] cease to power the specified motors (0, 1 or 2 or a list of those)."; M4NMotorFreeQ::usage="M4NMotorFreeQ[port,motor] indicates if the specified motor (0, 1 or 2) is occupied by a M4NRunMotorFor operation."; M4NUploadFromNXTFile::usage="M4NUploadFromNXTFile[port,filename] retrieve a file from the NXT. Default option is ContentType\[Rule]\"Binary\"."; M4NDownloadToNXTFile::usage="M4NDownloadToNXTFile[port,filename,content] writes the content list into the NXT in a filename."; M4NFileNames::usage="M4NFileNames[port,filestem] lists all the files on the NXT with a filename corresponding to filestem. Wildcards are allowed. Default option is FileDetails\[Rule]None."; M4NFileExistsQ::usage="M4NFileExistsQ[port,filename] indicates if the file named exists on the NXT."; M4NSearchModules::usage="M4NSearchModules[port,targetbytes] scans all the module content for the presence of the target bytes. Default option is ListModules\[Rule]False"; ContentType::usage="ContentType is an option for M4NUploadFromNXTFile that indicates if the data to be retrieved are to be displayed as \"Binary\" or as \"String\". "; FileDetails::usage="FileDetails is an option for M4NFileNames which indicates if filename and filesize are to be displayed (All) or only filenames (None)."; ListModules::usage="ListModules is an option for M4NSearchModules indicating whether a list of all the modules should be printed." MotorA::usage="The option to specify the first motor type, either RegularMotor or TachoMotor."; MotorB::usage="The option to specify the second motor type, either RegularMotor or TachoMotor."; MotorC::usage="The option to specify the third motor type, either RegularMotor or TachoMotor."; Sensor1::usage="The option to specify the first sensor type, either ."; Sensor2::usage="The option to specify the second sensor type, either ."; Sensor3::usage="The option to specify the third sensor type, either ."; Sensor4::usage="The option to specify the fourth sensor type, either ."; SetVolume::usage="SetVolume is an option for M4NInitialize that specify the volume of the NXT between 0 and 4. The Default SetVolume\[Rule]Automatic does not change the volume."; SetName::usage="SetName is an option for M4NInitialize that specify the name of the NXT. The default SetName\[Rule]Automatic does not change the name."; MotorControlPath::usage="MotorControlPath is an option for M4NInitialize that specifies where the program MotorControl22.rxe is located. Default value is the location of the notebook."; MotorPower::usage="The option to specify the power to sent to motors, between -100 and +100."; TachoLimit::usage="The option to specify the number of degress the motors must turn before stopping. 0 means no limit."; SpeedRegulation::usage="The option to specify if the speed is regulated."; HoldBrake::usage="The option to specify if brakes must be applied at the end of the movement."; SmoothStart::usage="The option to specify if a smoothstart is required at the beginning of the movement."; Duration::usage="Duration is an option for M4NBrakeMotor used to specify for how long the brakes must be applied before returning to float mode (saves battery)."; ResetTacho::usage="ResetTacho is an option for M4NSetSensor or M4NInitialize used to indicate that the tacho counter should be reset to zero."; RegularMotor::usage="RegularMotor is a possible value for the option Motor? used in M4NSetSensor or M4NInitialize toidentify regular motors (i.e. having no tacho counter)."; TachoMotor::usage="TachoMotor is a possible value for the option Motor? used in M4NSetSensor or M4NInitialize to identify tacho motors (i.e. those containing an internal tacho counter)."; TouchSensor::usage="TouchSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a touch sensor."; LightSensor::usage="LightSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a light sensor."; LightActiveSensor::usage="LightActiveSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a light sensor with light on."; SoundSensor::usage="SoundSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a sound sensor."; UltrasoundSensor::usage="UltrasoundSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a ultrasound sensor."; TemperatureSensor::usage="TemperatureSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a temperature sensor."; (*not implemented yet*)PulseSensor::usage="PulseSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a pulse sensor."; (*not implemented yet*)RotationSensor::usage="RotationSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a rotation sensor."; (*not implemented yet*)(*not implemented yet*)GyroSensor::usage="GyroSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify a HiTecnic gyroscope."; (*not implemented yet*)ColorSensor::usage="ColorSensor is a possible value for the option Sensor? used in M4NSetSensor or M4NInitialize to identify the NXT color sensor."; (*Begin private*) Begin["`Private`"] (*conversion functions*) toSBYTE[value_]:=If[value>=0,value,2^8+value] fromSBYTE[value_]:=If[value>=2^7,value-2^8,value] toUWORD[value_]:={Mod[value,256],Quotient[value,256]} fromUWORD[bytelo_,bytehi_]:=bytehi*256^1+bytelo*256^0 fromUWORD[{bytelo_,bytehi_}]:=bytehi*256^1+bytelo*256^0 toSWORD[value_]:=toUWORD[If[value>=0,value,2^16+value]] fromSWORD[bytelo_,bytehi_]:=If[fromUWORD[bytelo,bytehi]>=2^15,fromUWORD[bytelo,bytehi]-2^16,fromUWORD[bytelo,bytehi]] fromSWORD[{bytelo_,bytehi_}]:=If[fromUWORD[bytelo,bytehi]>=2^15,fromUWORD[bytelo,bytehi]-2^16,fromUWORD[bytelo,bytehi]] toULONG[value_]:={Mod[Mod[value,65536],256],Quotient[Mod[value,65536],256],Mod[Quotient[value,65536],256],Quotient[Quotient[value,65536],256]} fromULONG[byte0_,byte1_,byte2_,byte3_]:=byte3*256^3+byte2*256^2+byte1*256^1+byte0*256^0 fromULONG[{byte0_,byte1_,byte2_,byte3_}]:=byte3*256^3+byte2*256^2+byte1*256^1+byte0*256^0 toSLONG[value_]:=toULONG[If[value>=0,value,2^32+value]] fromSLONG[byte0_,byte1_,byte2_,byte3_]:=If[fromULONG[byte0,byte1,byte2,byte3]>=2^31,fromULONG[byte0,byte1,byte2,byte3]-2^32,fromULONG[byte0,byte1,byte2,byte3]] fromSLONG[{byte0_,byte1_,byte2_,byte3_}]:=If[fromULONG[byte0,byte1,byte2,byte3]>=2^31,fromULONG[byte0,byte1,byte2,byte3]-2^32,fromULONG[byte0,byte1,byte2,byte3]] (*Private functions: PadZeros, ChopZeros, PrintTrace *) PadZeros[astring_,totallength_:19]:=Array[0&,totallength-StringLength[astring]+1] ChopZeros[astring_]:=StringReplace[astring,{FromCharacterCode[0]->""}] inRed[bytes_]:=Style["PC \[DoubleLongRightArrow] NXT: "<>ToString[bytes],Red] inGrn[bytes_]:=Style["PC \[DoubleLongLeftArrow] NXT: "<>ToString[bytes],Darker[Green]] ShowMessageWindow:=If[Not[Visible/.Options[MessagesNotebook[]]],SetOptions[MessagesNotebook[],WindowFloating->False,Visible -> True] ] PrintTrace[bytes_]:=Module[{}, ShowMessageWindow; SetOptions[$FrontEnd,PrintAction->"PrintToConsole"]; Print[bytes]; SetOptions[$FrontEnd,PrintAction->"PrintToNotebook"]; ] (*Defining the direct commands NXT* *) NXTStartProgram::longfile="ERROR: the file name `1` is too long for the NXT"; NXTStartProgram::notrxe="ERROR: the file name `1` is not a .rxe file"; Options[NXTStartProgram]={Reply->False}; NXTStartProgram[brick_,file_String,opt___]:=Module[{r=0,thefile=file,telegram,res}, If[FileExtension[thefile]=="",thefile=thefile<>".rxe"]; If[StringLength[thefile]>19,Message[NXTStartProgram::longfile,thefile];Return[-1]]; If[ToLowerCase[FileExtension[thefile]]!="rxe",Message[NXTStartProgram::notrxe,thefile];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTStartProgram]],r=128]; telegram={r,0,ToCharacterCode[thefile],PadZeros[thefile]}//Flatten; If[Length[telegram]!=22,Print["-2.0: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTStartProgram], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] Options[NXTStopProgram]={Reply->False}; NXTStopProgram[brick_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTStopProgram]],r=128]; telegram={r,1}//Flatten; If[Length[telegram]!=2,Print["-2.1: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTStopProgram], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTPlaySoundFile::longfile="ERROR: the file name `1` is too long for the NXT"; NXTPlaySoundFile::notrso= "ERROR: the file name `1` is not a .rso file"; Options[NXTPlaySoundFile]={Reply->False,Loop->False}; NXTPlaySoundFile[brick_,file_String,opt___]:=Module[{r=0,repeated=0, thefile=file,telegram,res}, If[FileExtension[thefile]=="",thefile=thefile<>".rso"]; If[StringLength[thefile]>18,Message[NXTPlaySoundFile::longfile,thefile];Return[-1]]; If[ToLowerCase[FileExtension[thefile]]!="rso",Message[NXTPlaySoundFile::notrso,thefile];Return[-1]]; If[Loop/.{opt}/.Options[NXTPlaySoundFile],repeated=1]; If[(Reply/.{opt}/.Options[NXTPlaySoundFile])==False,r=128]; telegram={r,2,repeated,ToCharacterCode[thefile],PadZeros[thefile]}//Flatten; If[Length[telegram]!=23,Print["-2.2: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTPlaySoundFile], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] Options[NXTPlayTone]={Reply->False}; NXTPlayTone[brick_,freq_,dur_,opt___]:=Module[{r=0,telegram,res}, If[(Reply/.{opt}/.Options[NXTPlayTone])==False,r=128]; telegram={r,3,toUWORD[freq],toUWORD[dur]}//Flatten; If[Length[telegram]!=6,Print["-2.3: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{6,0,telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTPlayTone], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTSetOutputState::notport="ERROR: not a valid output port `1`"; NXTSetOutputState::notpow="The power `1` is not between -100 and 100."; NXTSetOutputState::notmode="The mode given `1` is not valid."; NXTSetOutputState::notrmode="The regulation mode given `1` is not valid."; NXTSetOutputState::nottrat="The TurnRatio `1` is not between -100 and 100."; NXTSetOutputState::notrstate="The regulation state `1` is not valid."; Options[NXTSetOutputState]={Reply->False}; NXTSetOutputState[brick_,port_,power_,mode_, regmode_,turnratio_,runstate_, tacholim_,opt___]:=Module[{r=0,telegram,res}, If[port<0||port>2,Message[NXTSetOutputState::notport,port];Return[-1]]; If[power>100||power<-100,Message[NXTSetOutputState::notpow,power];Return[-1]]; If[mode<0||mode>7,Message[NXTSetOutputState::notmode,mode];Return[-1]]; If[regmode<0||regmode>7,Message[NXTSetOutputState::notrmode,regmode];Return[-1]]; If[turnratio>100||turnratio<-100,Message[NXTSetOutputState::nottrat,turnratio];Return[-1]]; If[Not[MemberQ[{0,16,32,64},runstate]],Message[NXTSetOutputState::notrstate,runstate];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTSetOutputState]],r=128]; telegram={r,4,port,toSBYTE[power],mode,regmode,toSBYTE[turnratio],runstate,toULONG[tacholim]}//Flatten; If[Length[telegram]!=12,Print["-2.4: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTSetOutputState], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTSetInputMode::notport="ERROR: not a valid input port `1`"; NXTSetInputMode::nottype="ERROR: not a valid type `1`"; NXTSetInputMode::notmode="ERROR: not a valid mode `1`"; Options[NXTSetInputMode]={Reply->False}; NXTSetInputMode[brick_,port_,senstype_,sensmode_,opt___]:=Module[{r=0,telegram,res}, If[port<0||port>3,Message[NXTSetInputMode::notport,port];Return[-1]]; If[senstype<0||senstype>12,Message[NXTSetInputMode::nottype,senstype];Return[-1]]; If[Not[MemberQ[{0,32,64,96,128,160,192,224,31},sensmode]],Message[NXTSetInputMode::notmode,sensmode];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTSetInputMode]],r=128]; telegram={r,5,port,senstype,sensmode}//Flatten; If[Length[telegram]!=5,Print["-2.3: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTSetInputMode], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTGetOutputState::notport="ERROR: not a valid output port `1`"; NXTGetOutputState::noreply="WARNING: without a reply, this command is pointless."; Options[NXTGetOutputState]={Reply->True}; NXTGetOutputState[brick_,port_, opt___]:=Module[{r=0,telegram,res}, If[port<0||port>2,Message[NXTGetOutputState::notport,port];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTGetOutputState]],Message[NXTGetOutputState::noreply];r=128]; telegram={r,6,port}//Flatten; If[Length[telegram]!=3,Print["-2.8: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTGetOutputState], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],fromSBYTE[res[[7]]],res[[8]],res[[9]],res[[10]],res[[11]],fromULONG[res[[Range[12,15]]]],fromSLONG[res[[Range[16,19]]]],fromSLONG[res[[Range[20,23]]]],fromSLONG[res[[Range[20,23]]]]}, Named,{"Status"->res[[5]],"Port"->res[[6]],"Power"->fromSBYTE[res[[7]]],"Mode"->res[[8]],"RegulationMode"->res[[9]],"TurnRatio"->fromSBYTE[res[[10]]],"RunState"->res[[11]],"TachoLimit"->fromULONG[res[[Range[12,15]]]],"TotalTachoCount"->fromSLONG[res[[Range[16,19]]]],"RelativeTachoCount"->fromSLONG[res[[Range[20,23]]]],"AbsoluteTachoCount"->fromSLONG[res[[Range[20,23]]]]} ] ] ] NXTGetInputValues::notport="ERROR: not a valid input port `1`"; NXTGetInputValues::noreply="WARNING: without a reply, this command is pointless."; Options[NXTGetInputValues]={Reply->True}; NXTGetInputValues[brick_,port_,opt___]:=Module[{r=0,telegram,res}, If[port<0||port>3,Message[NXTGetInputValues::notport,port];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTGetInputValues]],r=128;Message[NXTGetInputValues::noreply]]; telegram={r,7,port}//Flatten; If[Length[telegram]!=3,Print["-2.7: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTGetInputValues], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],res[[7]],res[[8]],res[[9]],res[[10]],fromUWORD[res[[11]],res[[12]]],fromUWORD[res[[13]],res[[14]]],fromSWORD[res[[15]],res[[16]]],fromSWORD[res[[17]],res[[18]]]}, Named,{"Status"->res[[5]],"Port"->res[[6]],"Valid"->res[[7]],"Calibrated"->res[[8]],"SensorType"->res[[9]],"SensorMode"->res[[10]],"RawValue"->fromUWORD[res[[11]],res[[12]]],"NormalizedValue"->fromUWORD[res[[13]],res[[14]]],"ScaledValue"->fromSWORD[res[[15]],res[[16]]],"Unused"->fromSWORD[res[[17]],res[[18]]]} ] ] ] NXTResetInputScaledValue::notport="ERROR: not a valid output port `1`"; Options[NXTResetInputScaledValue]={Reply->False}; NXTResetInputScaledValue[brick_,port_, opt___]:=Module[{r=0,telegram,res}, If[port<0||port>3,Message[NXTResetInputScaledValue::notport,port];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTResetInputScaledValue]],r=128]; telegram={r,8,port}//Flatten; If[Length[telegram]!=3,Print["-2.8: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTResetInputScaledValue], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTMessageWrite::notmsgbox="There is not a message box number `1`"; NXTMessageWrite::toolongmsg="The message `1` is too long"; Options[NXTMessageWrite]={Reply->False}; NXTMessageWrite[brick_,boxno_,msg_String, opt___]:=Module[{r=0,telegram,res}, If[boxno<0||boxno>9,Message[NXTMessageWrite::notmsgbox,boxno];Return[-1]]; If[StringLength[msg]>58,Message[NXTMessageWrite::toolongmsg,msg];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTMessageWrite]],r=128]; telegram={r,9,boxno,StringLength[msg]+1,ToCharacterCode[msg],0}//Flatten; If[Length[telegram]!=5+StringLength[msg],Print["-2.9: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTMessageWrite], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTResetMotorPosition::notport="ERROR: not a valid output port `1`"; NXTResetMotorPosition::notrmode="The mode `1` is not 0 or 1."; Options[NXTResetMotorPosition]={Reply->False}; NXTResetMotorPosition[brick_,port_,mode_, opt___]:=Module[{r=0,telegram,res}, If[port<0||port>2,Message[NXTResetMotorPosition::notport,port];Return[-1]]; If[Not[MemberQ[{0,1},mode]],Message[NXTResetMotorPosition::notrmode,mode];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTResetMotorPosition]],r=128]; telegram={r,10,port,mode}//Flatten; If[Length[telegram]!=4,Print["-2.10: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTResetMotorPosition], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTGetBatteryLevel::noreply="WARNING: without a reply, this command is pointless."; Options[NXTGetBatteryLevel]={Reply->True}; NXTGetBatteryLevel[brick_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTGetBatteryLevel]],r=128;Message[NXTGetBatteryLevel::noreply]]; telegram={r,11}//Flatten; If[Length[telegram]!=2,Print["-2.11: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTGetBatteryLevel], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],fromUWORD[res[[6]],res[[7]]]}, Named,{"Status"->res[[5]],"VoltageMV"->fromUWORD[res[[6]],res[[7]]],"Percent"->(ToString[fromUWORD[res[[6]],res[[7]]]/9000.*100]<>"%")} ] ] ] Options[NXTStopSound]={Reply->False}; NXTStopSound[brick_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTStopSound]],r=128]; telegram={r,12}//Flatten; If[Length[telegram]!=2,Print["-2.12: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTStopSound], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTGetSleepTime::noreply="WARNING: without a reply, this command is pointless."; Options[NXTGetSleepTime]={Reply->True}; NXTGetSleepTime[brick_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTGetSleepTime]],r=128;Message[NXTGetSleepTime::noreply]]; telegram={r,13}//Flatten; If[Length[telegram]!=2,Print["-2.13: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTGetSleepTime], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],fromULONG[res[[Range[6,9]]]]/1000}, Named,{"Status"->res[[5]],"SleepTimeSec"->fromULONG[res[[Range[6,9]]]]/1000} ] ] ] Options[NXTKeepAwake]={Reply->False}; NXTKeepAwake[brick_,opt___]:=Module[{r=128,telegram,res}, If[Reply/.{opt}/.Options[NXTKeepAwake],r=0]; telegram={r,13}//Flatten; If[Length[telegram]!=2,Print["-2.13: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTKeepAwake], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTLSGetStatus::notport="ERROR: not a valid output port `1`"; NXTLSGetStatus::noreply="WARNING: without a reply, this command is pointless."; Options[NXTLSGetStatus]={Reply->True}; NXTLSGetStatus[brick_,port_Integer,opt___]:=Module[{r=0,telegram,res}, If[port<0||port>2,Message[NXTLSGetStatus::notport,port];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTLSGetStatus]],r=128;Message[NXTLSGetStatus::noreply]]; telegram={r,14,port}//Flatten; If[Length[telegram]!=3,Print["-2.14: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTLSGetStatus], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]]}, Named,{"Status"->res[[5]],"NumberOfBytesReady"->res[[6]]} ] ] ] NXTLSWrite::notport="ERROR: not a valid output port `1`"; NXTLSWrite::toolong="ERROR: Too many bytes sent or requested `1`"; Options[NXTLSWrite]={Reply->True}; NXTLSWrite[brick_,port_Integer,bytestobereceived_,bytestransmitted_,opt___]:=Module[{r=0,telegram,res}, If[port<0||port>2,Message[NXTLSWrite::notport,port];Return[-1]]; If[bytestobereceived>16,Message[NXTLSWrite::toolong,bytestobereceived];Return[-1]]; If[Length[bytestransmitted]>16,Message[NXTLSWrite::toolong,bytestransmitted];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTLSWrite]],r=128]; telegram={r,15,port,Length[bytestransmitted],bytestobereceived,bytestransmitted}//Flatten; If[Length[telegram]!=5+Length[bytestransmitted],Print["-2.15: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTLSWrite], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] NXTLSRead::notport="ERROR: not a valid output port `1`"; NXTLSRead::noreply="WARNING: without a reply, this command is pointless."; Options[NXTLSRead]={Reply->True}; NXTLSRead[brick_,port_Integer,opt___]:=Module[{r=0,telegram,res}, If[port<0||port>2,Message[NXTLSRead::notport,port];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTLSRead]],r=128;Message[NXTLSRead::noreply]]; telegram={r,16,port}//Flatten; If[Length[telegram]!=3,Print["-2.16: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTLSRead], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],res[[Range[7,16]]]}, Named,{"Status"->res[[5]],"NumberOfBytesRead"->res[[6]],"Content"->res[[Range[7,22]]]} ] ] ] NXTGetCurrentProgramName::noreply="WARNING: without a reply, this command is pointless."; Options[NXTGetCurrentProgramName]={Reply->True}; NXTGetCurrentProgramName[brick_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTGetCurrentProgramName]],r=128;Message[NXTGetCurrentProgramName::noreply]]; telegram={r,17}//Flatten; If[Length[telegram]!=2,Print["-2.17: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTGetCurrentProgramName], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],ChopZeros[FromCharacterCode[res[[Range[6,25]]]]]}, Named,{"Status"->res[[5]],"FileName"->ChopZeros[FromCharacterCode[res[[Range[6,25]]]]]} ] ] ] NXTMessageRead::notmsgbox="There is not a message box number `1`"; Options[NXTMessageRead]={Reply->True,Remove->True}; NXTMessageRead[brick_,boxno_,localboxno_, opt___]:=Module[{r=0,telegram,res}, If[boxno<0||boxno>19,Message[NXTMessageRead::notmsgbox,boxno];Return[-1]]; If[localboxno<0||localboxno>19,Message[NXTMessageRead::notmsgbox,localboxno];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTMessageRead]],r=128]; telegram={r,19,boxno,localboxno,If[Remove/.{opt}/.Options[NXTMessageRead],1,0]}//Flatten; If[Length[telegram]!=5,Print["-2.19: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTMessageRead], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],res[[7]],ChopZeros[FromCharacterCode[res[[Range[8,66]]]]]}, Named,{"Status"->res[[5]],"InputBox"->res[[6]],"MessageSize"->res[[7]],"Message"->ChopZeros[FromCharacterCode[res[[Range[8,66]]]]]} ] ] ] NXTOpenRead::longfile="ERROR: the file name `1` is too long for the NXT"; Options[NXTOpenRead]={Reply->True}; NXTOpenRead[brick_,file_String,opt___]:=Module[{r=0,thefile=file,telegram,res}, If[FileExtension[thefile]=="",thefile=thefile<>".txt"]; If[StringLength[thefile]>19,Message[NXTOpenRead::longfile,thefile];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTOpenRead]],r=128]; telegram={1+r,128,ToCharacterCode[thefile],PadZeros[thefile]}//Flatten; If[Length[telegram]!=22,Print["-2.128: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTOpenRead], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],fromUWORD[res[[Range[7,8]]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"Filesize"->fromUWORD[res[[Range[7,8]]]]} ] ] ] NXTOpenWrite::longfile="ERROR: the file name `1` is too long for the NXT"; Options[NXTOpenWrite]={Reply->True}; NXTOpenWrite[brick_,file_String,filesize_,opt___]:=Module[{r=0,thefile=file,telegram,res}, If[FileExtension[thefile]=="",thefile=thefile<>".rxe"]; If[StringLength[thefile]>19,Message[NXTOpenWrite::longfile,thefile];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTOpenWrite]],r=128]; telegram={1+r,129,ToCharacterCode[thefile],PadZeros[thefile],toULONG[filesize]}//Flatten; If[Length[telegram]!=26,Print["-2.129: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTOpenWrite], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]]} ] ] ] NXTRead::toolng="Too many bytes requested `1`; maximum 140 over Bluetooth link."; NXTRead::noreply="WARNING: without a reply, this command is pointless."; Options[NXTRead]={Reply->True}; NXTRead[brick_,handle_,length_,opt___]:=Module[{r=0,telegram,res,ln}, If[length>140,Message[NXTRead::toolng,length];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTRead]],Message[NXTRead::noreply];r=128]; telegram={1+r,130,handle,toUWORD[length]}//Flatten; If[Length[telegram]!=5,Print["-2.130: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; Pause[0.001 length]; If[Reply/.{opt}/.Options[NXTRead], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],ln=fromUWORD[res[[7]],res[[8]]],res[[Range[9,9+ln-1]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"BytesRead"->(ln=fromUWORD[res[[7]],res[[8]]]),"Content"->res[[Range[9,9+ln-1]]]} ] ] ] Options[NXTWrite]={Reply->True}; NXTWrite[brick_,handle_,data_List,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTWrite]],r=128]; telegram={1+r,131,handle,data}//Flatten; If[Length[telegram]!=3+Length[data],Print["-2.131: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; Pause[0.001 Length[data]]; If[Reply/.{opt}/.Options[NXTWrite], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],fromUWORD[res[[7]],res[[8]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"BytesWritten"->fromUWORD[res[[7]],res[[8]]]} ] ] ] Options[NXTClose]={Reply->True}; NXTClose[brick_,handle_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTOpenWrite]],r=128]; telegram={1+r,132,handle}//Flatten; If[Length[telegram]!=3,Print["-2.132: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTClose], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]]} ] ] ] NXTDelete::longfile="ERROR: the file name `1` is too long for the NXT"; Options[NXTDelete]={Reply->True}; NXTDelete[brick_,file_String,opt___]:=Module[{r=0,thefile=file,telegram,res}, If[FileExtension[thefile]=="",thefile=thefile<>".txt"]; If[StringLength[thefile]>19,Message[NXTDelete::longfile,thefile];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTDelete]],r=128]; telegram={1+r,133,ToCharacterCode[thefile],PadZeros[thefile]}//Flatten; If[Length[telegram]!=22,Print["-2.133: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTDelete], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],ChopZeros[FromCharacterCode[res[[Range[6,25]]]]]}, Named,{"Status"->res[[5]],"Filename"->ChopZeros[FromCharacterCode[res[[Range[6,25]]]]]} ] ] ] NXTFindFirst::noreply="WARNING: without a reply, this command is pointless."; NXTFindFirst::longfile="ERROR: the file name `1` is too long for the NXT"; Options[NXTFindFirst]={Reply->True}; NXTFindFirst[brick_,file_String,opt___]:=Module[{r=0,thefile=file,telegram,res}, If[FileExtension[thefile]=="",thefile=thefile<>".txt"]; If[StringLength[thefile]>19,Message[NXTFindFirst::longfile,thefile];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTFindFirst]],Message[NXTFindFirst::noreply];r=128]; telegram={1+r,134,ToCharacterCode[thefile],PadZeros[thefile]}//Flatten; If[Length[telegram]!=22,Print["-2.134: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTFindFirst], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],fromULONG[res[[Range[27,30]]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"Filename"->ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],"Filesize"->fromULONG[res[[Range[27,30]]]]} ] ] ] NXTFindNext::noreply="WARNING: without a reply, this command is pointless."; Options[NXTFindNext]={Reply->True}; NXTFindNext[brick_,handle_,opt___]:=Module[{r=0,telegram,res}, If[Not[Reply/.{opt}/.Options[NXTFindNext]],Message[NXTFindNext::noreply];r=128]; telegram={1+r,135,handle}//Flatten; If[Length[telegram]!=3,Print["-2.135: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; If[Reply/.{opt}/.Options[NXTFindNext], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]],ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],fromULONG[res[[Range[27,30]]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"Filename"->ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],"Filesize"->fromULONG[res[[Range[27,30]]]]} ] ] ] Options[NXTGetFirmwareVersion]={}; NXTGetFirmwareVersion[brick_,opt___]:=Module[{telegram,res}, telegram={1,136}//Flatten; If[Length[telegram]!=2,Print["-2.136: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],ToString[res[[7]]]<>"."<>ToString[res[[6]]],ToString[res[[9]]]<>"."<>ToString[res[[8]]]}, Named,{"Status"->res[[5]],"ProtocolVersion"->ToString[res[[7]]]<>"."<>ToString[res[[6]]],"FirmwareVersion"->ToString[res[[9]]]<>"."<>ToString[res[[8]]]} ] ] Options[NXTRequestFirstModule]={}; NXTRequestFirstModule::nmtoolong="File `1` is too long"; NXTRequestFirstModule::notmod="The file `1` is not a .mod module file name"; NXTRequestFirstModule[brick_,file_,opt___]:=Module[{telegram,res, thefile=file}, If[FileExtension[thefile]=="",thefile=thefile<>".rxe"]; If[StringLength[thefile]>19,Message[NXTRequestFirstModule::nmtoolong,thefile];Return[-1]]; If[ToLowerCase[FileExtension[thefile]]!="mod",Message[NXTRequestFirstModule::notmod,thefile];Return[-1]]; telegram={1,144,ToCharacterCode[thefile],PadZeros[thefile,19]}//Flatten; If[Length[telegram]!=22,Print["-2.144: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]], ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],res[[Range[27,30]]],fromULONG[res[[Range[31,34]]]],fromUWORD[res[[Range[35,36]]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"Filename"-> ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],"ModuleID"->res[[Range[27,30]]],"ModuleSize"->fromULONG[res[[Range[31,34]]]],"ModuleIOMapSize"->fromUWORD[res[[Range[35,36]]]]} ] ] Options[NXTRequestNextModule]={}; NXTRequestNextModule[brick_,handle_,opt___]:=Module[{telegram,res}, telegram={1,145,handle}//Flatten; If[Length[telegram]!=3,Print["-2.145: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]], ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],res[[Range[27,30]]],fromULONG[res[[Range[31,34]]]],fromUWORD[res[[Range[35,36]]]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]],"Filename"-> ChopZeros[FromCharacterCode[res[[Range[7,26]]]]],"ModuleID"->res[[Range[27,30]]],"ModuleSize"->fromULONG[res[[Range[31,34]]]],"ModuleIOMapSize"->fromUWORD[res[[Range[35,36]]]]} ] ] Options[NXTCloseModule]={}; NXTCloseModule[brick_,handle_,opt___]:=Module[{telegram,res}, telegram={1,146,handle}//Flatten; If[Length[telegram]!=3,Print["-2.146: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[6]]}, Named,{"Status"->res[[5]],"Handle"->res[[6]]} ] ] NXTReadIOMap::notMID="This is not a valid Module ID (a 4-byte list) `1`"; NXTReadIOMap::toolng="the number of bytes `1` you are trying to read exceed 119"; Options[NXTReadIOMap]={}; NXTReadIOMap[brick_,ModuleID_,Offset_,length_,opt___]:=Module[{telegram,res}, If[Length[ModuleID]!=4,Message[NXTReadIOMap::notMID,ModuleID];Return[-1]]; If[length>119,Message[NXTReadIOMap::toolng,length];Return[-1]]; telegram={1,148,ModuleID,toUWORD[Offset],toUWORD[length]}//Flatten; If[Length[telegram]!=10,Print["-2.148: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; Pause[0.2]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[Range[6,9]]],fromUWORD[res[[10]],res[[11]]],res[[Range[12,12+fromUWORD[res[[10]],res[[11]]]-1]]]}, Named,{"Status"->res[[5]],"ModuleID"->res[[Range[6,9]]],"ByteRead"->fromUWORD[res[[10]],res[[11]]],"Content"->res[[Range[12,12+fromUWORD[res[[10]],res[[11]]]-1]]]} ] ] NXTWriteIOMap::notMID="This is not a valid Module ID (a 4-byte list) `1`"; NXTWriteIOMap::toolng="the number of bytes `1` you are trying to write exceed 119"; Options[NXTWriteIOMap]={}; NXTWriteIOMap[brick_,ModuleID_,Offset_,newbytes_,opt___]:=Module[{telegram,res}, If[Length[ModuleID]!=4,Message[NXTWriteIOMap::notMID,ModuleID];Return[-1]]; If[Length[newbytes]>119,Message[NXTWriteIOMap::toolng,Length[newbytes]];Return[-1]]; telegram={1,149,ModuleID,toUWORD[Offset],toUWORD[Length[newbytes]],newbytes}//Flatten; If[Length[telegram]!=10+Length[newbytes],Print["-2.149: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; Pause[0.2]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],res[[Range[6,9]]],fromUWORD[res[[10]],res[[11]]]}, Named,{"Status"->res[[5]],"ModuleID"->res[[Range[6,9]]],"ByteWritten"->fromUWORD[res[[10]],res[[11]]]} ] ] NXTSetBrickName::nmtoolong="The name `1` contains too many characters"; Options[NXTSetBrickName]={Reply->False}; NXTSetBrickName[brick_,name_String,opt___]:=Module[{r=0,telegram,res}, If[StringLength[name]>15,Message[NXTSetBrickName::nmtoolong,name];Return[-1]]; If[Not[Reply/.{opt}/.Options[NXTStopSound]],r=128]; telegram={1+r,152,ToCharacterCode[name],PadZeros[name,15]}//Flatten; If[Length[telegram]!=18,Print["-2.152: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; Pause[0.1]; If[Reply/.{opt}/.Options[NXTSetBrickName], res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]]}, Named,{"Status"->res[[5]]} ] ] ] Options[NXTGetDeviceInfo]={}; NXTGetDeviceInfo[brick_,opt___]:=Module[{telegram,res}, telegram={1,155}//Flatten; If[Length[telegram]!=2,Print["-2.155: Somethings is unexpected",telegram]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inRed[{toUWORD[Length[telegram]],telegram}//Flatten]] ]; SerialWrite[brick,{toUWORD[Length[telegram]],telegram}//Flatten]; Pause[1]; res=ToCharacterCode[SerialRead[brick]]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[inGrn[res]]]; Switch[ResultFormat/.{opt}/.Options[NXT], Raw,res, Formatted,{res[[5]],ChopZeros[FromCharacterCode[res[[Range[3,17]+3]]]],{NumberForm[BaseForm[res[[21]],16],1,NumberPadding->{"0","0"}],NumberForm[BaseForm[res[[22]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[23]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[24]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[25]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[26]],16],1,NumberPadding->{"0","0"}]},fromULONG[res[[28]],res[[29]],res[[30]],res[[31]]],fromULONG[res[[32]],res[[33]],res[[34]],res[[35]]]}, Named,{"Status"->res[[5]],"BrickName"->ChopZeros[FromCharacterCode[res[[Range[3,17]+3]]]],"BTAddress"-> {NumberForm[BaseForm[res[[21]],16],1,NumberPadding->{"0","0"}],NumberForm[BaseForm[res[[22]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[23]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[24]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[25]],16],1,NumberPadding->{"","0"}], NumberForm[BaseForm[res[[26]],16],1,NumberPadding->{"0","0"}]},"BTSignalStrength"-> fromULONG[res[[28]],res[[29]],res[[30]],res[[31]]],"FreeMemory"->fromULONG[res[[32]],res[[33]],res[[34]],res[[35]]]} ] ] NXTSetVolume::notvol="The value `1` is not a legitimate volume between 0 and 4"; Options[NXTSetVolume]={Reply->False}; NXTSetVolume[brick_,volume_Integer,opt___]:=Module[{telegram,res}, If[volume>4||volume<0,Message[NXTSetVolume::notvol,volume];Return[-1]]; NXTWriteIOMap[brick,{01,00,08,00},29,{volume},opt]; NXTWriteIOMap[brick,{01,00,12,00},36,{volume},opt]; If[Reply/.{opt}/.Options[NXTStopSound], Switch[ResultFormat/.{opt}/.Options[NXT], Raw,{3,0,2,201,0}, Formatted,{0}, Named,{"Status"->0} ] ] ] Options[NXTGetVolume]={Reply->True}; NXTGetVolume[brick_,opt___]:=Module[{r,res}, res=NXTReadIOMap[brick,{01,00,08,00},29,1,ResultFormat->Named,opt]; If[Reply/.{opt}/.Options[NXTGetVolume], Switch[ResultFormat/.{opt}/.Options[NXT], Raw,{3,0,2,201,("Content"/.res)[[1]]}, Formatted,{0,("Content"/.res)[[1]]}, Named,{"Status"->0,"Volume"->("Content"/.res)[[1]]} ] ] ] (*Defining the global commands M4N* *) MotorControl22Started=False; motortypes={None,None,None}; sensortypes={None,None,None,None}; M4NInitialize::lobat="Warning: Battery low; consider recharging..."; M4NInitialize::lofrm="Warning: Firmware outdated; some functions may not work..."; M4NInitialize::nomc="Warning: MotorControl22.rxe not on the brick; uploading (may take several minutes)..."; M4NInitialize::lnmc="Warning: MotorControl22.rxe of the wrong size; may be corrupted..."; M4NInitialize::nomem="Error: Not enough memory to download MotorControl22.rxe. Exiting."; Options[M4NInitialize]={MotorA->None,MotorB->None,MotorC->None,Sensor1->None,Sensor2->None,Sensor3->None,Sensor4->None,SetVolume->Automatic,SetName->Automatic,MotorControlPath->Automatic}; M4NInitialize[brick_,opt___]:=Module[{temp,mc,tt}, If[ToExpression[StringTake["Percent"/.NXTGetBatteryLevel[brick,opt],2]]<70, Message[M4NInitialize::lobat] ]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Firmware ok?"]]; If[ToExpression["FirmwareVersion"/.NXTGetFirmwareVersion[brick,opt]]<1.27, Message[M4NInitialize::lofrm] ]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Set brick name"]]; If[Not[(tt=(SetName/.{opt}/.Options[M4NInitialize]))===Automatic], NXTSetBrickName[brick,tt,opt] ]; Pause[0.5]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Set volume"]]; If[Not[(tt=(SetVolume/.{opt}/.Options[M4NInitialize]))===Automatic], NXTSetVolume[brick,tt,opt] ]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Setting Sensors"]]; M4NSetSensor[brick,opt]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Setting Motors"]]; M4NSetMotor[brick,opt]; "All systems ok!" ] ApplySType[brick_,sensno_,type_,opt___]:=Module[{}, Switch[type, TouchSensor,sensortypes[[sensno]]=TouchSensor;NXTSetInputMode[brick,sensno-1,1,32,opt], LightSensor,sensortypes[[sensno]]=LightSensor;NXTSetInputMode[brick,sensno-1,6,128,opt], LightActiveSensor,sensortypes[[sensno]]=LightActiveSensor;NXTSetInputMode[brick,sensno-1,5,128,opt], PulseSensor,sensortypes[[sensno]]=PulseSensor; Message[M4NSetSensor::notimp,PulseSensor], SoundSensor,sensortypes[[sensno]]=SoundSensor;NXTSetInputMode[brick,sensno-1,7,128,opt], RotationSensor,sensortypes[[sensno]]=RotationSensor; Message[M4NSetSensor::notimp,RotationSensor], UltrasoundSensor,sensortypes[[sensno]]=UltrasoundSensor; Pause[0.05]; NXTSetInputMode[brick,sensno-1,11,0,opt]; Pause[0.05]; (*set mode 02 for continuous mode at address 02,65.*) NXTLSWrite[brick,sensno-1,0,{02,65,02},Reply->False,opt]; (*flush reader*) NXTLSGetStatus[brick,sensno-1,opt]; NXTLSRead[brick,sensno-1,opt]; Pause[0.02], TemperatureSensor,sensortypes[[sensno]]=TemperatureSensor;NXTSetInputMode[brick,sensno-1,2,160,opt], None,sensortypes[[sensno]]=None;NXTSetInputMode[brick,sensno-1,0,0,opt], True,Message[M4NSetSensor::nottyp,type] ] ] M4NSetSensor::notimp="The type `1` is not yet implemented..."; M4NSetSensor::nottyp="Sensor `1` is not TouchSensor, LightSensor, PulseSensor, SoundSensor, UltrasoundSensor, TemperatureSensor or RotationSensor."; Options[M4NSetSensor]={Sensor1->None,Sensor2->None,Sensor3->None,Sensor4->None,ResetTacho->False}; M4NSetSensor[brick_,opt___]:=Module[{}, (*For each sensor named in opt*) If[MemberQ[{opt}[[All,1]],Sensor1], ApplySType[brick,1, Sensor1/.{opt}/.Options[M4NSetSensor],opt]; ]; If[MemberQ[{opt}[[All,1]],Sensor2], ApplySType[brick,2, Sensor2/.{opt}/.Options[M4NSetSensor],opt]; ]; If[MemberQ[{opt}[[All,1]],Sensor3], ApplySType[brick,3, Sensor3/.{opt}/.Options[M4NSetSensor],opt]; ]; If[MemberQ[{opt}[[All,1]],Sensor4], ApplySType[brick,4, Sensor4/.{opt}/.Options[M4NSetSensor],opt]; ]; ] M4NReadSensor::notcfg="The sensor `1` is not yet configured. Use M4NSetSensor or M4NInitialize."; M4NReadSensor::notyi="The sensor type `1` is not yet implemented."; regularsensors={LightSensor,TemperatureSensor,TouchSensor,LightActiveSensor,SoundSensor}; M4NReadSensor[brick_,sensor_,opt___]:=Module[{}, Which[ sensortypes[[sensor+1]]===None, Message[M4NReadSensor::notcfg,sensor], sensortypes[[sensor+1]]===UltrasoundSensor, NXTLSWrite[brick,sensor,1,{02,66},Reply->False,opt]; Pause[0.01]; ("Content"/.NXTLSRead[brick,sensor,opt])[[1]], MemberQ[regularsensors,sensortypes[[sensor+1]]], "ScaledValue"/.NXTGetInputValues[brick,sensor,opt], True,Message[M4NReadSensor::notyi,sensortypes[[sensor+1]]] ] ] M4NSetMotor::nottyp="Motor `1` is not RegularMotor or TachoMotor."; Options[M4NSetMotor]={MotorA->None,MotorB->None,MotorC->None,ResetTacho->False}; M4NSetMotor[brick_,opt___]:=Module[{type}, If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Get filenames"]]; temp=M4NFileNames[brick,"*.rxe",FileDetails->All,opt]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace[temp]]; If[(tt=Position[temp,"MotorControl22.rxe"])=={}, If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Download MotorControl"]]; Message[M4NInitialize::nomc]; If[("FreeMemory"/.NXTGetDeviceInfo[brick,opt])<37600,Message[M4NInitialize::nomem];Return[-1]]; If[Not[(tt=(MotorControlPath/.{opt}/.Options[M4NInitialize]))===Automatic], SetDirectory[tt] ]; mc=Import["MotorControl22.rxe","Binary"]; M4NDownloadToNXTFile[brick,"MotorControl22.rxe",mc,opt], If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Check MotorControl"]]; If[temp[[tt[[1,1]],2]]!=37530,Message[M4NInitialize::lnmc]]; ]; If[(Echo/.{opt}/.Options[NXT]),PrintTrace["Start MotorControl"]]; NXTStartProgram[brick,"MotorControl22.rxe",opt]; MotorControl22Started=True; If[Not[(type=(MotorA/.{opt}/.Options[M4NSetMotor]))===None], If[type==RegularMotor,motortypes[[1]]=RegularMotor]; If[type==TachoMotor,motortypes[[1]]=TachoMotor]; If[ResetTacho/.{opt}/.Options[M4NSetMotor],NXTMessageWrite[brick,1,"20",opt]]; ]; If[Not[(type=(MotorB/.{opt}/.Options[M4NSetMotor]))===None], If[type==RegularMotor,motortypes[[2]]=RegularMotor]; If[type==TachoMotor,motortypes[[2]]=TachoMotor]; If[ResetTacho/.{opt}/.Options[M4NSetMotor],NXTMessageWrite[brick,1,"21",opt]]; ]; If[Not[(type=(MotorC/.{opt}/.Options[M4NSetMotor]))===None], If[type==RegularMotor,motortypes[[3]]=RegularMotor]; If[type==TachoMotor,motortypes[[3]]=TachoMotor]; If[ResetTacho/.{opt}/.Options[M4NSetMotor],NXTMessageWrite[brick,1,"22",opt]]; ]; ] M4NRunMotor::notmot="The motor `1` is not a valid motor port."; M4NRunMotor::notdef="The motor `1` was not defined using M4NSetMotor."; Options[M4NRunMotor]={MotorPower->100,TachoLimit->0,SpeedRegulation->True}; M4NRunMotor[brick_,motors_,opt___]:=Module[{cmd,pow,tac,mode}, If[Min[motors]<0||Max[motors]>3, Message[M4NRunMotor::notmot,motors];Return[-1]; ]; If[Not[MotorControl22Started===True],Message[M4NMotorFreeQ::notmc];Return[-1]]; If[ListQ[motors], Table[If[motortypes[[i+1]]==None,Message[M4NRunMotor::notdef,i]],{i,motors}], If[motortypes[[motors+1]]==None,Message[M4NRunMotor::notdef,motors]] ]; cmd="4"; If[ListQ[motors], Switch[motors, {0,1},cmd=cmd<>"3", {0,2},cmd=cmd<>"4", {1,2},cmd=cmd<>"5" ], cmd=cmd<>ToString[motors] ]; pow=(MotorPower/.{opt}/.Options[M4NRunMotor]); If[pow<0,pow=100-pow]; cmd=cmd<>StringJoin[Map[ToString,PadZeros[ToString[pow],2]]]<>ToString[pow]; tac=(TachoLimit/.{opt}/.Options[M4NRunMotor]); cmd=cmd<>StringJoin[Map[ToString,PadZeros[ToString[tac],5]]]<>ToString[tac]; mode=0; If[SpeedRegulation/.{opt}/.Options[M4NRunMotor],mode=mode+1]; cmd=cmd<>ToString[mode]; NXTMessageWrite[brick,1,cmd,opt]; Pause[.03] ] M4NRunMotorFor::notmot="The motor `1` is not a valid motor port."; M4NRunMotorFor::notfre="The motor `1` is currently occupied; results uncertain."; M4NRunMotorFor::nottac="The motor `1` was not defined as a TachoMotor. You cannot have a tacholimit for such motors"; Options[M4NRunMotorFor]={MotorPower->100,TachoLimit->360,HoldBrake->False,SpeedRegulation->True, SmoothStart->True}; M4NRunMotorFor[brick_,motors_,opt___]:=Module[{cmd,pow,tac,mode}, If[Min[motors]<0||Max[motors]>3, Message[M4NRunMotorFor::notmot,motors];Return[-1]; ]; If[ListQ[motors], Table[If[Not[M4NMotorFreeQ[brick,i]],Message[M4NRunMotorFor::notfre,i]],{i,motors}], If[Not[M4NMotorFreeQ[brick,motors]],Message[M4NRunMotorFor::notfre,motors]] ]; If[Not[MotorControl22Started===True],Message[M4NMotorFreeQ::notmc];Return[-1]]; tac=(TachoLimit/.{opt}/.Options[M4NRunMotorFor]); pow=(MotorPower/.{opt}/.Options[M4NRunMotorFor]); If[pow<0,pow=100-pow]; If[ListQ[motors], Table[If[motortypes[[i+1]]!=TachoMotor&&tac>0,Message[M4NRunMotorFor::nottac,i]],{i,motors}], If[motortypes[[motors+1]]!=TachoMotor&&tac>0,Message[M4NRunMotorFor::nottac,motors]] ]; cmd="1"; If[ListQ[motors], Switch[motors, {0,1},cmd=cmd<>"3", {0,2},cmd=cmd<>"4", {1,2},cmd=cmd<>"5" ], cmd=cmd<>ToString[motors] ]; cmd=cmd<>StringJoin[Map[ToString,PadZeros[ToString[pow],2]]]<>ToString[pow]; cmd=cmd<>StringJoin[Map[ToString,PadZeros[ToString[tac],5]]]<>ToString[tac]; mode=0; If[HoldBrake/.{opt}/.Options[M4NRunMotorFor],mode=mode+1]; If[SpeedRegulation/.{opt}/.Options[M4NRunMotorFor],mode=mode+2]; If[SmoothStart/.{opt}/.Options[M4NRunMotorFor],mode=mode+4]; cmd=cmd<>ToString[mode]; NXTMessageWrite[brick,1,cmd,opt]; Pause[.03] ] M4NBrakeMotor::notmot="The motor `1` is not a valid motor port."; Options[M4NBrakeMotor]={Duration->1}; M4NBrakeMotor[brick_,motors_,opt___]:=Module[{}, If[Min[motors]<0||Max[motors]>3, Message[M4NBrakeMotor::notmot,motors];Return[-1]; ]; If[Not[MotorControl22Started===True],Message[M4NMotorFreeQ::notmc];Return[-1]]; If[ListQ[motors], Map[NXTSetOutputState[brick,#,(*power*)0,(*mode*)7,(*regmode*)1,(*turnratio*)0,(*runstate*)32,(*tacholim*)0,opt],motors], NXTSetOutputState[brick,motors,0,7,1,0,32,0,opt] ]; Pause[Duration/.{opt}/.Options[M4NBrakeMotor]]; M4NStopMotor[brick,motors,opt,opt]; ] M4NStopMotor::notmot="The motor `1` is not a valid motor port."; M4NStopMotor[brick_,motors_,opt___]:=Module[{}, If[Min[motors]<0||Max[motors]>3, Message[M4NStopMotor::notmot,motors];Return[-1]; ]; If[Not[MotorControl22Started===True],Message[M4NMotorFreeQ::notmc];Return[-1]]; If[ListQ[motors], Map[NXTSetOutputState[brick,#,(*power*)0,(*mode*)1,(*regmode*)0,(*turnratio*)0,(*runstate*)0,(*tacholim*)0,opt],motors], NXTSetOutputState[brick,motors,0,1,0,0,0,0,opt] ] ] M4NMotorFreeQ::notmot="The motor `1` is not a valid motor port."; M4NMotorFreeQ::notmc="MotorControl22.rxe is not operational. Use M4NInitialize"; M4NMotorFreeQ[brick_,motor_,opt___]:=Module[{}, If[motor<0||motor>3, Message[M4NStopMotor::notmot,motor];Return[-1]; ]; If[Not[MotorControl22Started===True],Message[M4NMotorFreeQ::notmc];Return[-1]]; NXTMessageWrite[brick,1,"3"<>ToString[motor],opt]; Pause[.03]; If[StringTake["Message"/.NXTMessageRead[brick,0,1,ResultFormat->Named,opt],{2}]=="1",True,False] ] M4NUploadFromNXTFile::ntfile="File `1` does not exist on NXT"; Options[M4NUploadFromNXTFile]={ContentType->"Binary"}; M4NUploadFromNXTFile[brick_,filename_String, opt___]:=Module[{res,ln,h,fullres}, If[Not[M4NFileExistsQ[brick,filename]], Message[M4NUploadFromNXTFile::ntfile,filename,opt]; Return[-1]; ]; res=NXTOpenRead[brick,filename,ResultFormat->Named,opt]; ln=("Filesize"/.res); h=("Handle"/.res); If[ln>140, fullres=Table[ NXTRead[brick,h,140,opt], {j,1,Quotient[ln,140]} ]; fullres=Append[fullres,NXTRead[brick,h,Mod[ln,140]]]; res={"Status"->(("Status"/.fullres)//First),"Handle"->(("Handle"/.fullres)//First),"BytesRead"->(("BytesRead"/.fullres)//Total),"Content"->(("Content"/.fullres)//Flatten)}, res=NXTRead[brick,h,ln,opt] ]; NXTClose[brick,0,ResultFormat->Named,opt]; If[(ContentType/.{opt}/.Options[M4NUploadFromNXTFile])=="String", res[[4,2]]=FromCharacterCode[res[[4,2]]], res ] ] M4NDownloadToNXTFile::fexist="Warning: File `1` already exists on NXT; overwriting"; M4NDownloadToNXTFile[brick_,filename_,content_,opt___]:=Module[{h}, (*runs like NXTRead but without the limitation due to Bluetooth link*) If[M4NFileExistsQ[brick,filename,opt], Message[M4NDownloadToNXTFile::fexist,filename]; NXTDelete[brick,filename,opt] ]; h=("Handle"/.NXTOpenWrite[brick,filename,Length[content],ResultFormat->Named,opt]); If[StringQ[content], (*131*)NXTWrite[brick,h,ToCharacterCode[content],opt], (*131*)NXTWrite[brick,h,content,opt] ]; res=NXTClose[brick,h,ResultFormat->Named,opt]; ] Options[M4NFileNames]={FileDetails->None}; M4NFileNames[brick_,filestem_,opt___]:=Module[{filelist,res}, filelist={}; res=NXTFindFirst[brick,filestem,opt]; If[("Status"/.res)==0, filelist=Append[filelist,{("Filename"/.res),("Filesize"/.res)}]; While[("Status"/.(res=NXTFindNext[brick,"Handle"/.res,opt]))==0, filelist=Append[filelist,{("Filename"/.res),("Filesize"/.res)}]; ]; NXTClose[brick,0,opt] ]; If[(FileDetails/.{opt}/.Options[M4NFileNames])===All, filelist, filelist[[All,1]] ] ] M4NFileExistsQ[brick_,filename_String,opt___]:=Module[{filelist, res}, (*make the whole file list*) filelist={}; res=(*134*)NXTFindFirst[brick,"*.*",opt]; If[("Status"/.res)==0, filelist=Append[filelist,("Filename"/.res)]; While[("Status"/.(res=(*135*)NXTFindNext[brick,"Handle"/.res,opt]))==0, filelist=Append[filelist,("Filename"/.res)]; ]; NXTClose[brick,0,opt] ]; MemberQ[filelist,filename] ] Options[M4NSearchModules]={ListModules->False}; M4NSearchModules[brick_,cible_List,opt___]:=Module[{AllMods,res,AllCont,temp,sol}, (*first step, establish the list of all modules with their lengths*) AllMods={}; res=NXTRequestFirstModule[brick,"*.mod",opt]; While[("Status"/.res)==0, AppendTo[AllMods,res]; res=NXTRequestNextModule[brick,"Handle"/.res,opt]; ] (*146*)NXTCloseModule[brick,"Handle"/.res,opt]; AllMods=Sort[AllMods,#1[[4,2,3]]<#2[[4,2,3]]&]; If[ListModules/.{opt}/.Options[M4NSearchModules],Print[AllMods//TableForm]]; (* retrieve all the contents*) AllCont=Table[ If[("ModuleIOMapSize"/.AllMods[[i]])>119, temp=Table[ NXTReadIOMap[brick,"ModuleID"/.AllMods[[i]],119(j-1),Min[("ModuleIOMapSize"/.AllMods[[i]])-119(j-1),119],ResultFormat->Named,opt], {j,1,Quotient["ModuleIOMapSize"/.AllMods[[i]],119]+1} ]; res={temp[[1,1]],temp[[1,2]],"ByteRead"->("ByteRead"/.temp//Total),"Content"->("Content"/.temp//Flatten)}, NXTReadIOMap[brick,"ModuleID"/.AllMods[[i]],0,"ModuleIOMapSize"/.AllMods[[i]],ResultFormat->Named,opt] ], {i,1,Length[AllMods]} ]; (*search the cible, and for each position found, replace sublist with ModuleID*) temp=("Content"/.AllCont); temp=Map[Partition[#,Length[cible],1]&,temp]; sol=Position[temp,cible]; If[Length[sol]>0, sol[[All,1]]=("ModuleID"/.AllCont)[[sol\[Transpose][[1]]]]; sol[[All,2]]=sol[[All,2]]-1; sol, {} ] ] End[] CellPrint[Cell[TextData[{ StyleBox["Math4NXT",FontColor->Red,Bold], StyleBox[" loaded. \nDo "], StyleBox["? Math4NXT`*",FontColor->Red,Italic], StyleBox[" to see all the functions available \nor "], StyleBox["? NXT*",FontColor->Red,Italic], StyleBox[" for the direct commands and "], StyleBox["? M4N*",FontColor->Red,Italic], StyleBox[" for the higher-level commands."] }],"Text"] ] EndPackage[]