# speed vars var speed #var target_speed var left var right # mean and vari(ance) calculation vars var mini var maxi var mean var vari # actual value and n(ormalized)dev(iation) var p1 var ndev # pi controller vars var ireg var preg var pcoeff var icoeff #onevent startup speed=150 # mini=1024 maxi=0 mean=512 vari=512 # pcoeff=60 icoeff=33 ireg=0 onevent prox p1=prox.ground.delta[1] callsub statistics call math.muldiv(ndev,SIGMA,p1-mean,vari) if abs(ndev)<2*SIGMA then # Am I still on the edge? call leds.top(0,0,0) # pi controller preg=(pcoeff*ndev)/100 ireg+=(icoeff*preg)/100 # calc speed of motors left=speed+(preg+ireg) right=speed-(preg+ireg) else # I'm outside, help! call leds.top(32,0,0) ireg=0 left=ndev right=-ndev end #pc+=1 callsub set_motor sub statistics call math.max(maxi,maxi,p1) # find max p1 -> maxi call math.min(mini,mini,p1) # find min p1 -> mini if maxi-mini>400 then call math.muldiv(vari,45,maxi-mini,100) mean=(mini+maxi)/2 end sub set_motor emit motors [-100,100,0,ireg,ndev] motor.left.target=left motor.right.target=right