Propeller, CrustCrawler Nomad, HB25 and Position Controller issue

RoboPanther

Hi All,

I am quite new to the Parallax Propeller, HB25 and Parallax motor spin controllers.
My goal is to control a robot from a c# application. I am able send data to the propeller using the serial terminal to initiate the GoForward() spin function (see code below). I can also retrieve the distance information from the 3 PING sensors. But for some unknown reason the GetSpeed() function is not returning any useful data. Even if the robot is not moving I am getting different values back from this function. Even worse, after a couple of times, to be exact it always happens after 8 calls, the function GetSpeed() gets stuck, the execution just stops there.
(Btw, the function ChkForArrival() doesn't return any consistent data after 'arrival' for me either.)
Any advice would be greatly appreciated!

Peter

Below is the spin code I wrote:
(If I uncomment the line: speedRight := GetSpeed (RightWheel) execution stops after 8 repeat cycles.)

CON
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
PING_Pin_R = 1 ' Ping sensor pin - Right
PING_Pin_M = 6 ' Ping sensor pin - Middle
PING_Pin_L = 7 ' Ping sensor pin - Left

MPC_Pin = 4 ' Motor Position Controller pin
WMin = 381
QPOS = $08 ' Query Position
QSPD = $10 ' Query Speed
CHFA = $18 ' Check for Arrival
TRVL = $20 ' Travel Number of Positions
CLRP = $28 ' Clear Position
SREV = $30 ' Set Orientation as Reversed
STXD = $38 ' Set TX Delay
SMAX = $40 ' Set Speed Maximum
SSRR = $48 ' Set Speed Ramp Rate
AllWheels = 0
RightWheel = 2
LeftWheel = 1

OBJ
ping : "ping"
DrvMtr : "FullDuplexSerialPlus"
Terminal : "FullDuplexSerialPlus"

VAR
long range_R, range_M, range_L
long speedLeft
long speedRight
byte nRX, nRXCommand, nRXWheels, nRXSpeed, nRXMoveUnits, nReverse

PUB start | i, j, k, nCounter
' motor control
dira[MPC_Pin]~~
DrvMtr.Start(MPC_Pin,MPC_Pin,0,19200) ' Establish comunications to MPC
Terminal.Start(31, 30, 0, 57600)

PauseMSec(1000)
GoForward (AllWheels,150)
SetMaxSpeed(AllWheels,14)


repeat
' send PING sensor obstacle data
Terminal.tx(0) ' 0 indicates the beginning of a transmission
Terminal.tx(0)
range_L := ping.Inches(PING_Pin_L)
Terminal.tx(1)
Terminal.tx(range_L)
range_M := ping.Inches(PING_Pin_M)
Terminal.tx(2)
Terminal.tx(range_M)
range_R := ping.Inches(PING_Pin_R)
Terminal.tx(3)
Terminal.tx(range_R)
'speedRight := GetSpeed (RightWheel)
'Terminal.tx(4)
'Terminal.tx(speedRight.BYTE[1])
'Terminal.tx(speedRight.BYTE[0])

' receive direction data
nRX := Terminal.rxcheck
if(nRX <> 255)
nRXWheels := nRX
nRXSpeed := Terminal.rxcheck
nRXMoveUnits := Terminal.rxcheck

SetMaxSpeed(nRXWheels,nRXSpeed)
GoForward (nRXWheels,nRXMoveUnits)
PauseMSec(50)

PRI ClearPosition(Wheel)
DrvMtr.TX(CLRP+Wheel)

PRI GoForward (Wheel, Dist)
DrvMtr.TX(TRVL + Wheel)
DrvMtr.TX(Dist.BYTE[1])
DrvMtr.TX(Dist.BYTE[0])

PRI PauseMSec(Duration)
waitcnt(((clkfreq / 1_000 * Duration - 3932) #> WMin) + cnt)

PRI GetSpeed (Wheel) | SPD
DrvMtr.TX(QSPD + Wheel)
SPD := 0
SPD.BYTE[1] := DrvMtr.RX
SPD.BYTE[0] := DrvMtr.RX
Return SPD

PRI ChkForArrival (Wheel, Tollerance) | Arvd
DrvMtr.TX(CHFA + Wheel)
DrvMtr.TX(Tollerance.BYTE[0])
Arvd.BYTE[0] := DrvMtr.RX
Return Arvd
PRI WaitForArrival (Wheel, Tollerance) | Arvd
DrvMtr.TX(CHFA + Wheel)
DrvMtr.TX(Tollerance.BYTE[0])
Arvd.BYTE[0] := DrvMtr.RX
Return Arvd

PRI SetMaxSpeed (Wheel, MaxSpeed)
DrvMtr.TX(SMAX + Wheel)
DrvMtr.TX(MaxSpeed.BYTE[1])
DrvMtr.TX(MaxSpeed.BYTE[0])

Franklin

Since, in spin, indentation makes a difference you should go back and wrap your code with the CODE tags available in the advanced settings.

Tom C

RoboPanther,
You might want to take a look at the attached code from the Parallax Object Exchange as an example. Getting the Motor Controller/HB-25 combination to work as expected is no mean feat.
Regards,
TCIII

RoboPanther

Thanks Franklin and TCIII for your reply and please excuse my delayed response!
TCIII, thank you very much for the spin code! I tried it and had some encouraging results retrieving speed data. But for some reason go_Distance(20, ALL_WHEELS) makes only one wheel spin and after a while the wheel starts to go back and forth in rapid succession.

TCIII, would it be possible that you can post the how you call the library? Below is what I am using (simplified):

[FONT=Consolas][SIZE=2][FONT=Consolas][SIZE=2]CON

  _clkmode                      = xtal1 + pll16x

  _xinfreq                      = 5_000_000

  WMin                          = 381           

  

  MPC_Pin                       = 4                     ' Motor Position Controller pin

  ALL_WHEELS  = 0

  LEFT_WHEEL  = 1

  RIGHT_WHEEL = 2

OBJ

  DrvMtr  : "Wheel_Controller"                            

PUB start | i, j, k, nCounter

  DrvMtr.Open(MPC_Pin)

  DrvMtr.Start                                          ' Establish comunications to MPC

  DrvMtr.go_Distance(100, ALL_WHEELS)
[/SIZE][/FONT][/SIZE][/FONT]

Tom C

RoboPanther
The main calling program was written by Steve Norris from Norris Labs. It was written to work with a Nomad and a CrustCrawler, HB-25 and the Wheel Position Controller. The code for the Nomad is on the Robot Magazine website, but I do not know what issue it was in at the moment. I am providing a link to Steve's website and one of his projects that uses similar hardware. I suggets that you email Steve and ask him about the Nomad code.
Link to Norris Labs: http://www.norrislabs.com/Projects/RoboStool/index.html
Regards,
TCIII

RoboPanther

Thanks TCIII! I contacted Steve and hope he'll provide me the spin code. Thanks again!