From ddf18c3a9394aa8846b04ce47163a17804c8cb4c Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 2 Mar 2020 21:03:39 -0700 Subject: [PATCH] attempt at fix all notes: failed --- src/main/deploy/Robot Data - Angles.csv | 5 +- src/main/deploy/Robot Data - Distances.csv | 8 ++- src/main/java/frc4388/robot/Constants.java | 8 +-- .../java/frc4388/robot/RobotContainer.java | 45 ++++++++-------- .../frc4388/robot/commands/HoldTarget.java | 54 ++++++++++--------- .../robot/commands/HoodPositionPID.java | 6 +-- .../robot/commands/RunExtenderOutIn.java | 5 -- .../robot/commands/ShootFireGroup.java | 2 +- .../robot/commands/ShootPrepGroup.java | 8 +-- .../commands/ShooterVelocityControlPID.java | 2 +- .../frc4388/robot/commands/TrackTarget.java | 16 +++--- .../java/frc4388/robot/subsystems/Drive.java | 2 +- .../java/frc4388/robot/subsystems/Intake.java | 10 ++-- .../frc4388/robot/subsystems/Shooter.java | 34 ++++++------ 14 files changed, 100 insertions(+), 105 deletions(-) diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv index 273d4ba..5f3965e 100644 --- a/src/main/deploy/Robot Data - Angles.csv +++ b/src/main/deploy/Robot Data - Angles.csv @@ -1,6 +1,3 @@ Angle (deg),Displacement (deg) --20,-5 --10,-2 0,0 -10,2 -20,5 \ No newline at end of file +0,0 \ No newline at end of file diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index e058b92..5ff8a86 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -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 \ No newline at end of file +21,3,10000 +262,30,15000 +492,30,15000 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index fe6bedf..3685fd5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6c88f5c..08a5321 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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) diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index cbdee25..fa39663 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -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. diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java index efecb58..b48a3c4 100644 --- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java @@ -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; } diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index 64139a0..37d7ae1 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -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. diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 7d23c65..59bcf9c 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -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) diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index 5c20683..d234d7a 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java @@ -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) ); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 52dea7f..9dbd4f4 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -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()); diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 8bdae59..197dc72 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -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; } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 599e5e7..2d02a42 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -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()){ diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 1de90f4..11507ef 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -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); } - } + }*/ } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index cd0dda3..ae10d75 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -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(); } } \ No newline at end of file