From 56f4e09f8c134ae956172fcc201b4030923fc7b7 Mon Sep 17 00:00:00 2001 From: Sebastian <66741@psdschools.org> Date: Sat, 22 Feb 2020 15:34:10 -0700 Subject: [PATCH] Added the CSV reading things --- src/main/deploy/Robot Data - Angles.csv | 6 + src/main/deploy/Robot Data - Distances.csv | 6 + .../java/frc4388/robot/RobotContainer.java | 1 - .../frc4388/robot/subsystems/Shooter.java | 15 ++ .../java/frc4388/utility/ShooterTables.java | 149 ++++++++++++++++++ 5 files changed, 176 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/Robot Data - Angles.csv create mode 100644 src/main/deploy/Robot Data - Distances.csv create mode 100644 src/main/java/frc4388/utility/ShooterTables.java diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv new file mode 100644 index 0000000..273d4ba --- /dev/null +++ b/src/main/deploy/Robot Data - Angles.csv @@ -0,0 +1,6 @@ +Angle (deg),Displacement (deg) +-20,-5 +-10,-2 +0,0 +10,2 +20,5 \ No newline at end of file diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv new file mode 100644 index 0000000..e058b92 --- /dev/null +++ b/src/main/deploy/Robot Data - Distances.csv @@ -0,0 +1,6 @@ +Distance (in),Hood Ext. (u),Drum Velocity (u/ds) +21,10,10000 +100,23,11000 +200,30,14000 +300,56,17000 +480,100,20000 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b82cda6..c9a4ff8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,7 +13,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.WaitCommand; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index a18aca5..a62be93 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.ShooterTables; import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { @@ -48,6 +49,8 @@ public class Shooter extends SubsystemBase { double velP; double input; + + ShooterTables m_shooterTable; /* * Creates a new Shooter subsystem. @@ -71,6 +74,18 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterTable = new ShooterTables(); + + SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); + SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); + SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); + SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + + SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); + SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); + SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); + SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); } @Override diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java new file mode 100644 index 0000000..638f140 --- /dev/null +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -0,0 +1,149 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import java.io.BufferedReader; +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.IOException; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Add your docs here. + */ +public class ShooterTables { + double[][] m_distance = new double[50][3]; + double[][] m_angle = new double[50][2]; + + final int m_columnDistance = 0; + final int m_columnHood = 1; + final int m_columnVel = 2; + final int m_columnAngle = 0; + final int m_columnDisplacement = 1; + + int m_distanceLength; + int m_angleLength; + + public ShooterTables() { + int lineNum = 0; + + File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv"); + File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv"); + + try { + + + BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV)); + BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV)); + String line = ""; + + while ((line = distanceReader.readLine()) != null) { + + if (lineNum == 0) { + lineNum ++; + } else { + String[] values = line.split(","); + + m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); + m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); + m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); + + lineNum ++; + } + + } + + m_distanceLength = lineNum-1; + lineNum = 0; + + while ((line = angleReader.readLine()) != null) { + + if (lineNum == 0) { + lineNum ++; + } else { + String[] values = line.split(","); + + m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]); + m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]); + + lineNum ++; + } + } + + m_angleLength = lineNum-1; + + } catch (FileNotFoundException e) { + e.printStackTrace(); + } catch (IOException e) { + e.printStackTrace(); + } + SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); + SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); + SmartDashboard.putNumber("m_distanceLength", m_distanceLength); + SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); + SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); + SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); + } + + public double getHood(double distance) { + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnHood]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnHood, m_distance); + } + } + + public double getVelocity(double distance) { + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnVel]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnVel, m_distance); + } + } + + public double getAngleDisplacement(double angle) { + int i = 0; + while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) { + i ++; + } + if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) { + return m_angle[i][m_columnDisplacement]; + } else { + if (i >= m_angleLength) { + i = m_angleLength - 1; + } + return linearInterpolation(i, angle, m_columnDisplacement, m_angle); + } + } + + public double linearInterpolation(int i, double value, int column, double[][] table) { + if (i != 0) { + double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]); + value = slope * (value - table[i-1][0]) + table[i-1][column]; + return value; + } + return 0.0; + } +} \ No newline at end of file