stuff happened

src/main/java/frc4388/robot/Constants.java

@@ -4,6 +4,8 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 
@@ -80,4 +82,13 @@ public final class Constants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
   }
+  public static final class ShooterConstants {
+    /* PID Constants Shooter */
+    public static final int SHOOTER_TIMEOUT_MS = 30;
+    public static final int SHOOTER_SLOT_IDX = 0;
+    public static final int SHOOTER_PID_LOOP_IDX = 1;
+    public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
+
+    
+  }
 }

src/main/java/frc4388/robot/RobotContainer.java

@@ -10,8 +10,12 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.Vision;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
@@ -40,7 +44,10 @@ public class RobotContainer {
   );
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
-
+  private final BoomBoom m_robotBoomBoom = new BoomBoom();
+  private final Hood m_robotHood = new Hood();
+  private final Turret m_robotTurret = new Turret();
+  private final Vision m_robotVison = new Vision();
   /* Controllers */
   private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);

src/main/java/frc4388/robot/RobotMap.java

@@ -90,6 +90,9 @@ public class RobotMap {
     //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    /*Boom Boom Subsystem*/
+    
   }
 
+
 }

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.ShooterTables;
+
+public class BoomBoom extends SubsystemBase {
+public WPI_TalonFX m_shooterFalconLeft;
+public WPI_TalonFX m_shooterFalconRight;
+public ShooterTables m_shooterTable;
+
+  /** Creates a new BoomBoom. */
+  public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
+  m_shooterFalconLeft = shooterFalconLeft;
+  m_shooterFalconRight = shooterFalconRight;
+  
+    int closedLoopTimeMs = 1;
+      //LEFT FALCON
+      m_shooterFalconLeft.configFactoryDefault();
+      m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
+      m_shooterFalconLeft.setInverted(true);
+      m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      
+      //RIGHT FALCON
+      m_shooterFalconRight.configFactoryDefault();
+      m_shooterFalconRight.setNeutralMode(NeutralMode.Coast);
+      m_shooterFalconRight.setInverted(false);
+      m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
+      m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+      
+      m_shooterTable = new ShooterTables();
+  
+  
+  }
+    
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    // Abhi was here 
+  }
+public void runDrumShooter(double speed) {
+    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
+    m_shooterFalconRight.follow(m_shooterFalconLeft);
+  }    
+
+
+  public void runDrumShooterVelocityPID(double targetVel) {
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init
+    m_shooterFalconRight.follow(m_shooterFalconLeft);
+    }
+}

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Hood extends SubsystemBase {
+  /** Creates a new Hood. */
+  public Hood() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Turret extends SubsystemBase {
+  /** Creates a new Turret. */
+  public Turret() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Vision extends SubsystemBase {
+  /** Creates a new Vision. */
+  public Vision() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc4388/utility/ShooterTables.java

@@ -0,0 +1,164 @@
+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][4];
+    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_columnCenterDisplacement = 3;
+    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]);
+                    m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]);
+                    
+                    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;
+
+            distanceReader.close();
+            angleReader.close();
+
+        } 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) { //Rotations of motor
+        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) { //Units per 100ms
+        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 getCenterDisplacement(double distance) { //Degrees of Lime FOV
+        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_columnCenterDisplacement];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnCenterDisplacement, 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;
+    }
+}