Auto balance for Robobuilder in Lisp / L# - custom firmware

[l3v3rz]

I have a demo of auto-balance written in Lisp work - albeit very simple. It moves the arms to counteract the force being applied, but it can easily be extended as its data driven, to move any combination of servos. It requires custom firmware to get a reasonable speed.

When running on the standard firmware you have to switch between the two modes, PC remote and Direct control (DC) mode. In DC mode you can control the servos and in PC mode you can read the sensors, but to switch between the modes takes time. What's worse is that if you switch too early you get bad data sent, or it misses requests. It takes around 100 ms to switch modes reliably and you need to to twice each cycle i.e. (read sensor, switch, update servos, switch) So the fast cycle time is around 200-250ms or 5 Hz. I have written a custom piece of firmware called DC mode plus that enable the sensors to be read without switching modes. It does this by hijacking servo 30. read positions on that servo will return various sensor data. Its speed is around 40-50ms or 20-22Hz Hz

[Edit: the speed is limited by L# - writing in C# improves the speed of the loop to sub 10ms/+100Hz]

I have a dynamic balance demo working. The LISP code is quite straight forward, details and links at the end

Code: Select all


(def calibrateXYZ ()
 (= xyz (readAcc))
 (= x (car xyz)) (= y (cadr xyz)) (= z (car (cddr xyz)))
   (= gx x)
   (= gy y)
   (= gz z)
   (prn "Calibrated: " gx " " gy " " gz)
)

;reading Z value - by reading position of Servo 30 (with parameter "1")
(def getZ ()
   "DCMP - quicker if we just want Z"
   (.wckReadPos wck 30 1)                   ; get y & Z
   (with (z (nth (.respnse wck) 1))
   (= z (if (> z 127) (- z 256) z))
   )
)

(def bt4 ()   "dcm plus mode - high speed"

  (if (not (bound 'DCMODEPLUS)) (err "load DCMP.lisp"))
  (standup)
  (calibrateXYZ)

  (= base (getallServos 15))
  (pause)
 
  (try   
  (while (not (Console.keyavailable))

   (= c1 (- (getZ) gz))
     
   (= dz (rmatch c1 Z2))
   (if (or dz)
    (do
      (= nxt (mapcar + base dz))
 
      (try (.SyncPosSend wck 15 4 (toarray nxt) 0) (do (pr "overflow") (prl nxt)) null)

      (= base nxt)
     )
    )
 )
 (prn "Exception caught")
 null
 )
 (standup)
)


Note: This is only some of the code - the rest will be posted on line. It only works with customer firmware

How it works: Its extending the arms based on the offset of the Z value. The amount of offset is determined by rmatch - which looks for ranges and returns an array which is then added to the current position. SynchPosSend then updates the servo.

Edit: Latest version of balance code - available here
http://code.google.com/p/robobuildervc/ ... lance.lisp

Download stable version:
http://robobuildervc.googlecode.com/files/balance.lisp

Function to call is (bt4)

phpBB [media]

[l3v3rz]

The customer firmware - can be found here :

http://robobuilderlib.googlecode.com/fi ... P_r111.hex

Load on to Robobuilder - and on power on, its automatically in Direct control mode. Only this mode exists. Gain access to accelerometer and PSD using read position commands against servo 30.

RobobuilderLib.dll still works - although an update is required to pass parameter information to readServo command. (v.1.9.9)

http://robobuildervc.googlecode.com/fil ... derLib.dll

[pepep]

Great! Great! Great!

ASAP I load in my RB and try it!!!

[pepep]

Hi,

I try your (bt4) function, but I have somes problems with it ..
1. I don't know why but the Console.KeyAvailable method don't work properly inside this function ...
2. and the valors of gz, Z2, ...

Thanks,

Btw, your firmware works great, I comment in the other post!

[l3v3rz]

Did you try downloading this version ?

http://code.google.com/p/robobuildervc/ ... lance.lisp

(its the full code including Z2 etc) - gz is a global set by calibrateXYZ (which is missing in the code snippet above - oops)

And get same problem ?

Incidentally is the problem that it just drops straight through ? I think Ive seen that - I just restart the console and that seems fix - I think it gets characters buffered and so immediately detects a key and stops. I think there's a .NET function to clear keyboard buffer - must look it up sometime

If its generating an exception - what message do you get - type *e at the console prompt to print last exception.

cheers

[edit: BTW - just added the missing lines to the post above ]

[pepep]

And Color method?

[l3v3rz]

Sorry not sure I understand question?

If you're having problems with
(= p1 (new "Pen" (Color.FromName "Black")))

then make sure you have

(reference "System.Windows.Forms")
(using "System.Windows.Forms")

Its for the graphic display - which you only need if DISPLAY is true.

[pepep]

I get this:

Code: Select all

> (reference "System.Windows.Forms")
System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089
> (using "System.Windows.Forms")
null
> (load "balance.lisp")
.*** redefining mapcar
.*** redefining setl
...*** redefining rmatch
.*** redefining calibrateXYZ
.can't find type = Color
Exception : Se produjo una excepción de tipo 'System.Exception'.
>


???

[l3v3rz]

[Edit]

just noticed - my boot loader - loads utilities.lisp by default which includes at the top:


(reference "System.Windows.Forms"
"System.Drawing")

(using "System.Drawing"
"System.Windows.Forms")

[pepep]

Perfect!