attempt at fix all

notes: failed
2026-06-08 16:28:01 -06:00 · 2020-03-02 21:03:39 -07:00
parent a6e5103193
commit ddf18c3a93
14 changed files with 100 additions and 105 deletions
@@ -1,6 +1,3 @@
 Angle (deg),Displacement (deg)
-20,-5
-10,-2
 0,0
-10,2
-20,5
+0,0
@@ -1,6 +1,4 @@
 Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
-21,10,10000
-100,23,11000
-200,30,14000
-300,56,17000
-480,100,20000
+21,3,10000
+262,30,15000
+492,30,15000
@@ -121,8 +121,8 @@ public final class Constants {
        public static final int SHOOTER_SLOT_IDX = 0;
        public static final int SHOOTER_PID_LOOP_IDX = 1;
        public static final int SHOOTER_TIMEOUT_MS = 30;
-        //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
-        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0);
+        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
+        //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0);
        public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 1.0);
        public static final double SHOOTER_TURRET_MIN = -1.0;
@@ -139,7 +139,7 @@ public final class Constants {
        public static final int HOOD_DOWN_SOFT_LIMIT = 3;
        public static final double HOOD_CONVERT_SLOPE = 0.47;
        public static final double HOOD_CONVERT_B = 40.5;
-        public static final double HOOD_CALIBRATE_SPEED = 0.1;
+        public static final double HOOD_CALIBRATE_SPEED = 0.2;

        public static final double DRUM_RAMP_LIMIT = 1000;
        public static final double DRUM_VELOCITY_BOUND = 300;
@@ -194,7 +194,7 @@ public final class Constants {
    
    public static final class VisionConstants {
        public static final double FOV = 29.8; //Field of view of limelight
-        public static final double TARGET_HEIGHT = 64;
+        public static final double TARGET_HEIGHT = 71;
        public static final double LIME_ANGLE = 25;
        public static final double TURN_P_VALUE = 0.65;
        public static final double X_ANGLE_ERROR = 1.3;
@@ -181,22 +181,31 @@ public class RobotContainer {
            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));


+
+
        /* Operator Buttons */

        // shoots until released
        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
-            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)));

        // shoots one ball
        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
-            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)));

        // extends or retracts the extender
        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            //.whenPressed(new RunExtenderOutIn(m_robotIntake));
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.3)));
+            .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
+            .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0)));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
+            .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
+            .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0)));
+
        // safety for climber and leveler
        new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
            .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
@@ -206,9 +215,9 @@ public class RobotContainer {
        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
            //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
            //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3)));
-            
-
+            //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
+            .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
+            .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
        //Prepares storage for intaking
        //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
        //    .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
@@ -217,21 +226,11 @@ public class RobotContainer {
        //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
        //    .whileHeld(new StorageOutake(m_robotStorage));

-        //TEST FOR HOOD
-        new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
-            //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
-            //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
-            .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeOut(0.1)))
-            .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeOut(0.0)));
-            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3)));
-
-        //TEST FOR HOOD
+        //Run drum
        new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-            //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
-            //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
-            .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeIn(0.1)))
-            .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeIn(0.0)));
-
+            //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
+            .whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
+            //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));

        //Trims shooter
        new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
@@ -13,6 +13,7 @@ import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
+import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.networktables.NetworkTable;
@@ -56,7 +57,7 @@ public class HoldTarget extends CommandBase {
      //Vision Processing Mode
      NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
      NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
-  }
+    }



@@ -67,39 +68,44 @@ public class HoldTarget extends CommandBase {
    xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
    yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
    
-    if (target == 1.0){ //If target in view
-        //Aiming Left/Right
-      turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
-      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone 
-        //Deadzones
-      else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
-      else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
+    if (target == 1.0) { // If target in view
+      // Aiming Left/Right
+      turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
+      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
+        turnAmount = 0;
+      } // Angle Error Zone
+      // Deadzones
+      else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = VisionConstants.MOTOR_DEAD_ZONE;
+      } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
+      }
      m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);

-        //Finding Distance
-      distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
+      // Finding Distance
+      distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
      SmartDashboard.putNumber("Distance to Target", distance);

-      double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
-      double xVel = (distance*VisionConstants.GRAV)/(yVel);
+        //START Equation Code
+        /*
+      double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
+      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);
-      m_shooter.m_fireVel = fireVel;
-      m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
+      */
+        //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 = 33;
+        //END CSV Code

-    }/*
-    else{
-      System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition());
-      double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1;
-      if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){
-        curveInput = -curveInput;
-      }
-      System.err.println("Curve Input: " + curveInput);
      
-      m_shooterAim.runShooterWithInput(curveInput);
+      m_shooter.m_fireVel = fireVel;
+      m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
    }
-    */
  }

  // Called once the command ends or is interrupted.
@@ -31,9 +31,9 @@ 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 slope = ShooterConstants.HOOD_CONVERT_SLOPE;
    double b = ShooterConstants.HOOD_CONVERT_B;
-    firingAngle = (-slope*m_shooter.addFireAngle())+b;
+    firingAngle = (-slope*m_shooter.addFireAngle())+b;*/
    SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
    SmartDashboard.putNumber("Fire Angle", firingAngle);
    m_shooter.runAngleAdjustPID(firingAngle);
@@ -49,7 +49,7 @@ public class HoodPositionPID extends CommandBase {
  public boolean isFinished() {
    double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
    if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
-      return true;
+      return false;
    }
    return false;
  }
@@ -36,11 +36,6 @@ public class RunExtenderOutIn extends CommandBase {
    // Use addRequirements() here to declare subsystem dependencies.
    m_intake = subsystem;
    addRequirements(m_intake);
-
-    m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
-    m_extenderForwardLimit.enableLimitSwitch(true);
-    m_extenderReverseLimit.enableLimitSwitch(true);
  }

  // Called when the command is initially scheduled.
@@ -25,7 +25,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
   */
  public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
    addCommands(
-      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
      new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
      new HoldTarget(m_shooter, m_shooterAim),
      new StorageRun(m_storage)
@@ -25,10 +25,10 @@ public class ShootPrepGroup extends ParallelCommandGroup {
   */
  public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
    addCommands(
-      new TrackTarget(m_shooter, m_shooterAim),
-      new ShooterVelocityControlPID(m_shooter),
-      new StoragePrepAim(m_storage),
-      new HoodPositionPID(m_shooter)
+      //new TrackTarget(m_shooter, m_shooterAim),
+      //new ShooterVelocityControlPID(m_shooter)
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()))
+      //new StoragePrepAim(m_storage)
    );
  }
 }
@@ -34,7 +34,7 @@ public class ShooterVelocityControlPID extends CommandBase {
  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
-    m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
    m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
    SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
    SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
@@ -43,8 +43,6 @@ public class TrackTarget extends CommandBase {
  public double m_hoodTrim;
  public double m_turretTrim;

-  ShooterTables m_shooterTable;
-
  /**
   * Uses the Limelight to track the target
   * @param shooterSubsystem The Shooter subsystem
@@ -63,7 +61,6 @@ public class TrackTarget extends CommandBase {
    // Vision Processing Mode
    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
-    m_shooterTable = new ShooterTables();
  }

  // Called every time the scheduler runs while the command is scheduled.
@@ -92,21 +89,24 @@ public class TrackTarget extends CommandBase {
      SmartDashboard.putNumber("Distance to Target", distance);

        //START Equation Code
+        /*
      double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
      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);
+      */
        //END Equation Code

-        /*//START CSV Code
-      fireVel = m_shooterTable.getVelocity(distance);
-      fireAngle = m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
-        //END CSV 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 = 33;
+        //END CSV Code

      
      m_shooter.m_fireVel = fireVel;
-      m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
+      m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
    }
  }

@@ -786,7 +786,7 @@ public class Drive extends SubsystemBase {
      //SmartDashboard.putString("Odometry Values Meters", getPose().toString());
      //SmartDashboard.putNumber("Odometry Heading", getHeading());

-      //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
+      SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
      SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);

      if (m_currentSong != m_songChooser.getSelected()){
@@ -60,18 +60,14 @@ public class Intake extends SubsystemBase {
    m_intakeMotor.set(input);
  }

-  public void runIntakeIn(double input){
-    m_extenderMotor.set(-input);
-  }
-  
-  public void runIntakeOut(double input){
+  public void runExtender(double input){
    m_extenderMotor.set(input);
  }
   /**
   * Runs extender motor
   * @param input the percent output to run motor at
   */
-  public void runExtender(double input) {
+  /*public void runExtender(double input) {
    if (m_extenderForwardLimit.get()) {
      isExtended = true;
    }
@@ -88,5 +84,5 @@ public class Intake extends SubsystemBase {
    if (isExtended == true) {
      m_extenderMotor.set(-input);
    }
-  }
+  }*/
 }
@@ -47,7 +47,7 @@ public class Shooter extends SubsystemBase {
  double velP;
  double input;

-  ShooterTables m_shooterTable;
+  public ShooterTables m_shooterTable;
  
  public boolean velReached;
  public double m_fireVel;
@@ -69,6 +69,7 @@ public class Shooter extends SubsystemBase {
    m_shooterFalcon.setInverted(true);
    m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
    m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
    setShooterGains();

    m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -77,7 +78,6 @@ 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();

@@ -91,8 +91,6 @@ public class Shooter extends SubsystemBase {
    SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
    SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));

-
-
    m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
    m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
    m_hoodUpLimit.enableLimitSwitch(true);
@@ -108,7 +106,19 @@ public class Shooter extends SubsystemBase {
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
-    SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
+    try{
+    SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
+
+    SmartDashboard.putNumber("Fire Velocity From CSV", m_fireVel);
+    SmartDashboard.putNumber("Fire Angle From CSV", m_fireAngle);
+
+    //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
+    }
+
+    catch(Exception e)
+    {
+      
+    }
  }

  public double addFireVel() {
@@ -157,15 +167,9 @@ public class Shooter extends SubsystemBase {
   * @param falcon Motor to use
   * @param targetVel Target velocity to run motor at
   */
-  public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
-    SmartDashboard.putNumber("shoot", actualVel);
-    if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){
-      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
-    }
-    else{ //PID Based on targetVel
-      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
-    }
-    m_shooterFalcon.feed();
+  public void runDrumShooterVelocityPID(double targetVel) {
+    System.out.println("dddddddddddddddddddddddd" + targetVel);
+    m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
  }

  public void resetGyroAngleAdj(){
@@ -173,6 +177,6 @@ public class Shooter extends SubsystemBase {
  }

  public double getAnglePosition(){
-    return m_angleAdjustMotor.getEncoder().getPosition();
+    return m_angleEncoder.getPosition();
  }
 }