Exercise 4, Problem 10

We transposed the program to smr-cl:
minDist=0.01
minAngle=0.02

krho=0.3
kalph=0.8
kbeta=-0.35

d=0.26
rR=0.035
rL=0.035
maxIter=10000

pose2x=-0.5
pose2y=-0.5
pose2th=0
i=1

log "$odox" "$odoy" "$odoth"

label "update"
deltaX=pose2x-$odox
deltaY=pose2y-$odoy
theta=$odoth
rho=sqrt(deltaX*deltaX + deltaY*deltaY)
alph=normalizeanglerad(-theta + atan2(deltaY,deltaX))
beta=normalizeanglerad(-theta - alph + pose2th)

if((krho*rho) < 0.005) "finish"

vr=krho*rho-d*(kalph*alph+kbeta*beta)
vl=krho*rho+d*(kalph*alph+kbeta*beta)
motorcmds vr vl
i=i+1

if(((rho>minDist)|(abs(beta)>minAngle))&(i<maxIter)) "update"

label "finish"
stop