diff --git a/build.gradle b/build.gradle index e0194f3..2b322db 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.2.2" + id "edu.wpi.first.GradleRIO" version "2020.3.2" } sourceCompatibility = JavaVersion.VERSION_11 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..5a625da 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,6 +1,9 @@ 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 +74,20,8000 +144,23,10000 +162,28,10000 +208,29,10000 +296,32,12500 +418,33,16000 +430,31,16000 +450,31,16000 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1ccad70..d177c44 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -10,6 +10,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; /** @@ -44,6 +45,8 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 60, 40, 2); public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + public static final double COS_MULTIPLIER_LOW = 1.0; + public static final double COS_MULTIPLIER_HIGH = 0.8; /* Drive Train Characteristics */ public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; @@ -121,15 +124,18 @@ 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 Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; public static final double NEO_UNITS_PER_REV = 42; public static final double DEGREES_PER_ROT = 360; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + public static final int TURRET_RIGHT_SOFT_LIMIT = -2; public static final int TURRET_LEFT_SOFT_LIMIT = -55; public static final double TURRET_SPEED_MULTIPLIER = 0.3; @@ -139,7 +145,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; @@ -161,12 +167,10 @@ public final class Constants { public static final double STORAGE_TIMEOUT = 2000; /* Ball Indexes */ - public static final int BEAM_SENSOR_DIO_0 = 0; - public static final int BEAM_SENSOR_DIO_1 = 1; - public static final int BEAM_SENSOR_DIO_2 = 2; - public static final int BEAM_SENSOR_DIO_3 = 3; - public static final int BEAM_SENSOR_DIO_4 = 4; - public static final int BEAM_SENSOR_DIO_5 = 5; + public static final int BEAM_SENSOR_SHOOTER = 1; + public static final int BEAM_SENSOR_USELESS = 2; + public static final int BEAM_SENSOR_STORAGE = 3; + public static final int BEAM_SENSOR_INTAKE = 4; /* PID Values */ public static final int SLOT_DISTANCE = 0; @@ -196,7 +200,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 4655a12..d19ba10 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,23 +11,20 @@ import java.util.List; import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.CalibrateShooter; @@ -60,12 +57,40 @@ import frc4388.robot.commands.StorageOutake; import frc4388.robot.commands.StoragePrepAim; import frc4388.robot.commands.StoragePrepIntake; import frc4388.robot.subsystems.Camera; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.auto.AutoPath1FromCenter; +import frc4388.robot.commands.auto.Wait; +import frc4388.robot.commands.climber.DisengageRachet; +import frc4388.robot.commands.climber.RunClimberWithTriggers; +import frc4388.robot.commands.climber.RunLevelerWithJoystick; +import frc4388.robot.commands.drive.DriveStraightToPositionMM; +import frc4388.robot.commands.drive.DriveWithJoystick; +import frc4388.robot.commands.drive.TurnDegrees; +import frc4388.robot.commands.intake.RunIntakeWithTriggers; +import frc4388.robot.commands.shooter.CalibrateShooter; +import frc4388.robot.commands.shooter.ShootFireGroup; +import frc4388.robot.commands.shooter.ShootFullGroup; +import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.shooter.TrackTarget; +import frc4388.robot.commands.shooter.TrimShooter; +import frc4388.robot.commands.storage.StorageIntake; +import frc4388.robot.commands.storage.StoragePrepIntake; +import frc4388.robot.subsystems.Camera; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Pneumatics; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.XboxTriggerButton; /** * This class is where the bulk of the robot should be declared. Since @@ -82,6 +107,7 @@ public class RobotContainer { private final Intake m_robotIntake = new Intake(); private final Shooter m_robotShooter = new Shooter(); private final ShooterAim m_robotShooterAim = new ShooterAim(); + private final ShooterHood m_robotShooterHood = new ShooterHood(); private final Climber m_robotClimber = new Climber(); private final Leveler m_robotLeveler = new Leveler(); private final Storage m_robotStorage = new Storage(); @@ -104,14 +130,20 @@ public class RobotContainer { m_robotPneumatics.passRequiredSubsystem(m_robotDrive); m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); + m_robotShooterHood.passRequiredSubsystem(m_robotShooter); + m_robotShooterAim.passRequiredSubsystem(m_robotShooter); + + m_robotLeveler.passRequiredSubsystem(m_robotClimber); + configureButtonBindings(); /* Default Commands */ // drives the robot with a two-axis input from the driver controller - - m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, getDriverController())); + + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); - + // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // runs the turret with joystick @@ -121,10 +153,12 @@ public class RobotContainer { // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller - m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // runs the storage not //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -139,11 +173,10 @@ public class RobotContainer { // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0)); - + // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new TurnDegrees(m_robotDrive, 90)); - // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new Wait(m_robotDrive, 0, 0)); @@ -151,7 +184,9 @@ public class RobotContainer { // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand()); - + + + /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) @@ -161,22 +196,32 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + // Disengages the rachet to allow for a climb + new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) + .whileHeld(new DisengageRachet(m_robotClimber)); /* 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_robotShooterHood, m_robotStorage), false) + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // 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_robotShooterHood, m_robotStorage), false) + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(new RunExtenderOutIn(m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) + .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) + .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) @@ -185,27 +230,13 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); - - - //Prepares storage for intaking - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); - - //Runs storage to outtake - 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))); + .whileHeld(new TrackTarget(m_robotShooterAim)) + .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) + //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)) + .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); + //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); + //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); //Trims shooter new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) @@ -213,8 +244,23 @@ public class RobotContainer { //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); + .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + //Prepares storage for intaking + //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))) + //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + //.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); + + //Runs storage to outtake + //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER) + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))) + //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + //Run drum + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); } /** @@ -232,7 +278,7 @@ public class RobotContainer { //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); - + /*if (Constants.SELECTED_AUTO == 1) { return new SequentialCommandGroup(new Wait(m_robotDrive, 5, 0), new TurnDegrees(m_robotDrive, 45), @@ -263,7 +309,6 @@ public class RobotContainer { new Pose2d(50, 50, new Rotation2d(0)), // Pass config config); - return exampleTrajectory; } @@ -271,11 +316,11 @@ public class RobotContainer { RamseteCommand ramseteCommand = new RamseteCommand( trajectory, m_robotDrive::getPose, - new RamseteController(), + new RamseteController(), DriveConstants.kDriveKinematics, m_robotDrive::tankDriveVelocity, m_robotDrive); - + return ramseteCommand; } @@ -296,7 +341,7 @@ public class RobotContainer { } /** - * + * */ public void resetOdometry() { m_robotDrive.resetGyroAngles(); @@ -310,7 +355,7 @@ public class RobotContainer { public IHandController getDriverController() { return m_driverXbox; } - + /** * Used for analog inputs like triggers and axises. * @return The IHandController interface for the Operator Controller. @@ -319,7 +364,7 @@ public class RobotContainer { { return m_operatorXbox; } - + /** * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller. * Generic HIDs/Joysticks can be used to set up JoystickButtons. @@ -329,7 +374,7 @@ public class RobotContainer { { return m_operatorXbox.getJoyStick(); } - + /** * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Driver Xbox Controller. * Generic HIDs/Joysticks can be used to set up JoystickButtons. diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java b/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java index af8e29a..5ffd58c 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java @@ -5,10 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.auto.Wait; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java deleted file mode 100644 index cbdee25..0000000 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ /dev/null @@ -1,117 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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 frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.utility.controller.IHandController; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class HoldTarget extends CommandBase { - //Setup - NetworkTableEntry xEntry; - ShooterAim m_shooterAim; - Shooter m_shooter; - IHandController m_driverController; - //Aiming - double turnAmount = 0; - double xAngle = 0; - double yAngle = 0; - double target = 0; - public double distance; - public double fireVel; - public double fireAngle; - - public double m_hoodTrim; - public double m_turretTrim; - - /** - * Uses the Limelight to track the target - * @param shooterSubsystem The Shooter subsystem - * @param aimSubsystem The ShooterAim subsystem - */ - public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { - m_shooterAim = aimSubsystem; - m_shooter = shooterSubsystem; - addRequirements(m_shooterAim); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //Vision Processing Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - } - - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); - 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;} - 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); - - 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; - - }/* - 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); - } - */ - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java similarity index 95% rename from src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java rename to src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java index 7af0b3f..d96516d 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java +++ b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java @@ -5,9 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.drive.GotoCoordinates; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java similarity index 96% rename from src/main/java/frc4388/robot/commands/AutoPath2FromRight.java rename to src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java index 5c9da9f..3608f6a 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java @@ -5,9 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.drive.GotoCoordinates; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java similarity index 87% rename from src/main/java/frc4388/robot/commands/Wait.java rename to src/main/java/frc4388/robot/commands/auto/Wait.java index 580d279..6bbedf3 100644 --- a/src/main/java/frc4388/robot/commands/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.auto; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -46,11 +45,11 @@ public class Wait extends CommandBase { public void execute() { if (counter == 0) { - SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); + //SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); } m_currentTime = System.currentTimeMillis(); - SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); + //SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); counter ++; } diff --git a/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java b/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java new file mode 100644 index 0000000..631e23e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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.climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Climber; + +public class DisengageRachet extends CommandBase { + Climber m_climber; + + /** + * Creates a new DisengageRachet command. + */ + public DisengageRachet(Climber subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_climber = subsystem; + } + + // 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_climber.climberSafety) { + m_climber.shiftServo(false); + } + } + + // 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 true; + } +} diff --git a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java similarity index 96% rename from src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java rename to src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java index 9ab4285..7685202 100644 --- a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.climber; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Climber; @@ -43,6 +43,7 @@ public class RunClimberWithTriggers extends CommandBase { } if (leftTrigger > rightTrigger) { output = -leftTrigger; + m_climber.shiftServo(true); } } else { output = rightTrigger; diff --git a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java similarity index 95% rename from src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java rename to src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java index 0b91068..2052ef4 100644 --- a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.climber; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Leveler; @@ -36,7 +36,7 @@ public class RunLevelerWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double input = m_controller.getLeftXAxis(); + double input = m_controller.getRightXAxis(); m_leveler.runLeveler(input); } diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java similarity index 98% rename from src/main/java/frc4388/robot/commands/DrivePositionMPAux.java rename to src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java index 68d8390..901fdcb 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java similarity index 97% rename from src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java index c79ccbc..e63a5bf 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java @@ -4,8 +4,7 @@ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java similarity index 95% rename from src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java rename to src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java index c9aa927..a2be80e 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -61,7 +61,7 @@ public class DriveStraightToPositionMM extends CommandBase { //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); - SmartDashboard.putBoolean("MM Run", true); + //SmartDashboard.putBoolean("MM Run", true); i++; } @@ -74,7 +74,7 @@ public class DriveStraightToPositionMM extends CommandBase { @Override public boolean isFinished() { if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){ - SmartDashboard.putBoolean("MM Run", false); + //SmartDashboard.putBoolean("MM Run", false); return true; } else { if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) { diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java similarity index 97% rename from src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java index a457e26..337ff6d 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java similarity index 92% rename from src/main/java/frc4388/robot/commands/DriveWithJoystick.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java index 3c6e88f..75ab279 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java @@ -5,10 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; @@ -42,7 +42,7 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = -m_controller.getLeftYAxis(); + double moveInput = m_controller.getLeftYAxis(); double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; double steerOutput = 0; @@ -52,13 +52,13 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = 1.0; + double cosMultiplier; double deadzone = .1; if (m_pneumatics.m_isSpeedShiftHigh) { - cosMultiplier = 0.8; + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; } else { - cosMultiplier = 1.0; + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; } if (steerInput > 0){ diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java similarity index 92% rename from src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java index 2531847..6444141 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java @@ -5,9 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; - -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; @@ -61,9 +59,9 @@ public class DriveWithJoystickAuxPID extends CommandBase { m_drive.driveWithInputAux(moveOutput, m_targetGyro); - System.err.println("Target: " + m_targetGyro); - System.err.println("Current: " + currentGyro); - System.err.println(); + //System.err.println("Target: " + m_targetGyro); + //System.err.println("Current: " + currentGyro); + //System.err.println(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java similarity index 83% rename from src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java index ef9c1b9..56d2f96 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java @@ -5,16 +5,17 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; public class DriveWithJoystickDriveStraight extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetGyro, m_currentGyro; double m_stopPos; long m_currTime, m_deltaTime; @@ -22,6 +23,8 @@ public class DriveWithJoystickDriveStraight extends CommandBase { long m_deadTimeout = 100; IHandController m_controller; boolean m_isInterrupted; + double highGearMultiplier = 1; + double lowGearMultiplier = 1; /** * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller. @@ -35,6 +38,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = m_drive.m_pneumaticsSubsystem; m_controller = controller; addRequirements(m_drive); } @@ -84,9 +88,17 @@ public class DriveWithJoystickDriveStraight extends CommandBase { } private void runDriveWithInput(double move, double steer) { - double cosMultiplier = .7; + + double cosMultiplier; double steerOutput = 0; double deadzone = .1; + + if (m_pneumatics.m_isSpeedShiftHigh) { + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; + } else { + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; + } + /* Curves the steer output to be similarily gradual */ if (steer > 0) { steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier; @@ -94,12 +106,12 @@ public class DriveWithJoystickDriveStraight extends CommandBase { steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier; } m_drive.driveWithInput(move, steerOutput); - System.out.println("Driving With Input"); + //System.out.println("Driving With Input"); } private void runDriveStraight(double move) { m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - System.out.println("Driving Straight with Target: " + m_targetGyro); + //System.out.println("Driving Straight with Target: " + m_targetGyro); } /** @@ -107,8 +119,13 @@ public class DriveWithJoystickDriveStraight extends CommandBase { */ private void resetGyroTarget() { //m_targetGyro = m_currentGyro; - m_targetGyro = m_currentGyro - + m_drive.getTurnRate(); + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetGyro = m_currentGyro + + highGearMultiplier * m_drive.getTurnRate(); + } else { + m_targetGyro = m_currentGyro + + lowGearMultiplier * m_drive.getTurnRate(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java similarity index 96% rename from src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java index 0b154e7..13523af 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; @@ -133,12 +132,12 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } m_drive.driveWithInput(move, steerOutput); - System.out.println("Driving With Input"); + //System.out.println("Driving With Input"); } private void runDriveStraight(double move) { m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - System.out.println("Driving Straight with Target: " + m_targetGyro); + //System.out.println("Driving Straight with Target: " + m_targetGyro); } private void runStoppedTurn(double steer) { @@ -159,7 +158,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } else { m_drive.driveWithInputAux(0, m_targetGyro); } - System.out.println("Turning with Target: " + m_targetGyro); + //System.out.println("Turning with Target: " + m_targetGyro); } /** diff --git a/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java new file mode 100644 index 0000000..5ffd58c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* 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.drive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.auto.Wait; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class GotoCoordinatesRobotRelative extends SequentialCommandGroup { + Drive m_drive; + Pneumatics m_pneumatics; + + double m_xTarget; + double m_yTarget; + double m_currentAngle; + double m_hypotDist; + double m_endAngle; + + /** + * Creates a new GotoPosition. + */ + public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + m_pneumatics = subsystem2; + + m_xTarget = xTarget; + m_yTarget = yTarget; + + m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); + + m_currentAngle = calcAngle(); + + m_endAngle = endAngle; + + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 0, 0), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); + } + + public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { + m_drive = subsystem; + m_pneumatics = subsystem2; + + m_xTarget = xTarget; + m_yTarget = yTarget; + + m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); + + m_currentAngle = calcAngle(); + + m_endAngle = m_currentAngle; + + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 0, 0), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); + } + + public boolean isQuadrantThree() { + if ((m_xTarget < 0) && (m_yTarget < 0)) { + return true; + } else { + return false; + } + } + + public double calcAngle() { + if (isQuadrantThree()) { + return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; + } else { + return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; + } + } + +} diff --git a/src/main/java/frc4388/robot/commands/PlaySongDrive.java b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java similarity index 97% rename from src/main/java/frc4388/robot/commands/PlaySongDrive.java rename to src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java index 5e0e6b4..eaa0f07 100644 --- a/src/main/java/frc4388/robot/commands/PlaySongDrive.java +++ b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java similarity index 90% rename from src/main/java/frc4388/robot/commands/TurnDegrees.java rename to src/main/java/frc4388/robot/commands/drive/TurnDegrees.java index 83630dd..e0999e6 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -50,8 +50,8 @@ public class TurnDegrees extends CommandBase { m_drive.runTurningPID(m_targetAngleTicksOut); - SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); - SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); + //SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); + //SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); i++; } diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java similarity index 84% rename from src/main/java/frc4388/robot/commands/RunExtenderOutIn.java rename to src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java index 3c062a0..c222ad6 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java @@ -5,17 +5,15 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.intake; import com.revrobotics.CANDigitalInput; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.Intake; -import frc4388.utility.controller.IHandController; public class RunExtenderOutIn extends CommandBase { private Intake m_intake; @@ -36,11 +34,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.kNormallyClosed); - m_extenderForwardLimit.enableLimitSwitch(false); - m_extenderReverseLimit.enableLimitSwitch(false); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java similarity index 90% rename from src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java rename to src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java index 5f9dc4a..7f90f63 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.intake; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Intake; @@ -41,16 +41,12 @@ public class RunIntakeWithTriggers extends CommandBase { double rightTrigger = m_controller.getRightTriggerAxis(); double leftTrigger = m_controller.getLeftTriggerAxis(); double output = 0; - if (leftTrigger < .5) { - if(leftTrigger > rightTrigger) { - output = leftTrigger; - } - if (rightTrigger > leftTrigger) { - output = -leftTrigger; - } - } else { + if(leftTrigger >= rightTrigger) { output = leftTrigger; } + if (rightTrigger > leftTrigger) { + output = -rightTrigger; + } m_intake.runIntake(output); } diff --git a/src/main/java/frc4388/robot/commands/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java similarity index 75% rename from src/main/java/frc4388/robot/commands/CalibrateShooter.java rename to src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index 1b766d7..23be5fa 100644 --- a/src/main/java/frc4388/robot/commands/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -5,28 +5,31 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import com.revrobotics.CANSparkMax.SoftLimitDirection; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class CalibrateShooter extends CommandBase { Shooter m_shooter; ShooterAim m_shooterAim; + ShooterHood m_shooterHood; + /** * Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders * @param shootSub The Shooter subsystem * @param aimSub The ShooterAim subsystem */ - public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) { + public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) { m_shooter = shootSub; m_shooterAim = aimSub; - addRequirements(m_shooter, m_shooterAim); + m_shooterHood = hoodSub; + addRequirements(m_shooter, m_shooterHood, m_shooterAim); } // Called when the command is initially scheduled. @@ -37,10 +40,8 @@ public class CalibrateShooter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); - m_shooter.m_angleEncoder.setPosition(0); - m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); + m_shooterHood.m_angleEncoder.setPosition(0); + m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); @@ -51,9 +52,6 @@ public class CalibrateShooter extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); } diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java similarity index 59% rename from src/main/java/frc4388/robot/commands/HoodPositionPID.java rename to src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index efecb58..2f5a14a 100644 --- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -5,22 +5,22 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterHood; public class HoodPositionPID extends CommandBase { - Shooter m_shooter; + ShooterHood m_shooterHood; double firingAngle; + /** * Creates a new HoodPositionPID. */ - public HoodPositionPID(Shooter subSystem) { - m_shooter = subSystem; - //addRequirements(m_shooter); + public HoodPositionPID(ShooterHood subSystem) { + m_shooterHood = subSystem; + addRequirements(m_shooterHood); } // Called when the command is initially scheduled. @@ -31,12 +31,13 @@ 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_shooter.addFireAngle())+b; - SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); - SmartDashboard.putNumber("Fire Angle", firingAngle); - m_shooter.runAngleAdjustPID(firingAngle); + //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); } // Called once the command ends or is interrupted. @@ -47,9 +48,11 @@ public class HoodPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition(); + double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ - return true; + m_shooterHood.m_isHoodReady = true; + } else { + m_shooterHood.m_isHoodReady = false; } return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java new file mode 100644 index 0000000..c0ef7a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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.shooter; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.robot.subsystems.Storage; + +public class PrepChecker extends CommandBase { + Shooter m_shooter; + ShooterAim m_shooterAim; + ShooterHood m_shooterHood; + Storage m_storage; + + boolean m_isDrumReady = false; + boolean m_isAimReady = false; + boolean m_isHoodReady = false; + boolean m_isStorageReady = false; + + /** + * Creates a new PrepChecker. + * @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands. + * @param storage reads storage in a similar way to shooter. Not used as a requirement. + */ + public PrepChecker(Shooter shooter, Storage storage) { + m_shooter = shooter; + m_shooterAim = m_shooter.m_shooterAimSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; + m_storage = storage; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_isDrumReady = false; + m_isAimReady = false; + m_isHoodReady = false; + m_isStorageReady = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady); + m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady); + m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady); + m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_shooter.m_isDrumReady = false; + m_shooterAim.m_isAimReady = false; + m_shooterHood.m_isHoodReady = false; + m_storage.m_isStorageReadyToFire = false; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java similarity index 76% rename from src/main/java/frc4388/robot/commands/ShootFireGroup.java rename to src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index 7d23c65..15ee080 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -5,12 +5,14 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc4388.robot.commands.storage.StorageRun; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -23,12 +25,12 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { + public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), - new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), - new HoldTarget(m_shooter, m_shooterAim), - new StorageRun(m_storage) + 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) ); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java similarity index 80% rename from src/main/java/frc4388/robot/commands/ShootFullGroup.java rename to src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java index 55557b8..8863636 100644 --- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java @@ -5,11 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -22,10 +23,10 @@ public class ShootFullGroup extends SequentialCommandGroup { * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { + public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { addCommands( - new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), - new ShootFireGroup(m_shooter, m_shooterAim, m_storage) + new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage), + new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage) ); } } diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java similarity index 69% rename from src/main/java/frc4388/robot/commands/ShootPrepGroup.java rename to src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index 5c20683..14accc3 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -5,30 +5,32 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import frc4388.robot.commands.storage.StoragePrepAim; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShootPrepGroup extends ParallelCommandGroup { +public class ShootPrepGroup extends ParallelDeadlineGroup { /** * Prepares the Shooter to be fired * @param m_shooter The Shooter subsytem * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { - addCommands( - new TrackTarget(m_shooter, m_shooterAim), + public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { + super( + new PrepChecker(m_shooter, m_storage), + new TrackTarget(m_shooterAim), new ShooterVelocityControlPID(m_shooter), - new StoragePrepAim(m_storage), - new HoodPositionPID(m_shooter) + new HoodPositionPID(m_shooterHood), + new StoragePrepAim(m_storage) ); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java similarity index 74% rename from src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java rename to src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 52dea7f..215b6bd 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; @@ -16,6 +15,7 @@ public class ShooterVelocityControlPID extends CommandBase { Shooter m_shooter; double m_targetVel; double m_actualVel; + /** * Runs the drum at a velocity * @param subsystem The Shooter subsytem @@ -34,10 +34,10 @@ 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.runAngleAdjustPID(m_shooter.addFireAngle()); - SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); - SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); + m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); + //m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()); + //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); + //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } // Called once the command ends or is interrupted. @@ -52,12 +52,12 @@ public class ShooterVelocityControlPID extends CommandBase { double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND; double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND; if (m_actualVel < upperBound && m_actualVel > lowerBound){ - SmartDashboard.putBoolean("ShooterVelocityPID Finished", true); - return true; + m_shooter.m_isDrumReady = true; } else{ - SmartDashboard.putBoolean("ShooterVelocityPID Finished", false); - return false; + m_shooter.m_isDrumReady = false; } + + return false; } } diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java similarity index 74% rename from src/main/java/frc4388/robot/commands/TrackTarget.java rename to src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 8bdae59..6d35f49 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -5,31 +5,25 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Camera; -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.ShooterTables; -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; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.utility.controller.IHandController; public class TrackTarget extends CommandBase { // Setup - NetworkTableEntry xEntry; ShooterAim m_shooterAim; Shooter m_shooter; + ShooterHood m_shooterHood; + + NetworkTableEntry xEntry; IHandController m_driverController; // Aiming double turnAmount = 0; @@ -43,16 +37,15 @@ 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 * @param aimSubsystem The ShooterAim subsystem */ - public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { + public TrackTarget(ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; - m_shooter = shooterSubsystem; + m_shooter = m_shooterAim.m_shooterSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); } @@ -63,7 +56,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. @@ -85,7 +77,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_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)); @@ -95,18 +87,22 @@ public class TrackTarget extends CommandBase { 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); + //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 + + //fireVel = SmartDashboard.getNumber("Velocity Target", 0); + //fireAngle = SmartDashboard.getNumber("Angle Target", 3); m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim; + m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } } @@ -121,10 +117,11 @@ public class TrackTarget extends CommandBase { public boolean isFinished() { if (xAngle < 1 && xAngle > -1 && target == 1) { - SmartDashboard.putBoolean("TrackTarget Finished", true); - return true; + m_shooterAim.m_isAimReady = true; + } else { + m_shooterAim.m_isAimReady = false; } - SmartDashboard.putBoolean("TrackTarget Finished", false); + return false; } } diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java similarity index 91% rename from src/main/java/frc4388/robot/commands/TrimShooter.java rename to src/main/java/frc4388/robot/commands/shooter/TrimShooter.java index b77a4ba..2c673b8 100644 --- a/src/main/java/frc4388/robot/commands/TrimShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java @@ -5,16 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -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; +import frc4388.utility.controller.XboxController; public class TrimShooter extends CommandBase { private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java similarity index 87% rename from src/main/java/frc4388/robot/commands/StorageIntake.java rename to src/main/java/frc4388/robot/commands/storage/StorageIntake.java index baf1630..47769c4 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java @@ -5,10 +5,9 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Storage; @@ -39,7 +38,7 @@ public class StorageIntake extends CommandBase { @Override public void execute() { - if (!m_storage.getBeam(0)){ + if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); intook = true; } @@ -56,7 +55,7 @@ public class StorageIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(0) && intook){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java new file mode 100644 index 0000000..0d8c447 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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.storage; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class StorageIntakeFinal extends CommandBase { + /** + * Creates a new StorageIntakeFinal. + */ + public StorageIntakeFinal() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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() { + } + + // 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/commands/StorageOutake.java b/src/main/java/frc4388/robot/commands/storage/StorageOutake.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StorageOutake.java rename to src/main/java/frc4388/robot/commands/storage/StorageOutake.java index 3231600..658d8e5 100644 --- a/src/main/java/frc4388/robot/commands/StorageOutake.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageOutake.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StoragePositionPID.java rename to src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java index bc607e5..2a27b41 100644 --- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/commands/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java similarity index 80% rename from src/main/java/frc4388/robot/commands/StoragePrepAim.java rename to src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java index f0e52c1..6d47abf 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -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,11 +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()){ - SmartDashboard.putBoolean("StoragePrepAim Finished", true); + /*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ + m_storage.m_isStorageReadyToFire = true; return true; - } - SmartDashboard.putBoolean("StoragePrepAim Finished", false); - return false; + } else { + m_storage.m_isStorageReadyToFire = false; + return false; + }*/ + return true; } } diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java similarity index 86% rename from src/main/java/frc4388/robot/commands/StoragePrepIntake.java rename to src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java index 44a2d2e..b9a9cf1 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; @@ -38,7 +38,7 @@ public class StoragePrepIntake extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(0)){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){ m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ @@ -54,8 +54,8 @@ public class StoragePrepIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ - return true; + if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ + return false; } return false; } diff --git a/src/main/java/frc4388/robot/commands/StorageRun.java b/src/main/java/frc4388/robot/commands/storage/StorageRun.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StorageRun.java rename to src/main/java/frc4388/robot/commands/storage/StorageRun.java index 91632f5..11dbc4b 100644 --- a/src/main/java/frc4388/robot/commands/StorageRun.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageRun.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java index 332bb46..58bd8d1 100644 --- a/src/main/java/frc4388/robot/subsystems/Camera.java +++ b/src/main/java/frc4388/robot/subsystems/Camera.java @@ -7,11 +7,9 @@ package frc4388.robot.subsystems; -import edu.wpi.cscore.MjpegServer; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.VideoSource; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index e9adf86..fc932d0 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -13,6 +13,8 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClimberConstants; @@ -20,16 +22,21 @@ public class Climber extends SubsystemBase { CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); CANDigitalInput m_climberForwardLimit, m_climberReverseLimit; + Servo m_servo; + //Spark m_spark = new Spark(4); + public boolean climberSafety = false; /** * Creates a new Climber. */ public Climber() { + m_servo = new Servo(4); + m_climberMotor.restoreFactoryDefaults(); m_climberMotor.setIdleMode(IdleMode.kBrake); - m_climberMotor.setInverted(false); + m_climberMotor.setInverted(true); m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); @@ -48,6 +55,7 @@ public class Climber extends SubsystemBase { */ public void runClimber(double input) { if(climberSafety){ + input *= 1.0; m_climberMotor.set(input); } else{ @@ -66,4 +74,15 @@ public class Climber extends SubsystemBase { { climberSafety = false; } + + /** + * @param shift true to enage rachet, false to disengage + */ + public void shiftServo(boolean shift) { + if (shift) { + m_servo.setPosition(0.5); + } else { + m_servo.setPosition(0.56); + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 0613050..fe897e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -24,7 +24,6 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -34,10 +33,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.Constants.PneumaticsConstants; -import frc4388.robot.Gains; +import frc4388.utility.Gains; public class Drive extends SubsystemBase { /* Create Motors, Gyros, etc */ @@ -53,7 +50,7 @@ public class Drive extends SubsystemBase { public Orchestra m_orchestra; /* Pneumatics Subsystem */ - Pneumatics m_pneumaticsSubsystem; + public Pneumatics m_pneumaticsSubsystem; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; @@ -736,16 +733,16 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); - SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); + //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); + //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor RPM", leftRPM); - SmartDashboard.putNumber("Right Motor RPM", rightRPM); + //SmartDashboard.putNumber("Left Motor RPM", leftRPM); + //SmartDashboard.putNumber("Right Motor RPM", rightRPM); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -754,8 +751,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + //SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + //SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); @@ -763,30 +760,30 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); */ - SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); - SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); + //SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); + //SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); - SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); + //SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); - SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); + //SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); 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 f074b33..109b412 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -8,12 +8,11 @@ package frc4388.robot.subsystems; import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; @@ -38,10 +37,10 @@ public class Intake extends SubsystemBase { m_intakeMotor.setIdleMode(IdleMode.kCoast); m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(false); + m_extenderMotor.setInverted(true); - m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); m_extenderForwardLimit.enableLimitSwitch(true); m_extenderReverseLimit.enableLimitSwitch(true); } @@ -58,18 +57,24 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(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; } - if (m_extenderReverseLimit.get()) { + else if (m_extenderReverseLimit.get()) { isExtended = false; } + else{ + m_extenderMotor.set(-input); + } if (isExtended == false) { m_extenderMotor.set(input); @@ -77,5 +82,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/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 6ef70e5..cbad21c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -50,6 +50,6 @@ public class LED extends SubsystemBase { @Override public void periodic(){ - SmartDashboard.putNumber("LED", currentLED); + //SmartDashboard.putNumber("LED", currentLED); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index 0db0562..be5e77d 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -7,22 +7,17 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LevelerConstants; -import frc4388.robot.subsystems.*; public class Leveler extends SubsystemBase { CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); - private final Climber m_robotClimber = new Climber(); + Climber m_climberSubsystem; /** * Creates a new Leveler. @@ -44,11 +39,19 @@ public class Leveler extends SubsystemBase { * @param input the percent output to run motor at */ public void runLeveler(double input) { - if(m_robotClimber.climberSafety){ + if(m_climberSubsystem.climberSafety){ m_levelerMotor.set(input); } else{ m_levelerMotor.set(0); } } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Climber subsystem) { + m_climberSubsystem = subsystem; + } } diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java index 1c373ed..6b7327b 100644 --- a/src/main/java/frc4388/robot/subsystems/Pneumatics.java +++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.java @@ -8,9 +8,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.RobotContainer; import frc4388.robot.Constants.PneumaticsConstants; public class Pneumatics extends SubsystemBase { diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index cd0dda3..d8bb022 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -11,50 +11,34 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.ControlType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.Joystick; -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.Gains; import frc4388.utility.ShooterTables; +import frc4388.utility.Trims; import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - - CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; - public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; public static Shooter m_shooter; public static IHandController m_controller; double velP; double input; - ShooterTables m_shooterTable; + public ShooterTables m_shooterTable; - public boolean velReached; + public boolean m_isDrumReady = false; public double m_fireVel; - public double m_fireAngle; - CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; public Trims shooterTrims; + + public ShooterHood m_shooterHoodSubsystem; + public ShooterAim m_shooterAimSubsystem; /* * Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter. @@ -63,12 +47,14 @@ public class Shooter extends SubsystemBase { //Testing purposes reseting gyros //resetGyroAngleAdj(); shooterTrims = new Trims(0, 0); + //SmartDashboard.putNumber("Velocity Target", 10000); + //SmartDashboard.putNumber("Angle Target", 3); m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(true); - m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -77,47 +63,55 @@ 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(); - SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); - SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); - SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); - SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); - SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); - SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); - SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); + //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); + //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); + //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); + //SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); - - - m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodUpLimit.enableLimitSwitch(true); - m_hoodDownLimit.enableLimitSwitch(true); - - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); - + //SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); + //SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); + //SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); + //SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); } @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("Drum Velocity CSV", m_fireVel); + + SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature()); + + SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); + + //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); + } + + catch(Exception e) + { + + } + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) { + m_shooterHoodSubsystem = subsystem0; + m_shooterAimSubsystem = subsystem1; } public double addFireVel() { return m_fireVel; } - public double addFireAngle() { - return m_fireAngle; - } - /** * Runs drum shooter motor. @@ -138,41 +132,12 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } - /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle) - { - // Set PID Coefficients - m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); - m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); - m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); - m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); - m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); - m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); - - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); - } - /** * Runs drum shooter velocity PID. - * @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 resetGyroAngleAdj(){ - m_angleEncoder.setPosition(0); - } - - public double getAnglePosition(){ - return m_angleAdjustMotor.getEncoder().getPosition(); + public void runDrumShooterVelocityPID(double targetVel) { + //System.out.println("Target Velocity" + targetVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 1111749..a34d59f 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -8,30 +8,31 @@ package frc4388.robot.subsystems; import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.ControlType; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; public class ShooterAim extends SubsystemBase { + public Shooter m_shooterSubsystem; + public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; - // Configure PID Controllers - CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); - public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); + public boolean m_isAimReady = false; + // Configure PID Controllers + CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); + public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); /** * Creates a subsytem for the turret aiming @@ -49,6 +50,21 @@ public class ShooterAim extends SubsystemBase { m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT); + + m_shooterRotateMotor.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Shooter subsystem) { + m_shooterSubsystem = subsystem; } public void runShooterWithInput(double input) { @@ -73,17 +89,13 @@ public class ShooterAim extends SubsystemBase { m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); } - public void resetGyroShooterRotate() - { - m_shooterRotateEncoder.setPosition(0); - } + public void resetGyroShooterRotate() + { + m_shooterRotateEncoder.setPosition(0); + } - public double getShooterRotatePosition(){ - return m_shooterRotateMotor.getEncoder().getPosition(); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run + public double getShooterRotatePosition() + { + return m_shooterRotateMotor.getEncoder().getPosition(); } } diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java new file mode 100644 index 0000000..c177c38 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; + +public class ShooterHood extends SubsystemBase { + public Shooter m_shooterSubsystem; + + public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + + public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + + public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + + public boolean m_isHoodReady = false; + + public double m_fireAngle; + public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; + + /** + * Creates a new ShooterHood. + */ + public ShooterHood() { + m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); + + m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodUpLimit.enableLimitSwitch(true); + m_hoodDownLimit.enableLimitSwitch(true); + + //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); + //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Shooter subsystem) { + m_shooterSubsystem = subsystem; + } + + public double addFireAngle() { + return m_fireAngle; + } + + /* Angle Adjustment PID Control */ + public void runAngleAdjustPID(double targetAngle) + { + // Set PID Coefficients + m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); + m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); + m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); + m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); + m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); + m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroAngleAdj(){ + m_angleEncoder.setPosition(0); +} + + public double getAnglePosition(){ + return m_angleEncoder.getPosition(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7bcc411..3fe6ccd 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -7,23 +7,17 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; -import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.ControlType; + import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.command.WaitUntilCommand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; +import frc4388.utility.Gains; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); @@ -37,18 +31,17 @@ public class Storage extends SubsystemBase { Intake m_intake; + public boolean m_isStorageReadyToFire = false; /** * Creates a new Storage. */ public Storage() { resetEncoder(); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); - m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); - m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); - m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); - m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); - m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); + m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); + m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); } @Override @@ -85,8 +78,8 @@ public class Storage extends SubsystemBase { m_storagePIDController.setFF(storageGains.m_kF); m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); - SmartDashboard.putNumber("Storage Position PID Target", targetPos); - SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); + //SmartDashboard.putNumber("Storage Position PID Target", targetPos); + //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/utility/Gains.java similarity index 98% rename from src/main/java/frc4388/robot/Gains.java rename to src/main/java/frc4388/utility/Gains.java index 7d2b3d7..f1b78f9 100644 --- a/src/main/java/frc4388/robot/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot; +package frc4388.utility; /** * Add your docs here. diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index dc7fa83..85244ce 100644 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -85,12 +85,12 @@ public class ShooterTables { } catch (IOException e) { e.printStackTrace(); } - SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); - SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); - SmartDashboard.putNumber("m_distanceLength", m_distanceLength); - SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); - SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); - SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); + //SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); + //SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); + //SmartDashboard.putNumber("m_distanceLength", m_distanceLength); + //SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); + //SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); + //SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); } public double getHood(double distance) { //Rotations of motor diff --git a/src/main/java/frc4388/robot/Trims.java b/src/main/java/frc4388/utility/Trims.java similarity index 96% rename from src/main/java/frc4388/robot/Trims.java rename to src/main/java/frc4388/utility/Trims.java index 859794f..b97791d 100644 --- a/src/main/java/frc4388/robot/Trims.java +++ b/src/main/java/frc4388/utility/Trims.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot; +package frc4388.utility; public class Trims{ public double m_turretTrim; diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index c6ec878..aa5554e 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.18.1", + "version": "5.18.2", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.18.1" + "version": "5.18.2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.18.1" + "version": "5.18.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json index 63380b6..5eb069f 100644 --- a/vendordeps/REVRobotics.json +++ b/vendordeps/REVRobotics.json @@ -1,7 +1,7 @@ { "fileName": "REVRobotics.json", "name": "REVRobotics", - "version": "1.5.1", + "version": "1.5.2", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "http://www.revrobotics.com/content/sw/max/sdk/maven/" @@ -11,14 +11,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-java", - "version": "1.5.1" + "version": "1.5.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-driver", - "version": "1.5.1", + "version": "1.5.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-cpp", - "version": "1.5.1", + "version": "1.5.2", "libName": "SparkMax", "headerClassifier": "headers", "sharedLibrary": false, @@ -52,7 +52,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-driver", - "version": "1.5.1", + "version": "1.5.2", "libName": "SparkMaxDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/WPILibOldCommands.json b/vendordeps/WPILibOldCommands.json deleted file mode 100644 index acc8879..0000000 --- a/vendordeps/WPILibOldCommands.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "fileName": "WPILibOldCommands.json", - "name": "WPILib-Old-Commands", - "version": "2020.0.0", - "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-cpp", - "version": "wpilib", - "libName": "wpilibOldCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxaarch64bionic", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxx86-64" - ] - } - ] -} \ No newline at end of file