protected boolean updateVision() { boolean reslt = false; // Get the values we need from current vision results try { final NumberArray targetNum = new NumberArray(); ntserver.retrieveValue("BLOBS", targetNum); if (targetNum.size() > 0) { double blobx = targetNum.get(0); double bloby = targetNum.get(1); // Compute firing angle double baseCameraAngle = Robot.arm.getRawEncoder(); double targetInclineAngle = baseCameraAngle + vFoV * (bloby - ImageH / 2) * ImageH; double cameraHeight = CameraPivotHeight + CameraArmLength * Math.sin(Math.toRadians(baseCameraAngle)); double towerRange = (TargetHeight - cameraHeight) / Math.tan(Math.toRadians(targetInclineAngle)); double firingAngle = Math.toDegrees(Math.atan(Vfire2 / (G * towerRange) - Math.sqrt( Vfire2 * (Vfire2 - 2.0 * G * (TargetHeight - cameraHeight)) / (G * G * towerRange * towerRange) - 1))); Elevate(firingAngle); // Compute turn angle double targetTurnAngle = FoV * (blobx - ImageW / 2) * ImageW; Turn(targetTurnAngle); reslt = true; } } catch (TableKeyNotDefinedException exp) { } return reslt; }
/** * {@inheritDoc} */ public void initTable(ITable subtable) { m_table = subtable; commands = new StringArray(); ids = new NumberArray(); toCancel = new NumberArray(); m_table.putValue("Names", commands); m_table.putValue("Ids", ids); m_table.putValue("Cancel", toCancel); }