Program / Control your Robobuilder robot with LISP !

[l3v3rz]

Here's a Christmas treat A kind of Advent calendar - a new code snippet every day !!

Index
======
Day1 Introduction to L#
day2 Loading and lists
Day3 List basics CAR, CDR, CONS
Day4 Function and conditionals
Day5 Access NET / Windows libraries
Day6 Connecting to the serial port
Day7 Reading data from Robobuilder (Day7.lisp)
Day8 Motion and Menus
Day9 Accelerometer
Day10 Read Servo positions
Day11 Set Servo positions
Day12 Synchronous moves
Day13 Smooth moves
Day14 Making motions move (wckutils.lisp)
Day15 Load CSV files
Day16 Windows graphics
Day17 Plotting and animation
Day18 Realtime accelerometer display
Day19 property list
Day20 Blockworld/SHRDLU
Day21 Doctor, Doctor
Day22 useful functions
Day23 Joystick routines
Day24 Final Post

Day1 Introduction to L#
==================
I've been playing with controlling my robot with L# a dialect of Lisp built in .NET. Its big advantage is it can hook into any windows library such as my RobobuilderLib.dll for controlling your robot.

If you're interested in trying it out (and following along) I've built L# and added it into a zip with my library (one small 30K download).

http://robobuildervc.googlecode.com/files/LSharp%20with%20RBLib.zip
(should be 3 files; LSharpConsole.exe, LSharp.dll, RobobuilderLib.dll)

If you want you can build and download the L# yourself from LSharp.org - there is an svn to extract latest version (v2).

If you're not familiar with Lisp it stands for list processing - this particular lisp uses a dialect based on arc http://www.paulgraham.com/arc.html

A few simple examples:

Code: Select all

First run the command line environment and get a prompt:
LSharpConsole.exe
> (+ 2 3 4 5) 
14
> (= x 5)
5
> (= y 7)
7
> (* x y)
35


The code should work in Linux/Ubuntu using Mono without change. Just put Mono in front of the shell.

[Edit: I have LSharpConsole.exe working on Mono - running on a fresh install of Ubuntu 10.4 beta 1 - but it did require (minor) changes to get working - contact me if you want details]


I'll post how to access and run the RobobuilderLib, read firmware version read the accelerometer (if you have one), and read and control servo positions, and make complex motions, all in a Lsharp using the standard software on your robot. in addition to RobobuilderLib.dll library of course.

Final example: - try this (> is the prompt!)

Code: Select all

> (reference "System.Windows.Forms")
> (using "System.Windows.Forms")
> (MessageBox.Show  "Loaded?" "RoboBuilder is Go!")
OK


Next post : loading files, and some useful functions ...

[l3v3rz]

Day2 Loading and lists
==================

A little history - Lisp was invented by John McCarthy in 1960 - So next year is its 50th anniversary.

To load files into Lisp/Lsharp you use the command Load. So with you favorite text editor (vi or notepad) create a file "demo.lisp".

Content of file:

(prn "Hello world")


Now start LsharpConsole.exe

Code: Select all

> (Load "demo.lisp")
Hello world


Note: it runs the file immediately displaying the result to the console.

You might be wondering about the () and think there like {} in C but they really not. 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.

heres another list

Code: Select all

> (= x 5)
5
> x
5


This show how to assign a value to the atom x. X evaluates to value of x which in this case is 5.

Code: Select all

> (+ 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.

Tomorrow: lists - cons, car, cdr - list fundamentals - breaking apart list and sticking them back together!

[l3v3rz]

day 3 - 22 Days to Go
================

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

Code: Select all

> '(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. Its actually short hand for the function quote i.e. '(a b) is the same as (quote (a b)). Atoms can be numbers or symbols, and numbers can be integers or floating points (real numbers).
And lists can be me a mixture:

Code: Select all

> '(a b 1 2 4.0)
(a b 1 2 4.0)

;;; or even lists of lists [This is a comment by the way!
> '( (a b) (c d))
((a b) ( c d))

;;; I can set a atom to a list
> (= x '(a b c))
(a b c)
> x
(a b c)


But atoms can also be functions - which is where Lisp gets its power - its a user definable symbolic processing language that can be written in itself using just a few primitives
So (+ 1 2) is just another list:

Code: Select all

> '(+ 2 3)
(+ 2 3)
> (= x '(+ 2 3))
(+ 2 3)
> x
(+ 2 3)


Now a little history (from wikipedia)

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.

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)

Code: Select all

> (= x '(1 2 3 4 3 2 1))
> ( car x)
1
> (cdr x)
(2 3 4 3 2 1)


Note they can be chained.

Code: Select all

> (cdr (cdr ( cdr x)))
(4 3 2 1)


Also if you have an empty list ()

Code: Select all

> (cdr '())
null


This becomes important later when we do recursion.

Car and cdr breaks lists apart - how do we stick them back together ?

cons - construct a list

Code: Select all

> (cons 'a '(1))
(a 1)
> (cons 'b x)
(b 1 2 3 4 3 2 1)


Here a bit more complex example - adding a list to another list

Code: Select all

> (cons '(a b) '(c d))
((a b) c d)


So what would I get if I took car of the newly constructed list??

Tomorrow: Functions - we're getting closer !

[l3v3rz]

Day 4 - 21 Days to go
===============

So to yesterdays question :

So what would I get if I took car of the newly constructed list??

Code: Select all

> (car '((a b) c d))
(a b)


A list is returned - the first element

Did you try to create like this

Code: Select all

> (cons 'a 'b)
Exception : Not a sequence


This is because cons expects its second argument to be a list (or null). so to do the above
you need

Code: Select all

> (cons 'a (cons 'b nil))
(a b)
>


Now on to functions as promised.

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

Code: Select all

> (def hello () (prn "hello world"))
LSharp.Function
> (hello)
hello world
"hello world"


So whats 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

Code: Select all

> (def addone (x) (+ x 1))
LSharp.Function
> (addone 5)
6
> (addone (addone (addone 7)))
10


You might try an 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:

Code: Select all

> (if null 'true 'false)
false
> (if t 'true 'false)
true
> (if (> 3 5) 'true 'false)
false
> (if (<3>

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 belseif c then d else e.

A few examples:

Code: Select all

> (= 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 todays quiz - can you create a function 'spell' that uses an if statement and outputs the following

Code: Select all

> (spell 1)
one
> (spell 3)
three
> (spell 21)
twenty one


etc .. i.e. any number up to 99. Can you do it without listing all 99 values??

Tomorrow - access to windows functions - connecting to the serial port!!!

[l3v3rz]

Day 5 - 20 Days to go
===============

The weekends arrived ! So a simple spell function would provide an compound if statement - as follows:

Code: Select all

> (def spell (x) (if (is x 1) 'one (is x 2) 'two 'no))
LSharp.Function
> (spell 1)
one
> (spell 2)
two
> (spell 3)
no
>


But what about handling bigger numbers? Now you need to have multiple if statements:

Code: Select all

 
> (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  )
  )
)
*** redefining spell
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 its almost there - can you think how to improve this ? Send me your suggestions !

Ok - so how do we control the robobuilder ?? The first thing we need to do is establish a serial port connection to robot. And here's how we do it. Remember from Day 1 - this code launched a MessageBox ...

Code: Select all

(reference "System.Windows.Forms")
(using "System.Windows.Forms")
(MessageBox.Show  "Loaded?" "RoboBbuilder is Go!")


Its easy to call windows functions. Heres an alternative way of creating your own prn function, using windows console class.

Code: Select all

> (Console.WriteLine "helloworld")
helloworld
null


So now to create a function to connect to the robot !!

Code: Select all

> (reference "System.IO.Ports")
null
> (using "System.IO.Ports")
"System.IO.Ports"
> (def connect ()
 (= sport (new "SerialPort"))
 (.set_BaudRate sport 115200)
 (.set_PortName sport "COM3")
)
LSharp.Function
> (connect)
null


So what did this do ? "reference" loads the list of assemblies and "using" makes the code shorter so that functions don't need to be named in full. The heart of this connect functions is this line - (= sport (new "SerialPort")). This assigns ("=" function) to the atom 'sport' a new instance of the object SerialPort. This is a built in .NET function. .set_xxxx enables property xxxx on the object instance to be set. So in this case I set the baud rate and COM port required. This doesn't have to be a function - but I could have passed parameters into the function for com port or baud rate - something you might want to try??

At this point I've not attempted to connect to the robot. To do that I need to 'open' or close the port ...

Code: Select all

> (.open sport)
null

> (.close sport)
null


That really did open and close the port !

Next: We can connect serially - so next is to use the robobuilder lib to issue commands to the robot for reading the firmware and serial number.

BTW if you're reading and have any questions or just enjoying the thread, (or noticed any bloopers) then drop me a message.

[l3v3rz]

Day 6 - 19 days to Go!
===============

We saw how to connect to a serial port. So we now need to create some functions that use this. First off lets set up some headers and globals. Note you need the full path RobobuilderLib. If its in the same directory as the LSharpConsole.exe then just the name is fine.

Code: Select all

(reference "RobobuilderLib")


(using "RobobuilderLib")
(using "System.Windows.Forms")
(using "System.IO.Ports")

(= sport ()) ;; initialise global
(= pcr  ())  ;; initialise global

(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". But what would be neat is to write a function and let the user select the port from the keyboard. So here goes ....

Code: Select all

(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)
  )
)


So what does askPort do ? It gets a list of ports using the .NET function into a local atom / variable 'p. This will be a list i.e. "COM1" "COM2" etc. Using the each iterator function its displays each element of the list returned. Finally it ask 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, sol ets 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 its not at the moment. So make sure your serial cable is connected and the robot is on.

Code: Select all

(def getsn ()
  (do
    (.open sport)
    (= sn (.readSN  pcr))
    (.close sport)
    sn
  )
)


Finally we now have the top level function that brings it all together:

Code: Select all

(def run_robobuilder()
  (connect (askPort))
  (getsn)
)


Just type (run_robobuilder). You should see something like this:

Code: Select all

> (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.

Tomorrow: More PCremote functions - lets get it moving !

[limor]

back in 1987 Emacs was the best thing since sliced bread and with the Lisp back-end it beat the socks off VI and ED. i did some big projects in emacs lisp back then. control over emacs at the time was a great thing to have. till today no editor provides that kind of human-machine assimilation.

Once you have a working library for RoboBuilder, i hope people will choose this interactive environment to try out their ideas quickly and painlessly.

Maybe check out the latest Bioloid C library (robosavvy bioloid forum) to get ideas for the library as the bioloid and robobuilder are very similar.

[l3v3rz]

Day 7 - 18 days to Go!
================

I've put all the PCremote functions in a single file. http://robobuildervc.googlecode.com/files/Day7.lisp Download into the same directory as the LSharpConsole.exe

Now to use this just start Lsharp L#

Code: Select all

L Sharp 2.0.0.0 on 2.0.50727.3603
Copyright (c) Rob Blackwell. All rights reserved.
> (load "Day7.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
>


How does this work? All the key functions have been describe in previous posts - but now with an added top level function run_robobuilder.

Code: Select all

(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
)


Its 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. Here is the checkver function:

Code: Select all

(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.

Some of the PC remote functions do rely on recent firmware I suggest 2.23 or better (you can see I'm running 2.26) go to the http://Robobuilder.net/eng site to download.

The program then gets and displays, using the print (prn) command, the serial number and also now the distance. If you don't have a distance sensor it show 10 (cm). The prn is the same as pr but with newline added.

Finally the Day7.lisp also includes runMotion and runSound. More on these tomorrow - but for now assuming your robot is still on and connected

Code: Select all

> (runMotion 7)


This will make robot take up basic pose.

Form more information on PCremote http://code.google.com/p/robobuildervc/wiki/Starthere shows you the schema - and the details in the code http://code.google.com/p/robobuildervc/ ... Cremote.cs

To edit the files I use notepad but for those who prefer something with parenthesis mapping I can recommend Notepad2 or Programmers Notepad freeware which are both good. You could 'even' use Emacs as your editor with Lsharp - according to the Lsharp.org web site:

Code: Select all

;; Support for L Sharp
(show-paren-mode t)
(add-to-list 'auto-mode-alist '("\\.ls$" . lisp-mode))
(setq inferior-lisp-program "cmd /c  C:\\LSharp\\LSharpConsole\\bin\\Debug\\LSharpConsole.exe")


Next - menu driven motions

[l3v3rz]

Day 8 - 17 days to Go!
======================

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.

Code: Select all

(= 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.

Code: Select all

(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 pair 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 request input and look 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

Code: Select all

;; loop until 0 selected
(def domenu ()
  (with (item 1)
     (while (not (is item 0))
         (= item (ask "Choice :" "No such option" menu))
         (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.

Code: Select all

L Sharp 2.0.0.0 on 2.0.50727.3603
Copyright (c) Rob Blackwell. All rights reserved.

> (load "Day7.lisp")
> (run_robobuilder)
> (domenu)


Isn't Lisp/L# lovely and compact!
Tomorrow: Accessing the accelerometer.

[l3v3rz]

Day 9 - 16 days to Go!
======================

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.

Code: Select all

L Sharp 2.0.0.0 on 2.0.50727.3603
Copyright (c) Rob Blackwell. All rights reserved.
> (load "lisp/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

> (.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.

Code: Select all

> (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))
        )
    )
)

> (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 could 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 ....

Next: Access the servo directly

[l3v3rz]

Day 10 - 15 days to Go!
================

To read and control the wck servo requires switching into Direct control (DC) 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 everything can be controlled. 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.

Code: Select all


(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 its simple to access a servos positions using wckReadPos method.

Code: Select all

(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 cadr to return the position of second element. You could use nth command as explained yesterday to achieve the same result.

Thus to read the position of servo id 5, all that 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, as follows:

Code: Select all

(def setPassive (n)
  (.wckPassive wck n)
)


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 1/2 second - the values off the servo as I move it. here's the complete sequencing starting with loading Day7.lisp - available here - http://robobuildervc.googlecode.com/files/Day7.lisp

Code: Select all


L Sharp 2.0.0.0 on 2.0.50727.3603
Copyright (c) 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


Next: Now have the catch - next is how to play!

[l3v3rz]

Day 11 - 14 days to Go!
======================

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.

Code: Select all

(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. We use cadr function to get the second element in the list. The first element is the load on the servo which is generally zero unless under heavy load.

The function 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 functions. Here's an example session - you need to have loaded Day7.lisp and (run_robobuilder) as before:

Code: Select all

> (setServoPos 12 60 2)(capture 12 5 0.05)
2
> 20
28
43
57
60
60


See how the position changes as the servo moves from its initial position - 20 - to its commanded position 60. Capture is reading the current servo position every 0,05s - or 20ms. Here's the code for capture.

Code: Select all

(= 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. This record or list of positions can be played 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 for it to play back. Here is the code for the play function - it takes 3 parameters, the servo id, the number of points to send and the time interval in seconds.

Code: Select all

(def play (id n t)
  (for i 0 (- n 1)
     (do (pr ".")
         (setServoPos id (nth data i) 2)
         (sleep t)
     )
  )
  "Complete"
)


We could slow or speed up the motion using a different 't parameter, or even play it back through a different servo - to get the left arm to mirror the right arm for instance. Here's an example session:

Code: Select all

   
> (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.

Next: Synchronous moves

[l3v3rz]

Day 12 - 13 days to Go!
=================

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 that the global wck variable has been initialised by dcmodeOn before this function can be called.

Code: Select all

(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 for 16 servos (0-15) the "last id" would be 15. The torq is the speed it moves, a value between 0-2 with 0 being the fastest. Finally it the 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.

Code: Select all

(= 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 that the toarray function, it's required to convert a list into an array object[] so it can be passed into the wck method.

Now. what is 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

Code: Select all

(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 a new list. See how it uses cons to create the output list. The following assumes Day7.lisp is loaded and command (run_robobuilder) and (dcmodeOn).

Code: Select all

> (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 position basic. Next we need to calculate the in-between points. For example, if the servo is at position 20 and it moves to 60 in 10 steps, then each step is (60-20)/10 or 4. To move the servo would set the following positions in turn: 20 24 28 32 36 40 ... this will be a smooth move! Alternatives would be to use non-linear algorithms, such as a sinusoidal, that make the movement greatest in the middle part of the move.

Tomorrow: Smooth moves and the In-betweens

[l3v3rz]

Day 13 - 12 days to Go!
======================

More than halfway !

Shameless plug: If you want to give a gripper for your robobuilder for Xmas - you need to 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.

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 list 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. Here is the code to create that list:

Code: Select all

(def md (xs ys step)
     "calc distance between two list"
     (= mlist ())
     (for i 0 (- (len xs) 1)
           (= t  (/ (- (nth xs i)  (nth ys i)) step))
           (= mlist (cons t mlist))
     )
     (reverse mlist)
)


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 its 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:

Code: Select all

(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
           (= slist ())
           (for j 0 (- l 1)
             (= t  (- (nth a j)  (* (nth interval j) i)) )
             (= slist (cons t slist))
           )
          (prn "(setSyncMove " l " " 2 " '(" (reverse slist) "))")
          ;(setSyncMove l 2 (reverse slist)) (sleep 0.1)
     )
)


Here is an example of it running ..

Code: Select all

> (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.

Tomorrow - making complete motions

[l3v3rz]

Day 14 - 11 days to Go!
======================

Yesterday's 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:

Code: Select all

(= 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 numbers, 1 and 70 for example, specify the number of in-between step (or frames in robobuilder speak) and the total time to take. So for 1 frame in 70ms that's 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.

Code: Select all

(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")
  )
)


So to use this function we must first load and run Day7.lisp and run_robobuilder to connect the serial port. This also needs the other functions covered in the last few days posts. All the necessary functions into a single file that can be down loaded from: http://robobuildervc.googlecode.com/files/wckutils.lisp.

Note: This program assumes Day7.lisp is in the same directory and it will auto load it for you and run it.

The function demo make the necessary steps to perform the motion.

Code: Select all

(def demo ()
  (dcmodeOn)
  (= cur (getallServos 15))
  (smove cur basic 10 0.1)
  (playmotion cur punchleft)
)


Once loaded all you need to type is "(demo)" - here an example session ..

Code: Select all

L Sharp 2.0.0.0 on 2.0.50727.3603
Copyright (c) Rob Blackwell. All rights reserved.
> (load "wckutils.lisp")
.................
.Available Ports:
COM1
COM3
Select:
: COM3
connecting to COM3
.
.
.
> (demo)


This should will make your bot take up "basic" pose and then punch out left - so be warned! This is great, but having the servo data stored as code is not easy to manage, what would make things easier is if they can be loaded from a file directly into the list.

Tomorrow: loading data from CSV files