diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index d55f4e8..e46b6cf 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -106,12 +106,18 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - // targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - targetAngle = 0; + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + // if (error > 180) { - // error = 360 - error; // TODO: error - 360 - // this.inverted = -1; } else { this.inverted = 1; } + // error = error - 360; + // this.inverted = -1; + // } else { + // this.inverted = 1; + // } + if (error > 180) { error = error - 360; } + isAimedInTolerance = (Math.abs(error) <= tolerance); } @@ -122,7 +128,7 @@ public class Shoot extends CommandBase { startTime = 0; this.odoX = 0.9398;//-this.swerve.getOdometry().getY(); // 3.2766 - this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // 0.9398 + this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // -0.9398 this.distance = Math.hypot(odoX, odoY); @@ -151,7 +157,7 @@ public class Shoot extends CommandBase { this.integral = integral + (error * Constants.LOOP_TIME); this.derivative = (error - prevError) / Constants.LOOP_TIME; this.output = kP * proportional + kI * integral + kD * derivative; - this.normOutput = (output / 360) * inverted; + this.normOutput = (output / 360); // inverted; } // Called every time the scheduler runs while the command is scheduled. @@ -165,7 +171,6 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Normalized Output", this.normOutput); this.turret.runTurretWithCustomPID(normOutput); - // this.turret.m_boomBoomRotateMotor.set(normOutput); this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative if (this.toShoot) { diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 79f2a11..3c83331 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; @@ -34,29 +35,18 @@ public class TrackTarget extends CommandBase { BoomBoom m_boomBoom; Hood m_hood; - // use odometry to find x and y later - double x; - double y; - double distance; - double vel; - double hood; - double average; - double output; - Pose2d pos = new Pose2d(); + static double velocity; + static double hoodPosition; + ArrayList points = new ArrayList<>(); private boolean targetLocked = false; private double velocityTolerance = 100.0; private double hoodTolerance = 5.0; - double m=0; - double b=0; boolean isExecuted = false; - // public static Gains m_aimGains; - public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - // Use addRequirements() here to declare subsystem dependencies. m_swerve = swerve; m_turret = turret; m_boomBoom = boomBoom; @@ -69,81 +59,57 @@ public class TrackTarget extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - x = 0; - y = 0; + velocity = 0; + hoodPosition = 0; } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); - SmartDashboard.putBoolean("Target Locked", this.targetLocked); - + public void execute() { try { m_visionOdometry.setLEDs(true); + points = m_visionOdometry.getTargetPoints(); - points = filterPoints(points); - Point average = VisionOdometry.averagePoint(points); - DesmosServer.putPoint("average", average); - for(int i = 0; i < points.size(); i++) { - DesmosServer.putPoint("Point" + i, points.get(i)); - } - - output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2; - // //output *= 0.5; - // //DesmosServer.putDouble("output", output); + m_turret.runTurretWithInput(output); double position = m_turret.m_boomBoomRotateEncoder.getPosition(); if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) - m_swerve.driveWithInput(0, 0, output, false); + m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); + else + m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), + RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), + true); - double y_rot = average.y / VisionConstants.LIME_VIXELS; - y_rot *= Math.toRadians(VisionConstants.V_FOV); - y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; - y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + double regressedDistance = getDistance(average.y); - double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); - DesmosServer.putDouble("distance", distance); + // ! add 30 to the distance to get in target. May need to be adjusted + velocity = m_boomBoom.getVelocity(regressedDistance + 30); + hoodPosition = m_boomBoom.getHood(regressedDistance + 30); - updateRegressionDesmos(); - double regressedDistance = distanceRegression(distance); - regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; - DesmosServer.putDouble("distanceReg", regressedDistance); - - //Vision odometry circle fit based pose estimate - // Point targetOffset = m_visionOdometry.getTargetOffset(); - // DesmosServer.putPoint("targetOff", targetOffset); - - vel = m_boomBoom.getVelocity(regressedDistance + 30); - hood = m_boomBoom.getHood(regressedDistance + 30); - // m_boomBoom.runDrumShooter(vel); - m_boomBoom.runDrumShooterVelocityPID(vel); - m_hood.runAngleAdjustPID(hood); + m_boomBoom.runDrumShooterVelocityPID(velocity); + m_hood.runAngleAdjustPID(hoodPosition); double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); double currentHood = this.m_hood.getEncoderPosition(); - this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance); + targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance); SmartDashboard.putNumber("Regressed Distance", regressedDistance); - SmartDashboard.putNumber("Distance", distance); - SmartDashboard.putNumber("Hood Target Angle Track", hood); - SmartDashboard.putNumber("Vel Target Track", vel); - - // isExecuted = true; - } - catch (Exception e){ + // SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); + SmartDashboard.putNumber("Vel Target Track", velocity); + SmartDashboard.putBoolean("Target Locked", targetLocked); + } catch (Exception e){ e.printStackTrace(); - // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } - // m_turret.runshooterRotatePID(m_targetAngle); } public ArrayList filterPoints(ArrayList points) { @@ -166,15 +132,22 @@ public class TrackTarget extends CommandBase { return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); } - public final double distanceRegression(double distance) { - return (79.6078 * Math.pow(1.01343, distance) - 56.6671); + public final double getDistance(double averageY) { + double y_rot = averageY / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + + double regressedDistance = distanceRegression(distance); + regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; + + return regressedDistance; } - public void updateRegressionDesmos() { - m = DesmosServer.readDouble("m"); - b = DesmosServer.readDouble("b"); - - DesmosServer.putArray("MB", m, b); + public final double distanceRegression(double distance) { + return (79.6078 * Math.pow(1.01343, distance) - 56.6671); } // Called once the command ends or is interrupted.