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

Korean company maker of Robot kits and servos designed for of articulated robots. Re-incarnation of Megarobotics.
10 postsPage 1 of 1
10 postsPage 1 of 1

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

Post by l3v3rz » Tue May 04, 2010 12:25 am

Post by l3v3rz
Tue May 04, 2010 12:25 am

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]
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]
Last edited by l3v3rz on Sat May 08, 2010 10:56 am, edited 8 times in total.
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Post by l3v3rz » Tue May 04, 2010 9:11 am

Post by l3v3rz
Tue May 04, 2010 9:11 am

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
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
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Post by pepep » Tue May 04, 2010 6:29 pm

Post by pepep
Tue May 04, 2010 6:29 pm

Great! Great! Great!

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

ASAP I load in my RB and try it!!!
pepep offline
Savvy Roboteer
Savvy Roboteer
Posts: 38
Joined: Sun Mar 28, 2010 7:14 pm

Post by pepep » Sat May 08, 2010 10:04 am

Post by pepep
Sat May 08, 2010 10:04 am

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!
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!
pepep offline
Savvy Roboteer
Savvy Roboteer
Posts: 38
Joined: Sun Mar 28, 2010 7:14 pm

Post by l3v3rz » Sat May 08, 2010 10:49 am

Post by l3v3rz
Sat May 08, 2010 10:49 am

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 :) ]
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 :) ]
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Post by pepep » Sat May 08, 2010 1:50 pm

Post by pepep
Sat May 08, 2010 1:50 pm

And Color method?
And Color method?
pepep offline
Savvy Roboteer
Savvy Roboteer
Posts: 38
Joined: Sun Mar 28, 2010 7:14 pm

Post by l3v3rz » Sat May 08, 2010 2:04 pm

Post by l3v3rz
Sat May 08, 2010 2:04 pm

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.
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.
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Post by pepep » Sat May 08, 2010 2:09 pm

Post by pepep
Sat May 08, 2010 2:09 pm

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


???
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'.
>


???
pepep offline
Savvy Roboteer
Savvy Roboteer
Posts: 38
Joined: Sun Mar 28, 2010 7:14 pm

Post by l3v3rz » Sat May 08, 2010 2:15 pm

Post by l3v3rz
Sat May 08, 2010 2:15 pm

[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")
[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")
l3v3rz offline
Savvy Roboteer
Savvy Roboteer
Posts: 473
Joined: Fri Jul 18, 2008 2:34 pm

Post by pepep » Sat May 08, 2010 2:20 pm

Post by pepep
Sat May 08, 2010 2:20 pm

Perfect!
Perfect!
pepep offline
Savvy Roboteer
Savvy Roboteer
Posts: 38
Joined: Sun Mar 28, 2010 7:14 pm


10 postsPage 1 of 1
10 postsPage 1 of 1
cron