Change initial position error between runs
tmax=20; dt=.02; umax=4; //drive limit InitGraph(); ScaleWindow(-.5,-1,speed*tmax,1); DrawAxes(); Colour(Black); Label('Limited steering rate, control with constraints',1,.9); Label('Position, scale=1m',1.2,.8); Colour(Red); Label('Angle',1,.7); Colour(Green); Label('Steering',1,.6); Colour(Black); Label('Distance',1,-.2); for (i=5;i
//function RunModel() dist=0; while(t
anglim){angdemand=anglim;} if (angdemand<-anglim){angdemand=-anglim;} steertarget = gain2 * (angdemand - angseen); // steertarget = lim(gain2 * (angdemand - angseen), smax); if (steertarget>smax){steertarget=smax;} if (steertarget<-smax){steertarget=-smax;} dx = angle * speed; da = (steering + szero) * speed / wbase; ds = srate * ((steertarget - steering>0) ? 1:-1); //sign dist = dist + speed * dt; x = x + dx * dt; angle = angle + da * dt; steering = steering + ds * dt; t=t+dt; Colour(Black); LineStart(dist,x); Colour(Red); LineStart(dist, (wbase + focus) * angle); Colour(Green); LineStart(dist, steering / smax); }