41_ROBOT BUILDER IN LISP

1 .Controlling Robobuilder using L# Controlling Robobuilder Humanoid robot using Lisp/L# If you want to give a gripper for your robobuilder, order it now from my Shapeways shop at http://shapeways.com/shop/ip_robotics. And don’t forget to order a servo from Robosavvy to go with it. Edition 2 Phil Page 1 06/04/2010 

2 .Controlling Robobuilder using L# Index Introduction ....................................................................................................................................3 Lisp ................................................................................................................................................4 Car, Cdr, Cons, List ........................................................................................................................6 So how do these work? ...................................................................................................................6 Function and conditionals ...............................................................................................................8 Controlling the Robot ...................................................................................................................10 More PC Remote mode functions .................................................................................................13 Menu driven motions ....................................................................................................................15 Accessing the accelerometer. ........................................................................................................ 17 Access the servo directly...............................................................................................................19 How to play servo moves..............................................................................................................21 Synchronous moves ......................................................................................................................23 Smooth moves and the In-betweens ..............................................................................................24 Making complete motions............................................................................................................. 25 Loading data from CSV files ........................................................................................................ 27 Controlling the IO on a servo........................................................................................................ 29 Windows based graphics using L# !..............................................................................................30 Plotting and animation ..................................................................................................................32 Real-time accelerometer ...............................................................................................................35 Property lists.................................................................................................................................37 Blockworld / SHRDLU.................................................................................................................39 Searching and matching................................................................................................................43 Adding a gripper with dynamic control of a servo......................................................................... 46 A’mazing...................................................................................................................................... 48 Joystick ........................................................................................................................................ 51 Speech synthesis and recognition!.................................................................................................54 Useful features and functions........................................................................................................ 55 Concluding Remarks ....................................................................................................................58 Concluding Remarks ....................................................................................................................58 Appendix A – Function List.......................................................................................................... 60 Appendix B – File List..................................................................................................................63 Phil Page 2 06/04/2010 

3 .Controlling Robobuilder using L# Introduction This document is a tutorial guide on programming a Robobuilder(1) series humanoid robot using Lisp based dialect called LSharp.NET (2) (or L#). It will explain the basics of Lisp – although rather briefly – if the reader has no knowledge of Lisp you may well want to look at other books and websites that cover the Lisp language in more detail. This guide will then look at how to control the Robobuilder robot remotely from a PC using .NET. It will show how to control individual servos on the robot, and access its sensors such as the distance sensor and the accelerometer. It then looks at advanced control techniques such as dynamic control of a gripper, using the distance sensor for navigating a maze and accelerometer for auto-balance control. Later chapters deal with interfacing with Windows to provide simple graphics and different input methods such as joystick and speech libraries to control the robot. The final chapters cover a basic introduction to some AI techniques to improve the intelligence of your robot. This document has been based on use of LSharp.Net (L#), a dialect of Lisp built using .NET framework. Its big advantage is its ability to hook into .NET windows libraries including the standard ones such as “System.Windows.Forms” and also DirectX and Speech (.Net V3) and my own RobobuilderLib.dll for controlling the robot. All the files required to run the code in the document can be download from the internet and links can be found in Appendix B. A summary of the Lisp/L# functions is Appendix A Throughout the document are code examples and example sessions. Lisp/L# is an interactive and immediate language and the reader is encouraged to read and try the examples out at the same time. In the examples user input is colour blue and application program output coloured green. Note: (1) Robobuilder is trademark of Robobuilder co (see http://robobuilder.net for details) (2) LSharp.Net / L# are Copyright Rob Blackwell Phil Page 3 06/04/2010 

4 .Controlling Robobuilder using L# Lisp Lisp was invented by John McCarthy in 1960 - So this year is its 50th anniversary. If you’re not familiar with Lisp it stands for list processing - this particular lisp (L#) uses a dialect based on “arc” – a description can be found here: http://www.paulgraham.com/arc.html Here are a few simple examples to get you started. First run the command line environment and get a prompt: “>” C:\> LSharpConsole.exe L Sharp 2.0.0.0 on 2.0.50727.3603 Copyright © Rob Blackwell. All rights reserved. > (+ 2 3 4 5) 14 Assuming you have downloaded and run the console program you should be able to try out the above example. Lisp is a very simple language, it consists entirely of list of thing contained within brackets (). It evaluates the list supplied and returns a result. In the above example (+ 2 3 4 5) is a list that return the result 14. The console program evaluates the list by looking at the first ‘atom’ in the list (in this case +) and using that as a function to process the remaining elements in the list, in this case, summing the remaining elements. It would also correct to write the following (+ (+ 2 3) (+ 4 5)). Lisp would evaluate the inner elements first to give (+ 5 9) and the sum the outer elements giving 14. Fundamental things in LISP are atoms and atoms are formed into list by (). So (+ 1 2) represents a list of 3 atoms and if the list is evaluated the + is taken as function and 3 is the result. Here’s another list: > (= x 5) 5 > x 5 This shows how to assign a value to the atom “x”. If we type “x” to the prompt it evaluates to value of x which in this case is 5. We can now use x in an expression – here using “+” function which adds values: > (+ x x) 10 > x 5 Note although (+ x x) added the value of to itself it didn’t change its value. Atoms can also be made of ASCII characters - so (a b c) or (a b 1 3) are lists. I mentioned that Lisp is basically just atoms and list - or s-expressions as they are called. So what is a list and do we work on them? The simplest way is to do a few examples > ‘(a b c d) (a b c d) > ‘(1 2 3) (1 2 3) Note the quote (‘) - this is important! It means don’t evaluate the list - take its literal contents. Atoms can be numbers or symbols, and numbers can be integers or floating points (real numbers). Phil Page 4 06/04/2010 

5 .Controlling Robobuilder using L# And lists can be me a mixture of elements (and even other lists) > ‘(a b 1 2 4.0) (a b 1 2 4.0) > ‘( (a b) (c d)) ((a b) ( c d)) > (= x ‘(a b c)) (a b c) > x (a b c) But atoms can also be functions - which is where Lisp gets its power - it’s a user definable symbolic processing language that can be written in itself using just a few primitives So (+ 1 2) is just another list: > ‘(+ 2 3) (+ 2 3) > (= x ‘(+ 2 3)) (+ 2 3) > x (+ 2 3) To load files into Lisp/Lsharp you use the function load. So with you favourite text editor (vi or notepad) create a file “demo.lisp”. Add the following content to the file: (prn “Hello world”) Now start LsharpConsole.exe : > (Load “demo.lisp”) Hello world Note: it “runs” the file immediately displaying the result to the console. Phil Page 5 06/04/2010 

6 .Controlling Robobuilder using L# Car, Cdr, Cons, List Two assembly language routines for the IBM 704 became the primitive operations for decomposing lists: car (Contents of Address Register) and cdr (Contents of Decrement Register). Lisp dialects still use car and cdr (pronounced /’k?r/ and /’k?d?r/) for the operations that return the first item in a list and the rest of the list respectively. (from wikipedia) So how do these work?  car - a function that returns the first item in the list (some Lisp dialects call it first)  cdr - a function that return the rest of the list (some Lisp dialects call it last)  cons - construct a new list element  list – a function to create a new list from a list of elements > (= x ‘(1 2 3 4 3 2 1)) > ( car x) 1 > (cdr x) (2 3 4 3 2 1) These of course can be nested to access any element > (cdr (cdr ( cdr x))) (4 3 2 1) Also if you have an empty list () > (cdr ‘()) null This becomes important when you create recursion functions that strip off elements as they loop. car and cdr breaks lists apart - how do we stick them back together? This is where cons is used. > (cons ‘a ‘(1)) (a 1) > (cons ‘b x) (b 1 2 3 4 3 2 1) A more complex example is adding a list to another list > (cons ‘(a b) ‘(c d)) ((a b) c d) So what would I get if I took car of the newly constructed list?? > (car ‘((a b) c d)) (a b) A list is returned - the first element. Did you try to create like this? > (cons ‘a ‘b) Exception : Not a sequence Phil Page 6 06/04/2010 

7 .Controlling Robobuilder using L# This is because cons expects its second argument to be a list (or null). So to do the above you need > (cons ‘a (cons ‘b nil)) (a b) An alternative way to this is to use the list function as follows > (list 'a 'b) (a b) The list function builds list using cons for you – and so makes thinks a lot simpler ! Phil Page 7 06/04/2010 

8 .Controlling Robobuilder using L# Function and conditionals When a list is processed or evaluated the first atom is treated as a function. This function can be define by the user, here’s an example > (def hello () (prn “hello world”)) LSharp.Function > (hello) hello world “hello world” So what’s going on here? The first line defines a function called “hello” that takes no arguments (). The functions then calls prn to print with new line. Note how ‘def’ is a function that returns a type ‘Lsharp.Function’ I then invoke the function by using it in a list (hello). So that hello is evaluated. The output is “hello world”, and the return value from prn function “hello world” is also displayed. Here’s an example of a simple function with an argument: > (def addone (x) (+ x 1)) LSharp.Function > (addone 5) 6 > (addone (addone (addone 7))) 10 You might try and see what happens if you addone to a list. To make functions useful we need one more element, the conditional. In LISP this is normally called ‘cond’ however in this Lisp dialect it instead uses the more familiar (to non LISP people!) if function. if relies on the fact that the atom ‘t’ mean true and null is not true. So a few examples: > (if null ‘true ‘false) false > (if t ‘true ‘false) true > (if (> 3 5) ‘true ‘false) false To use an if statement you need a predicate - a function that return true (t) or false (null), such as “is x greater than y ?” which would be written (> x y), or “is x equal to y ?” written as (is x y). b]if[/b] statement can be compound so they are very similar to the old style LISP cond statements, i.e (if a b c d e) means if a then b elseif c then d else e. A few examples: > (= x 1) 1 > (if (is x 1) ‘one (is x 2) ‘two ‘no) one > (= x 2) 2 > (if (is x 1) ‘one (is x 2) ‘two ‘no) two > (= x 3) 3 > (if (is x 1) ‘one (is x 2) ‘two ‘no) no So how to create a function ‘spell’ that uses an ‘if’ statement and outputs any number up to 99 (without listing all 99 values)? We need to have multiple if statements: Phil Page 8 06/04/2010 

9 .Controlling Robobuilder using L# (def spell (x) (progn (if (> x 29) (progn (= x (- x 30)) (pr “thirty “))) (if (> x 19) (progn (= x (- x 20)) (pr “twenty “))) (if (is x 1) ‘one (is x 2) ‘two (is x 3) ‘three (is x 4) ‘four (is x 5) ‘five (is x 6) ‘six ‘no ))) LSharp.Function > (spell 1) one > (spell 2) two > (spell 21) twenty one > (spell 24) twenty four > (spell 20) twenty no > Important: look how to chain instructions you need a function - called progn. You can’t simply go ((pr “hello”) (pr “world”)) - can you see why ? The first element of the list isn’t a function - its a list ! So it errors. The correct way to write this is (progn (pr “hello”) (pr “world”)). Also: “twenty no” ??? - Well it’s almost there - can you think how to improve this? Phil Page 9 06/04/2010 

10 .Controlling Robobuilder using L# Controlling the Robot So what is the process for controlling Robobuilder from within L#. Here’s a diagram outline the basic approach. LISP / L# Code “example.lisp” Bluetooth / (connect “COM1”) or direct link LSharpConsole.Exe Serial connection RBC LSharp.dll 115200, 8, N, 1 RobobuilderLib.dll .NET CLR servos Robobuilder RBC = RoboBuilderController Windows PC Running XP or above Firmware: 2.23 or above .NET 3.5 loaded (this could be a Home PC/Desktop start or a miniature PC such Roboard mounted on back of the robot) PC Remote DC mode Figure 1 The LISP program or functions (in yellow) are defined in a file and loaded and run within the console application. This enables Lisp programs to access .NET dynamic libraries such as the built in .NET functions – such as System.Console, or Windows libraries – see later section, or the Robot API library – RobobuilderLib which will see more of later. The communication with the Robot is via a serial link, this can be established using the .NET object System.IO.Port.Serial. The link can be a direct physical link or if the optional module has been added to the RBC via Bluetooth (BT). The Serial object will take care of the physical device interface and all the application needs is the name of the COM port (real or virtual) that connects to the robot. Once connected, binary commands to the robot firmware make it possible to control either high level functions in the default PC remote mode – such as performing a built-in motion, or play a built in sound. There is also Direct Control (DC) mode that allows access directly to individual servos, so that they can be configured or controlled. Switching between modes is controlled again by a binary sequence. The RobobuilderLib provides and SDK so that the application program does not need to know the sequences to use the firmwares features. Phil Page 10 06/04/2010 

11 .Controlling Robobuilder using L# L# provides simple access to windows functions. Here’s an alternative way of creating your own prn function, using the built in system console class. > (Console.WriteLine “helloworld”) Helloworld To get the application to load the dynamic library (or assembly) Lisp has a function reference. So to access the serial port – which isn’t a built in core function – we first load it and then we can Now to create a function to connect to the robot !! (reference “System.IO.Ports”) (using “System.IO.Ports”) (def connect () (= sport (new “SerialPort”)) ; create new instance of serial port (.set_BaudRate sport 115200) ; set its baud rate (.set_PortName sport “COM3”) ; specify the COM port ) The function connect creates a new instance of a SerialPort (= sport (new “SerialPort”)) and assigns (“=” function) to the variable ‘sport’. In L# you can access .NET propert using .set_xxxx to access property xxxx on the object instance. The above example sets the baud rate to 115200 and port to COM3. At this point the serial port is not connected to the robot. To do that there is a method (or function) called Open (and likewise Close to disconnect). In L# to call the method you specify its name, preceded with a ‘.’ and provide the object instance as an argument. Here is an example session: > LSharp.Function > (connect) null > (.open sport) null > (.close sport) null Using the serial connection – we can pass it to the robobuilder lib to issue commands to the robot, for example for reading the firmware and serial number. First step is to load the library (as we did with the Serial Port), using reference. If the dll is not in the same folder as the Console you need to provide the full path RobobuilderLib. (reference “RobobuilderLib”) (using “System.IO.Ports”) (def connect (pn) (prn “connecting to “ pn) (= sport (new “SerialPort”)) (.set_BaudRate sport 115200) (.set_PortName sport pn) (= pcr (new “RobobuilderLib.PCremote” sport)) ) This creates a serial port object (sport) and also creates a PCremote object (pcr) that takes the serial port as a parameter, to enable it to do the communication with the robot. The serial port is still not open at this point. But everything is now set up. connect is defined as a function that takes the serial port as an argument - such as “COM1”. Phil Page 11 06/04/2010 

12 .Controlling Robobuilder using L# To make this easier to use and read, the code can be built into a function that’s lets the user select the port from the keyboard. Here it is: (def askPort () (with (k 0 p 0 y 0) (prn “Available Ports:”) (= p (System.IO.Ports.SerialPort.GetPortNames)) (each y p (prn y)) (prn “Select:”) (while (is (= k (Console.ReadLine)) “” ) (pr “: “)) (if (not (member? k p)) (do (prn “Invalid port?”) (askPort)) k))) The function askPort will prompt the user, and get a list of possible ports using the .NET function into a local atom / variable ‘p. This will contain a list i.e. “COM1” “COM2” etc. Using the each iterator function, its displays each element of the list returned. Finally it asks the user for input. If the input matches (is a member of the list returned) it exists with value of the data entered ‘k. Otherwise its displays an error message and re-enters (a recursive call) askPort - so the user can try again. So a lot of code and a few elements - well it is Sunday! We’ve got a connection using PCremote, so lets get data from the robot to prove its all working!. The simplest is the serial number. Using the readSN() method the function getsn opens the serial port - reads the serial number and then closes. sn is the return value of the function, it should really be a local variable. Note: If you are not connected at this point the system will hang - it should time out - but it’s not at the moment. So make sure your serial cable is connected and the robot is on. (def getsn () (do (.open sport) (= sn (.readSN pcr)) (.close sport) sn) ) Finally we now have the top level function that brings it all together: (def run_robobuilder() (connect (askPort)) (getsn) ) Just type (run_robobuilder). You should see something like this: > (run_robobuilder) Available Ports: COM1 COM3 COM9 Select: : COM3 connecting to COM3 “1041100010***” > BTW the * are there to obscure my serial number - it’s actually just 13 numbers. Phil Page 12 06/04/2010 

13 .Controlling Robobuilder using L# More PC Remote mode functions Download into the same directory as the LSharpConsole.exe. Now to use this just start Lsharp L Sharp 2.0.0.0 on 2.0.50727.3603 Copyright © Rob Blackwell. All rights reserved. > (load “Final.lisp”) ................ OK > (run_robobuilder) Available Ports: COM1 COM3 COM9 Select: : COM3 connecting to COM3 Good Firmware loaded (2.26) Serial Number = 1041100010*** Distance = 10 cm ok So how does this work. You’ve seen the key functions - but I’ve added a few more including a top level function run_robobuilder. Lets see this: (def run_robobuilder() (connect (askPort)) (MessageBox.Show “make sure robot is connected to serial port and on” “warning” ) (.open sport) (checkver) (prn “Serial Number = “ (getsn)) (prn “Distance = “ (readdistance) “ cm”) (.close sport) ‘ok ) It starts by setting up the connection to a serial port as we have already seen. It then pops up a message box to warn you to connect and switch on the robot. It then opens the serial connections (.open sport) and then calls a function to check the firmware version you are running. (def checkver () (if (not (.Isopen sport)) (.open sport)) (= v (.readVer pcr) ) (if (< v 2.23) (prn “Download new firmware”) (prn “Good Firmware loaded (“ v “)” ) ) V ) This routine get the firmware via PCremote using this (= v (.readVer pcr) ) storing the resultant string in a variable v. Note although v is a string it can still be used for a numeric test as L# converts seamlessly between string and number. Phil Page 13 06/04/2010 

14 .Controlling Robobuilder using L# RBC (COM dll wrapper) Open Close IsOpen SerialPort ReadByte WriteByte (System.Port.IO) readVer readSN readXYZ PlayFile (** new in v1.7) readDistance set_trigger (** new in v1.7) readPSD availMem PlayPose resetMem servoID_readservos readZeros Delay_ms zeroHuno PCRemote ------------------ setDCmode wckPassive readIR (RobobuilderLib) wkcReadPos readButton wckMovePos readSoundLevel syncPosSend executionStatus wckBreak() runMotion wckSetOper(byte d1,byte d2, byte d3, byte d4) runSound wckSetBaudRate(int baudrate, int id) -------------------- wckSetSpeed(int id, int speed, int acceleration) Command_1B wckSetPDgain(int id, int pGain, int dGain) Command_nB wckSetID(int id, int new_id) displayResponse wckSetIgain(int id, int iGain) wckMotion wckSetPDgainRT(int id, int pGain, int dGain) (RobobuilderLib) wckSetIgainRT(int id, int iGain) wckSetSpeedRT(int id, int speed, int acceleration) wckSetOverload(int id, int overT) wckSetBoundary(int id, int UBound, int LBound) wckWriteIO(int id,ch0,ch1 ) set_PSD(min.max) wckReadPDgain(int id) set_accel wckReadIgain(int id) set_timer (int ms) trigger wckReadSpeed(int id) set_trigger wckReadOverload(int id) print() (RobobuilderLib) wckReadBoundary(int id) activate() wckReadIO(int id) active() wckReadMotionData(int id) wckPosRead10Bit(int id) wckWriteMotionData(int id, int pos, int torq) wckPosMove10Bit(int id, int pos, int torq) Figure 2- PC Remote and wckMotion schema Some of the PC remote functions do rely on recent firmware I suggest 2.23 or better go to the http://Robobuilder.net/eng site to download the latest. The program then gets and displays (using the print (pr) command) the serial number and also now the distance. If you don’t have a distance sensor it shows 10 (cm). Final.lisp also includes runMotion and runSound. More on these tomorrow - but for now assuming your robot is still on and connected > (runMotion 7) This will make robot take up basic pose. Phil Page 14 06/04/2010 

15 .Controlling Robobuilder using L# Menu driven motions If you have it the code running and connected to the robot you can now try using the built in motions, As mentioned yesterday to play a motion use the function (runMotion n) where n is a number. The numbers are defined in the RBC_over_serial_protocol_1.13.pdf - see http://robosavvy.com/forum/viewtopic.php?t=3503 I’ve added the motions specified into a list along with a description. (= menu ‘( 1 “GET UP A” 2 “GET UP B” 3 “TURN LEFT” 4 “MOVE FORWARD” 5 “TURN RIGHT” 6 “MOVE LEFT” 7 “BASIC POSTURE” 8 “MOVE RIGHT” 9 “ATTACK LEFT” 10 “MOVE BACKWARDS” 11 “ATTACK RIGHT” 0 “EXIT”)) As you can see I’ve called the list ‘menu’ which should give a good clue what’s coming next. By modifying the code that is used to select the COM port I have a function that will display the menu and request input. (def ask (prompt error menu) (with (k 0 ) (prn prompt) (each x (pair menu) (prn (car x) “ - “ (cdr x))) (while (is (= k (Console.ReadLine)) “” ) (pr “: “)) (= k (coerce k “Int32”)) (if (not (member? k menu)) (do (prn error) (ask menu)) ) k ) ) How does this work? The menu is a list of items ‘(1 a 2 b) etc.. The function [b]pair[/b] converts this into another list, of pairs of items, i.e. (pair ‘(1 a 2 b) ) result is ‘((1 a) (2 b)). This list is then used by the each function so that x is first set to ‘(1 a) and then ‘(2 b) etc. The print function prn then prints the first element of the value of x using car. Since (car ‘(1 a)) is ‘1. and then “ - “ and then the rest of the list using cdr, i.e. (cdr ‘(1 a)) would be ‘a. It then goes on to treat each pair in the same way. This results in the menu being printed. The function then requests input and looks if that matches an member of ‘menu list. If it does it converts the text input to a number (using the coerce function) which becomes the return value of the function. Notice if I look in the list for “1” it doesn’t match with a ‘1 because one is string and the other Int32. A simple while loop looking for the user to press zero to exit or any number to call the motion can then be created as follows ;; loop until 0 selected (def domenu () (with (item 1) (while (not (is item 0)) (= item (ask “Choice :” “No such option” menu)) Phil Page 15 06/04/2010 

16 .Controlling Robobuilder using L# (if (> item 0) (runMotion item)) ) ) ) See how its calls the (ask) function passing in the prompt string and menu as a list - this could be used as generic function for any picklist. The value returned by ask is stored in ‘item and then passed to runMotion. This could equally be used to playSound in the same way. Assuming you have loaded Day7.lisp and (run_robobuilder), now enter (domenu) and you can select and play any of the built-in motions on robobuilder. i.e. L Sharp 2.0.0.0 on 2.0.50727.3603 Copyright © Rob Blackwell. All rights reserved. > (load “Day7.lisp”) > (run_robobuilder) > (domenu) Isn’t Lisp/L# lovely and compact! Phil Page 16 06/04/2010 

17 .Controlling Robobuilder using L# Accessing the accelerometer. One of the optional sensors that can be purchased with Robobuilder is a 3-axis accelerometer (Robosavvys’ Xmas edition bundles it along with the distance sensor). The sensor is wired into the RBC control box. The values of the accelerometer can be accessed using the PCremote library via a method readXYZ. The values can then be stored into a symbol xyz using (= xyz (.readXYZ pcr)) You must first have loaded Day7.lisp and (run_robobuilder). The symbol ‘xyz will contain a list of 3 numbers i.e. ‘(4 3 -6) representing the X, Y and Z values. To access X you use car i.e. (car xyz) which gives 4 in this example. By chaining the car with cdr you can access the other elements i.e. y is (car (cdr xyz)) and z would be (car (cdr (cdr xyz))). In LSharp an alternative way is to use the nth function which lets you access elements of a list directly. So to get x would be (nth xyz 0) and y would be (nth xyz 1). Notice the first element is index zero. L Sharp 2.0.0.0 on 2.0.50727.3603 Copyright © Rob Blackwell. All rights reserved. > (load “Final.lisp”) ................ OK > (run_robobuilder) Available Ports: COM1 COM3 Select: : COM3 connecting to COM3 Latest Firmware loaded (2.26) Serial Number = 1041100010*** Distance = 10 cm ok > (.open sport) > (= xyz (.readXYZ pcr)) System.Int32[] > (= xyz (tolist (.readXYZ pcr))) (6 -15 65) > (= xyz (.readXYZ pcr)) System.Int32[] > (nth xyz 1) 17 Notice how the data returned by PCremote is actually an array of int32s. They can be converted into a list or use directly. nth works equally well on either. It also works on strings - so (nth “hello” 3) is character “l”. Of course this has only read one value - to sample the data its needs a read in a loop with a delay, using another command sleep where (sleep x) to pause the program for x seconds. Here is a function readSensors that loops for 10 times and every 0.5 secs reads both XYZ values and the distance sensor and outputs the result. (def readSensors () (if (not (.isopen sport)) (.open sport)) ;; ensure com port open (do (prn “i X Y Z Distance”) (for i 0 10 (do (= xyz (.readXYZ pcr)) (prn i “ “ (nth xyz 0) “,” (nth xyz 1) “,” (nth xyz 2) “ : “ (.readdistance pcr)) (sleep 0.5)) ) ) ) Phil Page 17 06/04/2010 

18 .Controlling Robobuilder using L# > (readSensors) i X Y Z Distance 0 2,-17,61 : 10 1 1,-17,64 : 10 2 -1,-20,60 : 10 3 3,-14,64 : 10 4 -3,-1,63 : 10 5 -1,25,53 : 10 6 0,32,56 : 10 7 1,30,50 : 10 8 -7,10,62 : 10 9 -5,-15,63 : 10 10 -1,-15,65 : 10 null In this demo - the distance (PSD) sensor is not connected - hence the function always returns 10 back. Using this, the data can be stored, or an application could check the data and if the XYZ values falls outside a certain range invoke a motion to balance the robobuilder dynamically. Of course stock motions won’t be sufficient we need direct control of the servos .... Phil Page 18 06/04/2010 

19 .Controlling Robobuilder using L# Access the servo directly To read and control the wck servos requires switch into Direct Control mode on Robobuilder. A PC Remote binary sequence switches into this mode and then subsequent binary command are sent directly onto the wck servo bus. This means every aspect of a servo can be controlled including setting boundary conditions, PID values, even the servos ID and baudrate. The individual positions of servos can be read or can be moved to specified positions. The servos can be put into continuous rotation mode or switched to passive input mode. Using these commands each individual servo can be addressed and controlled using a unique ID between 0-31, or with the synchronous move they can all be commanded at once. To make this easy within the windows environment is the wcKMotion class. This class can be accessed like any other .NET class and is part of the RobobuilderLib.dll. The class takes as a parameter of its constructor the PCremote class used in previous examples. In the same way PCremote required SerialPort to be passed to it, wcKMotion requires an instance of PCremote. This is the code to do the set up. (def dcmodeOn () (if (not (.isopen sport)) (.open sport)) (= wck (new “RobobuilderLib.wckMotion” pcr))) ) The code first checks the serial port isOpen - and if not opens it. This is needed as the wckMotion issues the control codes to switch on DC mode when created. When DCmode is on you cannot issue PCremote commands - as they will be passed to the servos and treated as gibberish. The PCremote/wckMotion classes should protect you from this in that it will auto switch - and turn DCmode off. You can tell when the RBC is in DC mode as the amber light will come on on the RBC unit - the box at the back of the robot. Once in DCmode it’s simple to access a servos positions using wckReadPos method. (def getServoPos (n) (.wckReadPos wck n) (cadr (.respnse wck)) ;; could use nth here! ) The wckReadPos method returns a Boolean - true if successful and puts the returned data into a two element array called respnse. The first element is the load (generally this zero) and second element is the position. Hence the code used car and cdr to return the position of second element. So to read the position of servo id 5, all is now required is the command (getServoPos 5). The servo can also be put into passive mode - so that it can be moved into different positions: (def setPassive (n) (.wckPassive wck n) ) Phil Page 19 06/04/2010 

20 .Controlling Robobuilder using L# This can now be combined with a getServoPos to read the servos as they are moved - the start of creating a capture and play capability. Here’s an example session where I set servo 12 (left arm at elbow) to passive and then read 20 times - every ½ second - the values off the servo as I move it. L Sharp 2.0.0.0 on 2.0.50727.3603 Copyright © Rob Blackwell. All rights reserved. > (load “Day7.lisp”) ................ OK > (run_robobuilder) Available Ports: COM1 COM3 Select: : COM3 connecting to COM3 Latest Firmware loaded (2.26) Serial Number = 1041100010*** Distance = 10 cm ok > (def dcmodeOn () (if (not (.isopen sport)) (.open sport)) (= wck (new “RobobuilderLib.wckMotion” pcr)) ) LSharp.Function > (def dcmodeOff () (.Close wck) ) LSharp.Function > (def setPassive (n) (.wckPassive wck n)) LSharp.Function > (def getServoPos (n) (.wckReadPos wck n) (cadr (.respnse wck))) LSharp.Function > (dcmodeOn) RobobuilderLib.wckMotion > (setPassive 12) True > (for i 0 20 (do (prn (getServoPos 12)) (sleep 0.5))) 100 99 100 99 45 114 ... etc Phil Page 20 06/04/2010 

21 .Controlling Robobuilder using L# How to play servo moves The method to move a servo to a specified position is wckMovePos. This takes three parameters, the id of the servo the position of the servo on the torq setting. These values are explained in the wckProgramming guide. Here’s a Lisp function that the calls the method assuming that wck has been created using dcmodeOn as explained yesterday. (def setServoPos (id pos torq) (.wckMovePos wck id pos torq) (cadr (.respnse wck)) ) So to use this simply enter (setServoPos 12 120 2) and the servo will move. We could some extra tests to ensure the parameters are with in bounds. Also notice the return value is in the repnse data. This reports the position as the command is received by the servo. Repeated reads will then update the actual position as the servo attempts to move to its requested position. Using the code yesterday its straightforward to create a function to read and capture the data from the getServoPos function as follows: > (setServoPos 12 60 2)(capture 12 5 0.05) 20 20 28 43 57 60 60 (= data ()) (def capture (id n t) “capture n points every t secs” (for i 1 n (do (= data (cons (prn (getServoPos id)) data) ) (sleep t) ) ) “Captured”) Notice how capture stores the servo position in a list called data. Each new element is added using cons function however this then means the last element is at the front and the first at the back of the list. Now we have a list of positions we can play them back to the servo using the setServoPos function just created. First we need to use reverse function to convert the list into the order needed and then we play it back. (def play (id n t) (for i 0 (- n 1) (do (pr “.”) (setServoPos id (nth data i) 2) (sleep t)) ) “Complete” ) Phil Page 21 06/04/2010 

22 .Controlling Robobuilder using L# We could slow or speed up the motion using a different t parameter, or even play it back via a different servo - so get the left arm to mirror the right arm for instance. Here’s an example session: > (setPassive 12) True > (capture 12 10 0.5) 45 45 45 73 61 46 29 38 62 62 “Captured” > (= data (reverse data)) (45 45 45 73 61 46 29 38 62 62) > (play 12 10 0.5) ..........”Complete” This is all works well for one servo (and could be extended) but to move all the servo at the same time - for a complex motion requires a separate function. Phil Page 22 06/04/2010 

23 .Controlling Robobuilder using L# Synchronous moves The method to send an array of servo positions to move synchronously is called .SyncPosSend. A lisp function provides an easy to use wrapper around wckMotion class. The code assumes were using the global wck which will need to be initialised by dcmodeOn before this function can be called. (def setSyncMove (lastid torq position) (.SyncPosSend wck lastid torq position 0)) So to use this code it needs three parameters - the last id in the list - so for16 servo (0-15) last id would be 15. The torq is the speed it moves; a value between 0-2; 0 is the fastest. Finally it expects an array of at least last id +1 values in an array. So to create an array of positions for basic pose for instance would look like this. (= basic ‘(143 179 198 83 106 106 69 48 167 141 47 47 49 199 204 204)) (setSyncMove 15 2 (toarray basic)) Caution: this will immediately move to basic pose, very rapidly. Notice the toarray function, this is required to convert a list into an array object[] so it can be passed into the wck method. Now what is really needed is to move there slowly in gradual steps, from where we are now, to where we want to be, in this case the basic pose. To achieve this we must first create an array of our current position (def getallServos(n) (with (current () ) (for i 0 n (= current (cons (getServoPos (- n i)) current)) ))) This function is a simple iterative loop reading each element into Assuming Day7.lisp is loaded and (run_robobuilder) and (dcmodeOn). See how it uses cons to construct the output list. > (getServoPos 2) 248 > (getServoPos 1) 184 > (getServoPos 0) 146 > (cons (getServoPos 0) (cons (getServoPos 1) (cons (getServoPos 2) ()))) (146 184 248) ;;or all 15 in one go ! (= cur (getallServos 15)) (146 184 248 46 109 102 60 4 205 142 23 47 62 228 204 204) So we have two lists, current position cur - and our final potion basic. We need to calculate the in between points. So for example if servo is at position 20 and it moves to 60 in 10 steps, then each step is (60-20)/10 or 4. Id we can then move the servo in the following way 20 24 28 32 36 40 ... we will have a smooth move. You could off course use non-linear algorithm here such as a sinusoidal that make the movement greatest in the middle part of the move. Phil Page 23 06/04/2010 

24 .Controlling Robobuilder using L# Smooth moves and the In-betweens To generate a smooth move between to sets of servo positions it is necessary to calculate the difference or distance between the two lists. Lets take a simpler example of just three servos. The current position is represented by the list ‘(6 3 2) and the final position of the servos the list ‘(3 1 1). To evenly step between the two lists requires each servo to move by a different amount - an interval. To move in 5 equal steps would require a list formed from (6-3)/5, (3-1)/5, (2-1)/5. Heres the code to create that list: (def md (xs ys step) “calc distance between two list” (= list ()) (for i 0 (- (len xs) 1) (= t (/ (- (nth xs i) (nth ys i)) step)) (= list (cons t list)) ) (reverse list) ) So the command (md ‘(6 3 2) ‘(3 1 1) 5.0) will result in an output (0.6 0.4 0.2). Notice the step must be a floating point (or decimal) number. If it’s a whole or integer number it will get the wrong result. 6/3 = 0 in integers, but 6/3 =0.5 in decimal. Lisp will convert integers to decimals if part of the calculation is decimal, but if all parts are integers the result will be integer. See how list is built by successive cons commands as in getAllServos from yesterday. We can use the interval list created to calculate the in between moves by adding it successively to the start list. Here the complete function: (def smove (a b n) “smooth move position a to b in n steps” (= interval (md a b (* n 1.0))) ;; calculate distance (= l (len a)) (for i 0 n (= list ()) (for j 0 (- l 1) (= t (- (nth a j) (* (nth interval j) i)) ) (= list (cons t list)) ) (prn “(setSyncMove “ l “ “ 2 “ ‘(“ (reverse list) “))”) ;(setSyncMove l 2 (reverse list)) (sleep 0.1) ) ) Here is an example of it running.. > (smove ‘(6 3 2) ‘(3 1 1) 5) (setSyncMove 3 2 ‘(6 3 2) ) (setSyncMove 3 2 ‘(5.4 2.6 1.8)) (setSyncMove 3 2 ‘(4.8 2.2 1.6)) (setSyncMove 3 2 ‘(4.2 1.8 1.4)) (setSyncMove 3 2 ‘(3.6 1.4 1.2)) (setSyncMove 3 2 ‘(3 1 1) ) If the output looks correct then un-comment the SetSyncMove line (just remove the ;) and you’re ready for smooth move! To get the bot to smoothly take up basic position, apply the command (smove cur basic 10). This assumes you have followed yesterdays post and set up cur to be a list of the current servo positions and basic is the predefined basic pose list. If this is working you can now create your own motions and poses. The sleep will control the speed of the move. It set at 0.1s so a 10 step interval will take 1s to complete. This could also be passed as a parameter to the function. Phil Page 24 06/04/2010 

25 .Controlling Robobuilder using L# Making complete motions Yesterdays post showed how to move between to points A and B using a function to divide into equal step the movement of each servo and then play that out with a specific delay. To create complex motions all is that is now required is to create a list of points that make the motion up, with addition of how long and how many steps to take. If we look at the example of “Punch Left” this can be easily encoded as follows: (= initpos ‘( 125 179 199 88 108 126 72 49 163 141 51 47 49 199 205 205 )) (= Data0 ‘( 125 179 199 88 108 126 72 49 163 141 51 47 49 199 205 205 )) (= Scene1 ‘( 107 164 233 106 95 108 80 29 155 129 56 62 40 166 206 208 )) (= Scene2 ‘( 107 164 233 106 95 145 74 40 163 154 117 124 114 166 206 208 )) (= Scene4 ‘( 126 164 222 100 107 125 80 29 155 142 79 44 40 166 206 208 )) (= punchleft (list initpos (list 1 70 Data0) (list 8 310 Scene1) (list 1 420 Scene2) (list 5 200 Scene4) (list 15 300 Data0))) The individual points are specified as lists - in Robobuilder speak each is a “scene”. A motion such as punchleft is created by stringing together a number of scenes, in this example called Data0, Scene1 etc. These can be created using MotionBuilder software that comes with the bot. The extra two number 1 70 for example specify the number of in between step (or frames in Robobuilder speak) and the total time take. So for 1 frame in 70ms that 70ms, if it were 2 frames that would be 35ms per frame. The following function playmotion will take the list ‘punchleft and play it in real-time as if it had been upload. It uses a recursive subroutine called play1 which works by playing the top potion on its list and then calling itself with the remaining list until the playlist is empty. (def playmotion (c x) “Play a motion list” (= ip (car x)) ;; move from current pos to ip (prn “Do initial move”) (prl ip) ; move to initial position smoothly .. (smove c ip 10 0.05) ;;now loop (play1 ip (cdr x)) ) (def play1 (c x) “used by play move c -> x” (if (or x) ( do ;move through list (prn “Do next move”) (= cur (car x)) (= nof (car cur)) (= trt (/ (cadr cur) nof)) (= gt (car (cdr (cdr cur)))) (smove c gt nof (/ trt 1000.0) ) ;smooth move from c to gt ;recurse (play1 gt (cdr x)) ) ;else we’ve finished (prn “Done”) )) Phil Page 25 06/04/2010 

26 .Controlling Robobuilder using L# So to use this function we must first load and run final.lisp and run_robobuilder to connect the serial port. This also needs the other functions covered in the last chapters. (load “wckutils18.lisp”) (demo) This is great, but having the servo data stores as code is not easy to manage, what would make things easier is if they can be loaded from a file. Phil Page 26 06/04/2010 

27 .Controlling Robobuilder using L# Loading data from CSV files If we want to manage a lot of motion data - the best way to access would be if the positions were stored in a file. Robobuilder use a file form called rbm, but here I've used Comma separated values (CSV). The big plus is that you can then manipulate the data easily in Excel or Openoffice calc. There are many ways to read a CSV files using .NET libraries, the method used here uses a simple character based parsing looking for commas and then assumes the cell contains a number. So first we need to create a CSV file using a favourite text editor (or spreadsheet such OpenOffice Calc). Here is an example using the "punch left" data from previous section. Example test.csv #comments 0,1,125,179,199,88,108,126,72,49,163,141,51,47,49,199,205,205 70,1,125,179,199,88,108,126,72,49,163,141,51,47,49,199,205,205 310,8,107,164,233,106,95,108,80,29,155,129,56,62,40,166,206,208 420,1,107,164,233,106,95,145,74,40,163,154,117,124,114,166,206,208 200,5,126,164,222,100,107,125,80,29,155,142,79,44,40,166,206,208 300,15,125,179,199,88,108,126,72,49,163,141,51,47,49,199,205,205 The format of the CSV file is very straightforward, any line that starts with a ‘#’ is trated as a comment and ignored. The first column is the duration the motion should take and the second column is the number of intermediate steps. So 10 step in 250 ms represents each step taking 25ms. Each row is a motion that starts from where the last one finished. The first row uses the current position as its starting point (by doing a read servo position). The remain columns represent the servo position for each corresponding servo starting with servo ID 0 in column 2 (or C). To read this data requires two functions. The first readcsv takes a filename as a parameter and then uses the .NET function File.ReadAllLines to create a string array, one line per record. So f will be a list containing '("line1" "line2" "line3") for example. Using for the each command then sets line to each row intern. If the line starts with a # the line is treated as a comment and ignored, in the example above the #comment line is ignored. The line is passed to the second function splitline that separates the entries using the comma into a list of numeric fields. splitline also separates out the first two entries so that it follows the format used yesterday. It also defaults any empty cells to zero. (def readcsv (filename) (with (f "" z ()) (= f (System.IO.File.ReadAllLines filename)) (each line f (if (not (is #\# (car line)) ) (do (= z (cons (splitline line) z))) ) ) (reverse z) ) ) def splitline (text) (with (l "0" z () r 0) (each a (toarray text ) (= a (str a)) (if (is a ",") (do (= z (cons (coerce l "Int32") z)) (= l "0") ) ;else (not (is a " ")) (= l (+ l a)) Phil Page 27 06/04/2010 

28 .Controlling Robobuilder using L# ) ) (= z (cons (coerce l "Int32") z)) ;; last arg (= r (reverse z)) (cons (cadr r) (cons (car r) (list (cdr (cdr r))))) )) Here is an example of it in action. > (= data (readcsv "test.csv")) ((1 0 (125 179 199 88 108 126 72 49 163 141 51 47 49 199 205 205)) (1 70 (125 179 199 88 108 126 72 49 163 141 51 47 49 199 205 205)) (310 8 (107 164 233 106 95 108 80 29 155 129 56 62 40 166 206 208)) (1 420 (107 164 233 106 95 145 74 40 1 63 154 117 124 114 166 206 208)) (5 200 (126 164 222 100 107 125 80 29 155 142 79 44 40 166 206 208)) (15 300 (125 179 1 99 88 108 126 72 49 163 141 51 47 49 199 205 205))) The first entry needs the two zero stripped out and this is achieved using a combination of : (car (cdr ( cdr (car data)))). The result can then be passed to the playmotion as outlined yesterday by constructing a new list called punchleft (= punchleft (cons (car (cdr ( cdr (car data)))) (cdr data))) (playmotion cur punchleft) So using L# it is possible to create pre-programmed motions - stored in CSV file format and then loaded on demand and played in real time. L# is not only capable of text and list processing but through use of .NET functions can manipulate graphics and windows to enable real time display of the robot status. To simplify the work need to play a CSV file, there is (since V1.7 of RobobuilderLib) a built-in function PlayFile that will take read a file sending each line to the Robot and calculating (or interpolating) the in-between positions. In addition there is a trigger mechanism that enable the application to set up pre-configured limits which when reach cause the motion to stop. In the following example firstly the file test.csv is played (assuming wck has been previously created as an instance of a wckMotion.). Then it creates a trigger that is set to stop the motion if the value of the PSD sensor is outside of the range 30-60 cm. The PSD sensor is read every 260 ms. The trigger is set against the instance of wckMotion object but only affects the motion if activated. Here’s example code fragment. The first line plays the file without a trigger set. Then the code creates a trigger object for the distance sensor to fire when outside the range 30-60cm. It the set the trigger against the wck object (already assumed to be created) and then activates the trigger and replays the file. (.Playfile wck "test.csv") (= trg (new "RobobuilderLib.trigger")) (.set_PSD trg 30 60) (.set_timer trg 250) (.print trg) (.set_trigger wck trg) (.activate trg true) (.Playfile wck "test.csv") Other triggers will be able to be set, but currently it only supports PSD sensor and the accelerometer, although the object has been designed to cover the IR sensor and Sound sensor. Phil Page 28 06/04/2010 

29 .Controlling Robobuilder using L# Controlling the IO on a servo Robobuilder servos come in to two forms, black and transparent with colour LEDs. Using the RobobuilderLib library you can directly control the LEDs. If you have the black form – you can also use the as readable input ports – need to read the wck manual for details. To support writing to LEDs the library provides the method wckWriteIO. To use from within L# the code is as follows: (load "final.lisp") ; load PC remote routines (load "wckutils18.lisp") ; load wck mode routines (def wckwriteIO (n c0 c1) "turn IO on/off" (if (not (bound 'wck)) (dcmodeOn)) (.wckWriteIO wck i c0 c1) ) (def setallLeds (n c0 c1) ; "turn on/off all servo leds" (for i 0 n (wckwriteIO i c0 c1) ) ) (run_robobuilder) ; initialise connection to robot (dcmodeOn) ; switch into DC mode (wckWriteIO 10 true true) ; set both LED on servo 10 on (setallLeds 16 true false) ; set all C1 high = red (sleep 1) ; wait 1s (setallLeds 16 false true) ; set all C2 high = blue (dcmodeOff) ; exit DC mode Phil Page 29 06/04/2010