private void autoAim(double now, boolean allow_changing_tracks) {
List<ShooterAimingParameters> aimingParameters = getCurrentAimingParameters(now);
if (aimingParameters.isEmpty() && (allow_changing_tracks || mTurretManualSetpoint != null)) {
// Manual search
if (mTurretManualSetpoint != null) {
mTurret.setDesiredAngle(mTurretManualSetpoint.getTurretAngle());
mHood.setDesiredAngle(Rotation2d.fromDegrees(getHoodAngleForRange(mTurretManualSetpoint.range)));
} else {
mTurret.setOpenLoop(mTurretManualScanOutput);
if (!mTuningMode) {
mHood.setDesiredAngle(Rotation2d.fromDegrees(Constants.kHoodNeutralAngle));
} else {
mHood.setOpenLoop(mHoodManualScanOutput);
}
}
mFlywheel.setRpm(Constants.kFlywheelGoodBallRpmSetpoint);
} else {
// Pick the target to aim at
boolean has_target = false;
for (ShooterAimingParameters param : aimingParameters) {
double turret_angle_degrees = param.getTurretAngle().getDegrees();
if (turret_angle_degrees >= Constants.kSoftMinTurretAngle
&& turret_angle_degrees <= Constants.kSoftMaxTurretAngle
&& param.getRange() >= Constants.kAutoAimMinRange
&& param.getRange() <= Constants.kAutoAimMaxRange
&& (allow_changing_tracks || mCurrentTrackId == param.getTrackid())) {
// This target works
mFlywheel.setRpm(getShootingSetpointRpm(param.getRange()));
if (!mTuningMode) {
double angle_degrees = getHoodAngleForRange(param.getRange()) + mHoodAdjustment;
angle_degrees = Math.max(angle_degrees, Constants.kMinHoodAngle);
angle_degrees = Math.min(angle_degrees, Constants.kMaxHoodAngle);
mHood.setDesiredAngle(Rotation2d.fromDegrees(angle_degrees));
} else {
mHood.setOpenLoop(mHoodManualScanOutput);
}
mTurret.setDesiredAngle(param.getTurretAngle());
mCurrentAngleForLogging = param.getTurretAngle().getDegrees();
mCurrentRangeForLogging = param.getRange();
mCurrentTrackId = param.getTrackid();
has_target = true;
break;
}
}
if (!has_target) {
mCurrentTrackId = -1;
}
}
}