double aheadDistanceToWall? = (axis / Math.cos(Util.normalAbsoluteAngle?(heading + 90))) - 36; double backDistanceToWall? = (axis / Math.cos(Util.normalAbsoluteAngle?(heading - 90))) - 36; |
double aheadDistanceToWall? = (axis / Math.cos(heading)) - 36; double backDistanceToWall? = (axis / Math.cos(Util.normalAbsoluteAngle?(heading - 180))) - 36; |
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