diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7cb314e..674daa8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -59,6 +59,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.TrimShooter; import frc4388.robot.commands.StorageOutake; import frc4388.robot.commands.StoragePrepAim; import frc4388.robot.commands.StoragePrepIntake; @@ -97,6 +98,7 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -174,17 +176,12 @@ public class RobotContainer { // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) - //.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) - //.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); - .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Angle Current", m_robotShooter.m_angleAdjustMotor.getOutputCurrent()))) - .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Rotate Current", m_robotShooterAim.m_shooterRotateMotor.getOutputCurrent()))); + .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - //.whileHeld(new ParallelCommandGroup( - //new HoldTarget(m_robotShooter,m_robotShooterAim), - //new HoodPositionPID(m_robotShooter))) .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); @@ -196,16 +193,24 @@ 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))); + //TEST FOR HOOD 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))); + //Trims shooter + new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) + .whenPressed(new TrimShooter(m_robotShooter)); + + //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); + } /** @@ -323,4 +328,4 @@ public class RobotContainer { { return m_driverXbox.getJoyStick(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Trims.java b/src/main/java/frc4388/robot/Trims.java new file mode 100644 index 0000000..859794f --- /dev/null +++ b/src/main/java/frc4388/robot/Trims.java @@ -0,0 +1,18 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +public class Trims{ + public double m_turretTrim; + public double m_hoodTrim; + + public Trims(double turretTrim, double hoodTrim){ + m_turretTrim = turretTrim; + m_hoodTrim = hoodTrim; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index 791266d..cfed837 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -32,8 +32,11 @@ public class HoldTarget extends CommandBase { double yAngle = 0; double target = 0; public double distance; - public static double fireVel; - public static double fireAngle; + public double fireVel; + public double fireAngle; + + public double m_hoodTrim; + public double m_turretTrim; /** * Uses the Limelight to track the target @@ -69,7 +72,7 @@ public class HoldTarget extends CommandBase { //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_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); //Finding Distance distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); @@ -81,7 +84,7 @@ public class HoldTarget extends CommandBase { 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.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim; }/* else{ diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index fc49d81..95689e8 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -14,12 +14,14 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.commands.TrimShooter; import frc4388.utility.controller.IHandController; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class TrackTarget extends CommandBase { @@ -37,6 +39,9 @@ public class TrackTarget extends CommandBase { public static double fireVel; public static double fireAngle; + public double m_hoodTrim; + public double m_turretTrim; + /** * Uses the Limelight to track the target */ @@ -74,7 +79,7 @@ public class TrackTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(-turnAmount); + m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); @@ -85,8 +90,10 @@ public class TrackTarget extends CommandBase { 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.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim; } } diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/TrimShooter.java new file mode 100644 index 0000000..e83d00a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TrimShooter.java @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj.Joystick; +import frc4388.utility.controller.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; +import frc4388.robot.Trims; +import frc4388.robot.Constants.OIConstants; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; + +public class TrimShooter extends CommandBase { + private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + public double turretTrim = 0; + public double hoodTrim = 0; + + public Shooter m_shooter; + /** + * Creates a new TrimShooter. + */ + public TrimShooter(Shooter shootSub) { + m_shooter = shootSub; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if(m_operatorXbox.getDPadTop()){ + hoodTrim += 1; + } + else if(m_operatorXbox.getDPadBottom()){ + hoodTrim -= 1; + } + else if(m_operatorXbox.getDPadRight()){ + turretTrim += 1; + } + else if(m_operatorXbox.getDPadLeft()){ + turretTrim -= 1; + } + + m_shooter.shooterTrims.m_turretTrim = turretTrim; + m_shooter.shooterTrims.m_hoodTrim = hoodTrim; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 1f0a699..b46247a 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; +import frc4388.robot.Trims; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; import frc4388.utility.controller.IHandController; @@ -52,6 +53,8 @@ public class Shooter extends SubsystemBase { public double m_fireVel; public double m_fireAngle; CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; + + public Trims shooterTrims; /* * Creates a new Shooter subsystem. @@ -59,6 +62,8 @@ public class Shooter extends SubsystemBase { public Shooter() { //Testing purposes reseting gyros //resetGyroAngleAdj(); + shooterTrims = new Trims(0, 0); + m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(true); @@ -97,6 +102,7 @@ public class Shooter extends SubsystemBase { m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, 33); m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3); + } @Override diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index be69e18..fde5350 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -32,6 +32,7 @@ public class ShooterAim extends SubsystemBase { CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); + /** * Creates a new ShooterAim. */