diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08a5321..ceea2c7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -216,7 +216,7 @@ public class RobotContainer { //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); - .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim)) .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); //Prepares storage for intaking //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) @@ -229,12 +229,8 @@ public class RobotContainer { //Run drum new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) //.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) - .whenPressed(new TrimShooter(m_robotShooter)); + .whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))) + .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index fa39663..0bb327c 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -80,7 +80,7 @@ public class HoldTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); + m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java index b48a3c4..1ab2f43 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); diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 197dc72..2236913 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -82,25 +82,23 @@ public class TrackTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); + m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); 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))); + //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_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 diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index ae10d75..63ce02d 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -164,11 +164,10 @@ public class Shooter extends SubsystemBase { /** * Runs drum shooter velocity PID. - * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel) { - System.out.println("dddddddddddddddddddddddd" + targetVel); + System.out.println("Target Velocity" + targetVel); m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID }