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
Thanks again Tony for being willing to hang in there with me. I know and understand how busy you must be. You do amazing work and I'm always impressed and inspired by what you come up with. That Cube World looks like a real scream. I hope you have success bringing it to market and I have the opportunity to get a stack of them. I think I remember something about them a while ago. How long ago did they premiere and were they sold in the USA?
What you state makes sense somewhat. I understand you have your Roo in Mixed mode and this script is written for wheels turning in tandem because the Sabertooth has two channels that drives one motor each.
I have my Kangaroo that controls the Sabertooth set on independent mode because I have each motor attached to the Sabertooth operating different functions of the B9 robot. Channel 1 operates the waist turning motor. This is the motor you pointed me to that has this exceptional encoder attached. This is the channel I really need the ramping abilities of this script and where I'll be sending most of my position and speed commands to. Channel 2 of the Sabertooth drives my hip motor that bends the robot over. This motor also has the same type encoder sending feedback but is attached differently. This will be just basic up and down movement with maybe a stop half way once in a while.The need for ramping is not so important here but would be nice as I think it helps with mechanical wear at the least.
So to make a long story short (too late I guess) I'll need to be sending different position and speed commands to each of the channels.
As for my first numbered listing above I see that the position returned is always 2. The section you referenced as not having the flag set looks like the position section. I wonder if somehow I'm not getting the position feedback from the Roo into the script. I see there is a Delay of 50 after the Getp command. Maybe I need more time?
Also i changed your $COMM_CHR="DF3" to $COMM_CHR="1F3" in my script. Should I have done this since my channels are numbered 1 and 2?
Just for reference here's a couple pics showing the two motors I want to move independently:
Hip Motor, You can see the EZB v4 and the Sabertooth in the background,
Waist Motor, It's covered in Dynamat to dampen the noise. If I reverse direction to quickly the gears skip teeth and screws up the Home. Ramping will stop this.
OK, I've had limited success. I got the motor to move to the assigned position with your script and the print() in the console shows the progressed position as it moves. However the motor moves very slowly with no ramp up to top speed. I'm not sure yet how to set the top speed with this script so that may be that problem. Sadly though the way I got it to work is impracticable for each time I use this script which may be a ton.
I haven't reset my Roo to Mixed mode yet as you suggested but I can't really operate in that mode but I will if you think it will help. However I tried this first; I inserted a command into the beginning of your script to send Kangaroo's "Home" command to each channel and a Sleep() command to let it complete the move. After the Homing was complete and the Sleep() expired the motor took off (very slowly) to the assigned position and stopped.
The start of the modified script looks like this with the added lines:
I tried to run the script again without the Home command and it did nothing till I again added in the commands. This is odd and I have no idea why it is doing this but at least I know the script will move the motor.
As an added note; I have the following commands in my start up script when ARC starts So I shouldn't need them again in your script (I think):
I have no idea what to do now or where to look after this. Hopefully this will aid you with the tweaks. Of course when you have time and ambition to do it. Family, friends, personal enjoyment and the day job always comes first.
Tony, what do you mean by Ascii to Decimal?
This will send a single byte with the value of 123...
Hi DJ
The Kangaroo returns large numbers (just 10 wheel revolutions would be 638900) in an ASCII string for position (> a byte and would require a Dword var) and I want to convert this into a single decimal number so I devised the routine (***** convert ASCII digits to decimal number) in my listing to do this. I was just not sure if their was a better way? I really like your scripting language and want to use it efficiently.
My code listing got corrupted when it was pasted into the code view. I have looked through and there are a number of omissions/problems caused by pasting it in via the code view, complete lines missing and "<" and ">" directives are even extracted from some lines.
Apologies to anyone trying to use the code but with the corrupted parts its probably unusable, as I said before I am happy to sent the script to anyone that wants it.
Tony
@Tony, the code I've been using is the one you sent me through the web.
Dave, I know you have the full code, but the omissions made my script look completely wrong and it then did not make sense in places so I wanted to alert other forum members. Below is a video that shows the script working, the first demo calls for the Kangaroo to drive forward 2 wheel revolutions at slow speed (3) "DF3" you can see the returned positions from the Kangaroo (in decimal) on the screen. The second demo (on the video) is a kind of course ramp up to speed and then ramp down. I will not be using the V4 to control my Kangaroo as I have a dedicated locomotion PIC that sits as an interface for the V4 so it does not to get "bogged down" with controlling, this is why this script/demo are a bit limited as I just knocked it up quickly to help you guys on the forum.
Cube World sold all over the World, it was huge in the States and also Japan, here is a very cool Japanese TV advert
youtu.be/AotdNHrk0iM Here is my favorite US TV advert for series 4 (sports) youtu.be/fToQbGUT-s8
With our new cubes characters can now jump out of their cube into a tablet device.
Tony
Thanks Tony for the video. Good luck with your cube project. I hope you keep us informed of its progress.
Dave, I have spent a hour and modified my rig to "independent mode" and modified my script and got it to work now in independent mode. I will spend another hour tomorrow (refine the code a bit) and send you the revised script.
Tony