All Tutorials / Toymaker / Controlling A Kangaroo With The V4

Controlling A Kangaroo With The V4

Save As PDF

Description

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 EZ-Builder so I had to write a block of code to do this DJ maybe I am missing...

Controls

This tutorial uses the following controls. Select a control to view the manual page.

Step 1

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 EZ-Builder 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

Code:


# -------------------------------------------------
# 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

Steps