Asked
— Edited
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
Hey Mel, The auto tune on the Kangaroo X2 make set up of it very easy. It's important however to have your motor attached properly, have a feedback device like a pot or encoder attached to it and everything wired correctly. If any of these are not present the Kangaroo will sense this and shut down. This can be both good and bad as it's frustrating trying to find what's not working properly but it will also sense a catastrophe situation like a runaway or short and shut it's self down. The auto tune is a wonder and a lot of fun to watch. I love watching my robot twist and turn at different speeds while the tune is going on. Here's a few Ytube vids I found of this:
Nice , kangaroo sounds like it is an excellent add on for the sabertooth. I should pick on up after ezbs arrive.
@Joeh, I highly recommend this combination. It would be a snap and a no brainer for a guy with your talent.
Josh, I second Dave's comments the Kangaroo/Sabertooth combo is an excellent PID motor controller, look at my earlier parts in this thread where I show coding the Kangaroo from EZ-Script it covers pretty much all you need to know about fully controlling motors and analysing feedback from the encoders etc.
I have though discovered a problem with the Kangaroo in that you cannot use high resolution encoders (of the type I use) in mixed mode (dual motor locomotion), I am having to design a circuit that divides the encoders outputs to get it all to work. With a single motor with high res encoder (independent mode) you should be ok, its just when the Kangaroo has to deal with 2 motors simultaneously it cannot keep up with the counts.
Tony
OK, I sort of understand now. Thanks , Guys! And, I do crawl under a rock from time to time to rest my eyes and brain.
@Tony, I can confirm that the kangaroo works perfectly in independent mode useing the high resolution encoder. That's how I'm controlling my motors and they function fine.