following is done Webots, can you please improve the pid and wallfollowing section so that the robots moves better and more accurately.

public void set$\_$velocity$($double base, double control$)\{$

$//$ base gives the velocity of the wheels in m$/$s

$//$ control is an adjustment on the main velocity

double base$\_$av $=($base$/$this$.$WHEEL$\_$RADIUS$)$;

double lv $=$ base$\_$av;

double rv $=$ base$\_$av;

if $($control $!=0)\{$

double control$\_$av $=($control$/$this$.$WHEEL$\_$RADIUS$)$;

$//$ Check if we exceed max velocity and compensate

double correction $=1$;

lv $=$ base$\_$av $-$ control$\_$av;

rv $=$ base$\_$av $+$ control$\_$av;

if $($lv $>$ this.max$\_$vel$)\{$

correction $=$ this.max$\_$vel $/$ lv;

lv $=$ lv $*$ correction;

rv $=$ rv $*$ correction;

$\}$

if $($rv $>$ this.max$\_$vel$)\{$

correction $=$ this.max$\_$vel $/$ rv;

lv $=$ lv $*$ correction;

rv $=$ rv $*$ correction;

$\}$

$\}$

this.left$\_$motor.setVelocity$($lv$)$;

this.right$\_$motor.setVelocity$($rv$)$;

$\}$

private double pid$($double error$)\{$

double kp $=0\mathrm{.}5$; $//$ proportional weight $($may need tuning$)$

double kd $=3\mathrm{.}0$; $//$ differential weight $($may need tuning$)$

double ki $=0\mathrm{.}00$; $//$ integral weight $($may need tuning$)$

double prop $=$ error;

double diff $=$ error $-$ this.prev$\_$error;

this.total$\_$error $+=$ error;

double control $=($kp $*$ prop$)+($ki $*$ this.total$\_$error$)+($kd $*$ diff$)$;

this.prev$\_$error $=$ error;

return control;

$\}$

public void follow$\_$wall$($double robot$\_$linearvelocity, double desired$\_$distance$\_$from$\_$wall, boolean right$)\{$

int direction$\_$coeff $=1$;

double error;

double control;

double wall$\_$dist

if $($right$)$ direction$\_$coeff $=-1$; $//$ invert the values for the control

$//$ Check if the robot is too close to the wall

if $($Math$.$min$($this$.$prox$\_$sensors$.$get$\_$value$(1),$

Math.min$($this$.$prox$\_$sensors$.$get$\_$value$(2),$

Math.min$($this$.$prox$\_$sensors$.$get$\_$value$(3),$

Math.min$($this$.$prox$\_$sensors$.$get$\_$value$(4),$

Math.min$($this$.$prox$\_$sensors$.$get$\_$value$(5),$

this.prox$\_$sensors$.$get$\_$value$(6))))))<$ desired$\_$distance$\_$from$\_$wall$)\{$

this.set$\_$velocity$($robot$\_$linearvelocity $/3,-0\mathrm{.}1*$ direction$\_$coeff$)$; $//$ Reduce turning speed

$\}$ else $\{$

$//$ Determine the closest wall distance

if $(!$right$)$ wall$\_$dist $=$ Math.min$($this$.$prox$\_$sensors$.$get$\_$value$(1),$

this.prox$\_$sensors$.$get$\_$value$(0))$;

else wall$\_$dist $=$ Math.min$($this$.$prox$\_$sensors$.$get$\_$value$(7),$

this.prox$\_$sensors$.$get$\_$value$(8))$;

$//$ Check if the robot is running approximately parallel to the wall

if $($wall$\_$dist $<$ this.prox$\_$sensors$.$get$\_$maxRange$())\{$

$//$ Calculate the error and control using PID

error $=$ wall$\_$dist $-$ desired$\_$distance$\_$from$\_$wall;

control $=$ this.pid$($error$)$;

$//$ Adjust for right wall

this.set$\_$velocity$($robot$\_$linearvelocity, control $*$ direction$\_$coeff $*0\mathrm{.}5)$; $//$ Reduce turning speed

$\}$ else $\{$

$//$ No wall detected, so turn

this.set$\_$velocity$($robot$\_$linearvelocity, $0\mathrm{.}04*$ direction$\_$coeff$)$; $//$ Reduce turning speed

$\}$

$\}$

this.state $=$ MoveState.FOLLOW$\_$WALL;

$\}$