diff --git a/src/main/java/frc/subsystem/Superstructure.java b/src/main/java/frc/subsystem/Superstructure.java index 1e04423..6b86e41 100644 --- a/src/main/java/frc/subsystem/Superstructure.java +++ b/src/main/java/frc/subsystem/Superstructure.java @@ -1,5 +1,13 @@ package frc.subsystem; +import edu.wpi.first.wpilibj.Filesystem; +import frc.subsystem.shooter.Shooter; + +import java.io.BufferedReader; +import java.io.FileReader; +import java.io.IOException; +import java.util.HashMap; +import java.util.Map; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants; @@ -13,7 +21,6 @@ import frc.utility.MathUtil; import org.littletonrobotics.junction.Logger; - public class Superstructure extends AbstractSubsystem { //unfortunately all of these need to be static so that the enum can access them private static Arm arm; @@ -250,4 +257,76 @@ public static Superstructure getSuperstructure() { public Rotation2d getTargetAngle() { return targetAngle; } + + private String path = Filesystem.getDeployDirectory().getPath(); // need to append exact location of CSV file + HashMap map = new HashMap(); + + public void toHashMap() { + + + String line = ""; + String splitBy = ","; + try { + BufferedReader br = new BufferedReader(new FileReader(path)); + br.readLine(); + while ((line = br.readLine()) != null) { + String[] fields = line.split(splitBy); + + String distanceDirectionHeight = fields[0]; + String location = fields[1]; + double shooterAngle = Double.parseDouble(fields[2]); + double shooterVelocity = Double.parseDouble(fields[3]); + + ShooterConfiguration shooterConfiguration = new ShooterConfiguration(location, shooterAngle, shooterVelocity); + map.put(distanceDirectionHeight, shooterConfiguration); + + } + + br.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + + + private String convertToKey(double distance, String direction, String height) { + return distance + direction + height; + + } + + public String getLocationOfRobot(double distance, String direction, String height){ + return map.get(convertToKey(distance, direction, height)).getLocation(); + } + public double getShooterAngle(double distance, String direction, String height){ + return map.get(convertToKey(distance, direction, height)).getShooterVelocity(); + } + public double getShooterVelocity(double distance, String direction, String height){ + return map.get(convertToKey(distance, direction, height)).getShooterAngle(); + } + + static class ShooterConfiguration { + private String location; + private double shooterAngle; + private double shooterVelocity; + + public ShooterConfiguration(String location, double shooterAngle, double shooterVelocity) { + this.location = location; + this.shooterAngle = shooterAngle; + this.shooterVelocity = shooterVelocity; + } + + public String getLocation() { + return location; + } + + public double getShooterAngle() { + return shooterAngle; + } + + public double getShooterVelocity() { + return shooterVelocity; + } + + } } \ No newline at end of file