public void onScannedRobot(ScannedRobotEvent e) {
setTurnRight(Util.normalRelativeAngle(e.getBearing() + 90));
double x = getX();
double y = getY();
double angleUR = Math.atan(y / (getBattleFieldWidth() - x));
double angleLR = Math.atan((getBattleFieldHeight() - y) / (getBattleFieldWidth() - x));
double angleLL = Math.atan((getBattleFieldHeight() - y) / x);
double angleUL = Math.atan(y / x);
double heading = getHeading();
double axis;
if ((heading > angleUL && heading <= angleUR) || (heading > angleLR && heading <= angleLL)) {
axis = y;
} else {
axis = x;
}
double aheadDistanceToWall = (axis / Math.cos(heading)) - 36;
double backDistanceToWall = (axis / Math.cos(Util.normalAbsoluteAngle(heading - 180))) - 36;
if (aheadDistanceToWall > 100) {
setAhead(100 - 18);
} else if (backDistanceToWall > 100) {
setBack(100 - 18);
}
}
--AaronR