please implement a obstacle avoidance algorithm here.

navigator class

public static enum MoveState $\{$

STOP,

FORWARD,

ARC,

CONFIGURE,

FOLLOW$\_$WALL $\}$

$//$ more private class

private PioneerProxSensors$1$ prox$\_$sensors;

$//////////////////////////////$

more code her

$/////////////////////////////$

public int forward$($double target$\_$dist, double robot$\_$linearvelocity$)\{$

double wheel$\_$av $=($robot$\_$linearvelocity$/$this$.$WHEEL$\_$RADIUS$)$;

double target$\_$time $=$ target$\_$dist$/$robot$\_$linearvelocity;

this.left$\_$motor.setVelocity$($wheel$\_$av$)$;

this.right$\_$motor.setVelocity$($wheel$\_$av$)$;

this.state $=$ MoveState.FORWARD;

$//$ return target$\_$time as millisecs

return $($int$)(1000\mathrm{.}0*$target$\_$time$)$;

$\}$

public int arc$($double icr$\_$angle, double icr$\_$r$,$ double icr$\_$omega$)\{$

double target$\_$time $=$ icr$\_$angle $/$ icr$\_$omega;

$//$ Calculate each wheel velocity around ICR

double vl $=$ icr$\_$omega $*($icr$\_$r $-($this$.$AXEL$\_$LENGTH $/2))$;

double vr $=$ icr$\_$omega $*($icr$\_$r $+($this$.$AXEL$\_$LENGTH $/2))$;

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

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

this.left$\_$motor.setVelocity$($leftwheel$\_$av$)$;

this.right$\_$motor.setVelocity$($rightwheel$\_$av$)$;

this.state $=$ MoveState.ARC;

$//$ return target$\_$time as millisecs

return $($int$)(1000\mathrm{.}0*$target$\_$time$)$;

$\}$

public void stop$()\{$

this.left$\_$motor.setVelocity$(0\mathrm{.}0)$;

this.right$\_$motor.setVelocity$(0\mathrm{.}0)$;

this.state $=$ MoveState.STOP;

$\}$

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$)$;

$\}$

public MoveState getState$()\{$

return this.state;

$\}$

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

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

double kd $=3$; $//$ differential weight $($may need tuning$)$

double ki $=0\mathrm{.}0475$; $//$ 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 set$\_$point, boolean right$)\{$

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

double error;

double control;

double wall$\_$dist;

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

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))))))<$ set$\_$point$)$

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

else $\{$

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))$;

$//$ Running aproximately parallel to the wall

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

error $=$ wall$\_$dist $-$ set$\_$point;

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

$//$ adjust for right wall

this.set$\_$velocity$($robot$\_$linearvelocity, control$*$direction$\_$coeff$)$;

$\}$ else $\{$

$//$ No wall, so turn

this.set$\_$velocity$($robot$\_$linearvelocity, $0\mathrm{.}08*$direction$\_$coeff$)$;

$\}$

$\}$

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

$\}$

controller class

double time$\_$elapsed $=0$;

double target$\_$time $=0$;

double robot$\_$velocity $=0\mathrm{.}3$;

$//$ define schedule

PioneerNav$2.$MoveState$[]$ schedule $=\{$ PioneerNav$2.$MoveState.FOLLOW$\_$WALL$\}$;

int schedule$\_$index $=-1$; $//$ we increment before selecting the current action

PioneerNav$2.$MoveState state; $//$ current state

$//$ set up display

String display$\_$action $=""$;

if $($schedule$[0]==$ PioneerNav$2.$MoveState.CONFIGURE$)\{$

schedule$\_$index$++$;

nav.configure$\_$initialise$\_$parameters$(2*$Math$.$PI$)$;

$\}$

while $($robot$.$step$($timeStep$)!=-1)\{$

robot$\_$pose $=$ nav.get$\_$real$\_$pose$()$;

prox$\_$sensors$.$set$\_$pose$($robot$\_$pose$)$;

etc