can you please add completed bug $2$ algorith to the following code. import com.cyberbotics.webots.controller.Motor;

import com.cyberbotics.webots.controller.Supervisor;

import com.cyberbotics.webots.controller.Node;

public class PioneerNav$2\{$

public static enum MoveState $\{$

STOP,

FORWARD,

ARC,

FOLLOW$\_$WALL $\}$;

private Supervisor robot; $//$ reference to the robot

private Node robot$\_$node; $//$ reference to the robot node

private Pose robot$\_$pose; $//$ the robots percieved pose

private Motor left$\_$motor;

private Motor right$\_$motor;

private double max$\_$vel;

private double prev$\_$error;

private double total$\_$error;

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

private MoveState state;

private final double WHEEL$\_$RADIUS $=0\mathrm{.}96$; $//$ in meters $-$ found using CONFIGURE

private final double AXEL$\_$LENGTH $=0\mathrm{.}5$; $//$ in meters $-$ found using CONFIGURE

$//==================================================================================$

$//$ Constructor

$//==================================================================================$

public PioneerNav$2($Supervisor robot, Pose init$\_$pose,PioneerProxSensors$1$ ps$)\{$

this.prox$\_$sensors $=$ ps;

this.robot $=$ robot;

this.robot$\_$node $=$ this.robot.getSelf$()$; $//$ reference to the robot node

this.state $=$ MoveState.STOP;

$//$ enable motors

this.left$\_$motor $=$ robot.getMotor$("$left wheel"$)$;

this.right$\_$motor $=$ robot.getMotor$("$right wheel"$)$;

this.left$\_$motor.setPosition$($Double$.$POSITIVE$\_$INFINITY$)$;

this.right$\_$motor.setPosition$($Double$.$POSITIVE$\_$INFINITY$)$;

this.max$\_$vel $=$ this.left$\_$motor.getMaxVelocity$()-0\mathrm{.}1$; $//$ Fudge: just under max vel

int timeStep $=($int$)$ Math.round$($robot$.$getBasicTimeStep$())$;

this.max$\_$vel $=$ this.left$\_$motor.getMaxVelocity$()-0\mathrm{.}1$; $//$

$//$ set up pose

this.robot$\_$pose $=$ new Pose$($init$\_$pose.getX$(),$ init$\_$pose.getY$(),$ init$\_$pose.getTheta$())$;

$//$ Initialise motor velocity

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

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

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

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

$\}$

$//$ The following method only works if in supervisor mode

public Pose get$\_$real$\_$pose$()\{$

if $($this$.$robot$\_$node $==$ null$)$

return new Pose$(0,0,0)$;

double$[]$ realPos $=$ robot$\_$node.getPosition$()$;

double$[]$ rot $=$ this.robot$\_$node.getOrientation$()$; $//3$x$3$ Rotation matrix as vector of length $9$

double theta$1=$ Math.atan$2(-$rot$[0],$ rot$[3])$;

double halfPi $=$ Math.PI$/2$;

double theta$2=$ theta$1+$ halfPi;

if $($theta$1>$ halfPi$)$

theta$2=-(3*$halfPi$)+$theta$1$;

return new Pose$($realPos$[0],$ realPos$[1],$ theta$2)$;

$\}$

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 MoveState getState$()\{$

return this.state;

$\}$

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

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;

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

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

$//$ Check and limit velocity to avoid exceeding maximum velocity

double correction $=1\mathrm{.}0$;

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

double maxVel $=$ Math.max$($lv$,$ rv$)$;

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

lv $*=$ correction;

rv $*=$ correction;

$\}$

$\}$

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

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

$\}$

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

public void follow$\_$wall$\{\}$