the following code in done in WeBots and written in java. Can you imrpove the PID wallFollowing section. 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{.}0957$; $//$ in meters $-$ found using CONFIGURE

private final double AXEL$\_$LENGTH $=0\mathrm{.}323$; $//$ 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$)\{$

$//$ 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$; $//$ prop