United Kingdom
Asked — Edited

Controlling A Kangaroo With The V4

For Dave and Richard and anyone else who is interested, here is fully working and tested code for using the Kangaroo (with bi-directional comms) via the UART on the V4 with encoder feedback.

I am still quite new with EZ scripting so the code may not be as elegant as I would like, I could not find an ASCII to decimal function in ARC so I had to write a block of code to do this DJ maybe I am missing something here?

Anyway I think this does show how fantastic and versatile the V4 is, well done DJ and team!

Tony


# -------------------------------------------------
# Name    : V4_Roo_serial
# Author  : Tony Ellis
# Date    : 01/07/2014
# Version : 1.0
# -------------------------------------------------

definearray($byte,10)
$wheel_turn=63890
$dly=50
$FLAG=0
uartinit(0,0,19200)          # initalise UART

# ***** Initalise Kangaroo
uartwrite(0,0,"D,start",13)  # drive channel start
uartwrite(0,0,"T,start",13)  # turn channel start
uartwrite(0,0,"T,p0",13)     # this is also needed at start

#goto(START)

$val=2*$wheel_turn   # load position
$COMM_CHR="DF3"      # D=drive channel - F=move forward - 3=speed 3
goto(INC_POS)
halt()

:START

$val=$wheel_turn-500
$position=0
$unit=round($val/8)
$str="4000"
$x=1
goto(RAMP)
$str="7000"
$x=2
goto(RAMP)
$str="10000"
$x=3
goto(RAMP)
$str="7500"
$x=6
goto(RAMP)
$str="5500"
$x=7
goto(RAMP)
$str="2500"
$x=8
goto(RAMP)
uartwrite(0,0,"D,s0",13)
halt()

:RAMP
uartwrite(0,0,"D,s"+$str,13)
$v=$unit*$x
REPEATUNTIL($position>$v)
  goto(GET_POS)
ENDREPEATUNTIL 
print("position="+$position)
print("speed="+$str)
return

# -------------------------------------------------
# Subroutines

# ***** POSITION commands *****

:INC_POS                           # increment position subroutine
$str=tostring($val)                # convert variable to string
$COMM="pi"                         # determine direction
IF (getcharat($COMM_CHR,1)="R")
  $COMM="pi-"
ENDIF 
goto(SEND_DATA)
goto(GET_POS)
REPEATUNTIL($FLAG=1)
  goto(GET_POS)
ENDREPEATUNTIL 
print("POSITION = "+$position)
return

:GET_POS                           # get current position subroutine
uartwrite(0,0,"D,getp",13)
sleep($dly)
goto(GET_DATA)
print($position)
return

# -------------------------------------------------
:SEND_DATA
$M_SPEED=(getbyteat($COMM_CHR,2)-48)*1200
$string=(getcharat($COMM_CHR,0)+","+$COMM+$str+"s"+$M_SPEED) # build data packet
uartwrite(0,0,$string,13)                                    # send packet
sleep($dly)                                                  # delay required
return

# -------------------------------------------------
:GET_DATA
$num_bytes=uartavailable(0,0)

$data=uartread(0,0,$num_bytes)

$byte1=getbyteat($data,0)    # byte 1 = command
$byte2=getbyteat($data,1)    # byte 2 = ","
$byte3=getbyteat($data,2)    # byte 3 = command completed status

# ***** Convert ASCII digits to decimal number
$total=0
$multiplier=1
$x=$num_bytes-2
REPEATUNTIL($x=3)#$num_bytes-7)
  $byte[$x]=getbyteat($data,($x-1))
  $byte[$x]=($byte[$x]-48)*$multiplier
  $total=$total+$byte[$x]
  $multiplier=$multiplier*10
  $x--
ENDREPEATUNTIL 
  
  # ***** Determine byte 3
  # $FLAG=0
IF ($byte3=80)
  $FLAG=1      # "P" returned so move completed
ELSEIF ($byte3=112)
  $FLAG=2      # lowercase "p" so move not completed
ELSEIF ($byte3=83)
  $FLAG=3      # "S" returned so speed reached
ELSEIF ($byte3=115)
  $FLAG=4      # lowercase "s" so still accelerating
ENDIF 

IF ($FLAG<3)
  $position=$total
ELSEIF ($FLAG>2)
  $speed=$total
ENDIF   
return

ARC Pro

Upgrade to ARC Pro

Stay at the forefront of robot programming innovation with ARC Pro, ensuring your robot is always equipped with the latest advancements.

#17  

@Toymaker

Rather than just sending the script to individuals, can I recommend that you put it in an EZ-B project (with nothing else) and attach the project to a post? That way months/years from now, users will still be able to download and work with it without you needing to respond to every request.

Alan

#18  

@Tony, I know how valuable and limited your time is. Your truly an exceptional and top quality person to do this for me and others here. I can't properly express how much this is appreciated. With your workload and a family life I never expected this so quickly. Thanks my friend. I hope that someday, somehow I can return the favor. :)

PRO
United Kingdom
#19  

Dave, here is the script for independent mode it works fine here, you will need to set the SPEED_MULTIPLIER var (it probably will be double my value), also wheel_turn will be half. I will send you the project file by email.

The example first goes forward 2 revolutions (at speed 9) and continues to report positions back from the Kangaroo until it reaches the target position. It then goes in reverse for 1 revolution (at speed 3) again reporting back the position. You will see a slight lag in the position printout on the script window, this is because the V4 cannot fully keep up with the Kangaroo at these baud rates so it over-runs a bit, the position will still be got to accurately though.

For channel 2 just change the command string to something like "2F7" etc

As this is a tutorial I will post the script here so all can see.


# -------------------------------------------------
# Name    : V4_Roo_independent
# Author  : Tony Ellis
# Date    : 01/08/2014
# Version : 1.0
# -------------------------------------------------

definearray($byte,10)
$wheel_turn=63890
$dly=50
$FLAG=0
$CHANNEL=1
$SPEED_MULTIPLIER=1200       
uartinit(0,0,9600)  # initalise UART

# ***** Initalise Kangaroo
uartwrite(0,0,"1,start",13)  # drive channel start
uartwrite(0,0,"2,start",13)  # drive channel start
uartwrite(0,0,"1,p0",13)     # this is also needed at start
uartwrite(0,0,"2,p0",13)     # this is also needed at start

#goto(EXAMPLE_2)

# ***** Example for CHANNEL 1
$val=2*$wheel_turn   # load position = 2 wheel turns
$COMM_CHR="1F9"      # 1=drive channel - F=move forward - 9=speed 9
goto(INC_POS)
print("position attained")
sleep(2000)
$val=$wheel_turn     # load position = 1 wheel turn
$COMM_CHR="1R3"      # 1=drive channel - R=move forward - 3=speed 3
goto(INC_POS)
print("position attained")
halt()

# -------------------------------------------------
# Subroutines

# ***** POSITION commands *****

:INC_POS                           # increment position subroutine
$str=tostring($val)                # convert POSITION variable to string
$COMM_CHR,0=tostring($CHANNEL)     # convert CHANNEL variable to string
$COMM="pi"                         # determine direction
IF (getcharat($COMM_CHR,1)="R")
  $COMM="pi-"
ENDIF 
goto(SEND_DATA)
goto(GET_POS)
REPEATUNTIL($FLAG=1)
  goto(GET_POS)
ENDREPEATUNTIL 
print("POSITION = "+$position)
return

# -------------------------------------------------

:GET_POS                           # get current position subroutine
uartwrite(0,0,$CHANNEL+",getp",13)
sleep($dly)
goto(GET_DATA)
print($position)
return

# -------------------------------------------------
:SEND_DATA                         # assemble data packet and send to Kangaroo
$M_SPEED=(getbyteat($COMM_CHR,2)-48)*$SPEED_MULTIPLIER
$string=(getcharat($COMM_CHR,0)+","+$COMM+$str+"s"+$M_SPEED) # build data packet
uartwrite(0,0,$string,13)                                    # send packet
sleep($dly)                                                  # delay required
return

# -------------------------------------------------
:GET_DATA                          # get data back from Kangaroo
$num_bytes=uartavailable(0,0)

$data=uartread(0,0,$num_bytes)

$byte1=getbyteat($data,0)    # byte 1 = command
$byte2=getbyteat($data,1)    # byte 2 = ","
$byte3=getbyteat($data,2)    # byte 3 = command completed status

# ***** Convert ASCII digits to decimal number
$total=0
$multiplier=1
$x=$num_bytes-2
REPEATUNTIL($x=3)#$num_bytes-7)
  $byte[$x]=getbyteat($data,($x-1))
  $byte[$x]=($byte[$x]-48)*$multiplier
  $total=$total+$byte[$x]
  $multiplier=$multiplier*10
  $x--
ENDREPEATUNTIL 
  
# ***** Determine byte 3
# $FLAG=0
IF ($byte3=80)
  $FLAG=1      # "P" returned so move completed
ELSEIF ($byte3=112)
  $FLAG=2      # lowercase "p" so move not completed
ELSEIF ($byte3=83)
  $FLAG=3      # "S" returned so speed reached
ELSEIF ($byte3=115)
  $FLAG=4      # lowercase "s" so still accelerating
ENDIF 
  
IF ($FLAG<3)
  $position=$total
ELSEIF ($FLAG>2)
  $speed=$total
ENDIF   
return
#20  

@Tony, I said this to you privately in my last e-mail but I'd like to repeat it here publicly;

I hope I'm not starting to sound too mushy or scripted but I really do appreciate this. I'm honored to be using scripts in my B9 crafted by a designer of your caliber and fame. I really look up to you and respect and amazed at what you can do. I'm looking forward to digging into this script and seeing if I can figure it out and apply it. I'm sure I'll have a question or two. Please don't feel like you need to respond right away and help figure it out. We both have busy lives and lots to do.

I've been showing my family your Cube World commercials and some of them remember them. My 40 yr old son (I was very young when he was born ;-) ) members clearly and got real excited. He says he really wanted to buy a set when they were available but didn't have the opportunity or cash. Good luck on the new roll out when it ready. I hope you are able to let me and others on the forum know when that will be. These and your little dinosaurs would make excellent gifts for my grand kids (and me too).

#21  

Thanks, Tony for sharing this code with us. Mel

#22  

BTW, what is the purpose of the Kangaroo? Is it just for serial communications? Or does it do something else?

thanks,

#23  

Jeez Mel.... have you been under a rock for the last 6 months lol.... What it is a how to use the kangaroo has seen a couple of major threads on the subject (including this one).... just do a search here (Dave stated a major one for his B9)...Also go to Dimension Engineering's web page to read up on it.... Why don't you take the time to read up on it.... then ask the questions...

PRO
United Kingdom
#24  

Mel, the Kangaroo is an ingenious device that converts the Sabertooth motorcontroller into a fantastically versatile closed-loop dual motor dive system (closed-loop = feedback of position etc - open loop = no feedback), so a servo is a closed-loop device as it has a feedback potentiometer. The Kangaroo also allows for encoder feedback and I use this (see my threads on it) to make my robots go in totally straight lines and precise distances. It uses an algorithm known a PID (Proportional-Integral-Derivative) which is normally quite difficult to tune, the genius thing about the Kangaroo is that it has an "autotune" function that does the tuning for you. Here is a link

en.wikipedia.org/wiki/PID_controller

Tony