SHOOTER WORKS SEMI MANUAL

src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java

@@ -40,8 +40,6 @@ public class CalibrateShooter extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
     m_shooterHood.m_angleEncoder.setPosition(0);
     m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
 
@@ -54,9 +52,6 @@ public class CalibrateShooter extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-
     m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
     m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
   }

src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java

@@ -31,9 +31,10 @@ public class HoodPositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
-    double b = ShooterConstants.HOOD_CONVERT_B;
-    firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
+    //double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
+    //double b = ShooterConstants.HOOD_CONVERT_B;
+    //firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
+    firingAngle = m_shooterHood.addFireAngle();
     //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
     //SmartDashboard.putNumber("Fire Angle", firingAngle);
     m_shooterHood.runAngleAdjustPID(firingAngle);

src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java

@@ -29,8 +29,8 @@ public class ShootFireGroup extends ParallelRaceGroup {
     addCommands(
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
       new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
-      new TrackTarget(m_shooterAim),
-      new StorageRun(m_storage)
+      new TrackTarget(m_shooterAim)
+      //new StorageRun(m_storage)
     );
   }
 }

src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java

@@ -31,7 +31,6 @@ public class ShootPrepGroup extends ParallelDeadlineGroup {
       new ShooterVelocityControlPID(m_shooter),
       new HoodPositionPID(m_shooterHood),
       new StoragePrepAim(m_storage)
-      //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
     );
   }
 }

src/main/java/frc4388/robot/commands/shooter/TrackTarget.java

@@ -88,15 +88,18 @@ public class TrackTarget extends CommandBase {
       double xVel = (distance * VisionConstants.GRAV) / (yVel);
 
       //fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
-      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
+      //fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
         //END Equation Code
 
         //START CSV Code
       fireVel = m_shooter.m_shooterTable.getVelocity(distance);
-      //fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
+      fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
       //fireAngle = 33;
         //END CSV Code
 
+      //fireVel = SmartDashboard.getNumber("Velocity Target", 0);
+      //fireAngle = SmartDashboard.getNumber("Angle Target", 3);
+
       
       m_shooter.m_fireVel = fireVel;
       m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;

src/main/java/frc4388/robot/commands/storage/StorageIntake.java

@@ -38,7 +38,7 @@ public class StorageIntake extends CommandBase {
   @Override
   public void execute() {
 
-    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
+    if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
       intook = true;
     }

src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java

@@ -34,7 +34,7 @@ public class StoragePrepAim extends CommandBase {
   @Override
   public void execute() {
     if (m_storage.getBeam(1)){
-      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+      //m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
       m_storage.runStorage(0);
@@ -49,12 +49,13 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
+    /*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
       m_storage.m_isStorageReadyToFire = true;
       return true;
     } else {
       m_storage.m_isStorageReadyToFire = false;
       return false;
-    }
+    }*/
+    return true;
   }
 }

src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java

@@ -55,7 +55,7 @@ public class StoragePrepIntake extends CommandBase {
   @Override
   public boolean isFinished() {
     if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
-      return true;
+      return false;
     }
     return false;
   }