diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..3d5a824 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..230563b --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + /** Organizes all the non-changing values we use in the code */ + public static final int leftFrontPort = 10; + public static final int leftBackPort = 11; + public static final int rightFrontPort = 12; + public static final int rightBackPort = 13; + +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..8776e5d --- /dev/null +++ b/src/main/java/frc/robot/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..885590d --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,119 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.DriveTrain; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + /** You can leave this in if you want, it only changes anything if you are using the autonomous part of the code, + * if you do remove it, just delete any errors it creates */ + private Command m_autonomousCommand; + + /**Creates an object for our DriveTrain class so we can call things from the class + * make sure to include any variables needed for the parameters + */ + public static DriveTrain m_driveTrain = new DriveTrain(Constants.leftFrontPort, Constants.leftBackPort, Constants.rightFrontPort, Constants.rightBackPort); + /** Similar to the previous line, creates a RobotContainer object so we can call things from the robotcontainer class, + * what you name this is up to you */ + private RobotContainer m_robotContainer; + + + /** The values that we will use from the joystick */ + double xAxis; + double yAxis; + /** Values that will go in the parameters that are needed for the RobotDrive method */ + double leftSpeed; + double rightSpeed; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() {} + + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() { + /** Defining the XAxis and yAxis objects as the x and y values from our joysticks on the controller */ + xAxis = -m_robotContainer.xboxController.getRawAxis(1); + yAxis = m_robotContainer.xboxController.getRawAxis(4); + /** The math required to make the robot turn left and right properly according to the joystick configuration. */ + leftSpeed = yAxis + xAxis; + rightSpeed = yAxis - xAxis; + /** Calling back the robotDrive method from the DriveTrain class so that it runs fifty times a second + * uses the parameters leftSpeed and rightSpeed that we just defined + */ + m_driveTrain.robotDrive(leftSpeed, rightSpeed); + + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..44489ff --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; + +import edu.wpi.first.wpilibj2.command.Command; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + /** Declaring and defining our joystick object */ + public static Joystick xboxController = new Joystick(1); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() {} + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + +} diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java new file mode 100644 index 0000000..ba233bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +/** YOU DO NOT HAVE TO WORRY ABOUT THIS SECTION, USING TAB WILL ADD ANYTHING NEEDED HERE AUTOMATICALLY */ +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + + +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriveTrain extends SubsystemBase { + /** Creates a new DriveTrain. */ + /** Creates a differential drive which allows the two speed controller groups to work together */ + DifferentialDrive diffDrive; + /**Creates a speed controller group which allows you to run multiple motors with one object */ + SpeedControllerGroup leftSide; + SpeedControllerGroup rightSide; + /**Creates the objects for each motor controller */ + CANSparkMax leftFront; + CANSparkMax leftBack; + CANSparkMax rightFront; + CANSparkMax rightBack; + + + public DriveTrain(int leftFrontPort, int leftBackPort, int rightFrontPort, int rightBackPort) { + /** Defining all the motor controllers with the CANSparkMax(port, motor type) method */ + leftFront = new CANSparkMax(leftFrontPort, MotorType.kBrushless); + leftBack = new CANSparkMax(leftBackPort, MotorType.kBrushless); + rightBack = new CANSparkMax(rightBackPort, MotorType.kBrushless); + rightFront = new CANSparkMax(rightFrontPort, MotorType.kBrushless); + /**Defining the two speed controller groups using the SpeedControllerGroup(motor, motor) method */ + leftSide = new SpeedControllerGroup(leftFront, leftBack); + rightSide = new SpeedControllerGroup(rightFront, rightBack); + + /**Defining the differential drive object using the DifferentialDrive(speedcontrollergroup, speedcontrollergroup) method */ + diffDrive = new DifferentialDrive(leftSide, rightSide); + } + + /**Method we will use to actually control the robot, which need the parameters leftSpeed and rightSpeed, which will be defined in robot */ + public void robotDrive(double leftSpeed, double rightSpeed){ + /** A method tied to the DifferentialDrive object in the Wpilib libraries, basically lets us control the motors in a tank drive fashion */ + diffDrive.tankDrive(leftSpeed, rightSpeed); + /** Creates a minimum required output for the joystick to run the motor */ + diffDrive.setDeadband(0.1); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run (50 times a second) + } +} diff --git a/src/main/java/org/team696/robot/Constants.java b/src/main/java/org/team696/robot/Constants.java index 4016734..2055303 100644 --- a/src/main/java/org/team696/robot/Constants.java +++ b/src/main/java/org/team696/robot/Constants.java @@ -57,8 +57,8 @@ public static final class SpindexerConstants { public static final double KickMotorSpeed = 1.; public static final double KickMotorReverseSpeed = -0.1; - public static final double loadingDrumPower = 0.22; - public static final double continuousShootDrumPower = 0.55; + public static final double loadingDrumPower = 0.22 ; + public static final double continuousShootDrumPower = 0.7; public static final double stopDrumPower = 0; public static final double timeBetweenIndex = 1; @@ -167,6 +167,8 @@ public static final class OperatorConstants { public static final int climberDownButton = 17; public static final int colorWheelRotationControl = 18; public static final int driveAssistButton = 19; + public static final int intakeDownButton = 6; + public static final int intakeUpButton = 5; public static final int shooterManualAxis = 3; public static final int turretManualAxis = 2; @@ -218,12 +220,60 @@ public static final class DrivetrainConstants { public static final int slowTasksSpeed = 50; } - public static final class IntakeConstants { - public static final int IntakeMotorPort = 50; + public static final class IntakeConstants { + public static final double leftIntkP = 0.08; + public static final double leftIntkI = 0; + public static final double leftIntkD = 1; + public static final double leftIntIZone = 0; + public static final double leftIntFF = 0; + public static final double leftIntMin = -0.5; + public static final double leftIntMax = 0.5; + + public static final double rightIntkP = 0.08; + public static final double rightIntkI = 0; + public static final double rightIntkD = 1; + public static final double rightIntIZone = 0; + public static final double rightIntFF = 0; + public static final double rightIntMin = -0.5; + public static final double rightIntMax = 0.5; + + public static final int intakeTimeout = 30; + public static final int rollerPIDSlot = 1; + public static final double rollerkF = 0.06; + public static final double rollerkP = 0.6; + public static final double rollerkI = 0.0; + public static final double rollerkD = 0.0; + + public static final double intakeDownPosition = 23; + public static final double intakeUpPosition = 2; + public static final double intakeMidPosition = 13; + + public static final double rollerVelocity = 0.3; + + public static final int leftIntakeMotorPort = 50; + + public static final int rightIntakeMotorPort = 30; public static final boolean IntakeInverted = false; - public static final double intakePower = 0.7; + public static final double intakePower = 0.3; public static final double outtakePower = -1; public static final double stopIntakePower = 0; + } + public static final class ClimberConstants{ + + public static final int climberPIDSlot = 3; + public static final int climberTimeout = 30; + public static final double climberkF = 0; + public static final double climberkP = 0.1; + public static final double climberkI = 0; + public static final double climberkD = 1.0; + public static final double climberPos = 20; + public static final double climberServoUnlock = 105; + public static final double climberServoLock = 1; + + + + + } } diff --git a/src/main/java/org/team696/robot/Robot.java b/src/main/java/org/team696/robot/Robot.java index f869600..062abe1 100644 --- a/src/main/java/org/team696/robot/Robot.java +++ b/src/main/java/org/team696/robot/Robot.java @@ -8,6 +8,7 @@ package org.team696.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -55,6 +56,7 @@ public void robotInit() { */ @Override public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic @@ -71,6 +73,9 @@ public void robotPeriodic() { SmartDashboard.putNumber("Spindexer Current", m_robotContainer.spindexer.getCurrent()); SmartDashboard.putNumber("Hood Angle", m_robotContainer.shooterHood.servoAngle()); SmartDashboard.putNumber("Accelerator Velocity", m_robotContainer.shooter.getAcceleratorVelocity()); + SmartDashboard.putNumber("Right Joystick", m_robotContainer.driverController.getX(Hand.kRight)); + SmartDashboard.putNumber("Left Joystick", m_robotContainer.driverController.getY(Hand.kLeft)); + SmartDashboard.putNumber("Servo Angle ", m_robotContainer.climberServos.getServoAngle()); // SmartDashboard.putNumber("spin pos", m_robotContainer.spindexer.getEncoderPosition()); @@ -132,6 +137,8 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + m_robotContainer.intake.leftIntDepMotor.getEncoder().setPosition(0); + m_robotContainer.intake.rightIntDepMotor.getEncoder().setPosition(0); logger.info("Transitioning to teleop."); m_robotContainer.spindexer.setBrake(true); // SmartDashboard.putNumber("Target RPM", 3900); @@ -146,42 +153,30 @@ public void teleopInit() { @Override public void teleopPeriodic() { - if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.hoodAutoButton)){ - //Previous comp: m_robotContainer.shooterHood.moveServoAngle(51); - m_robotContainer.shooterHood.moveServoAngle(51); + // if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.hoodAutoButton)){ + // m_robotContainer.shooterHood.moveServoAngle(51); - } - else{ - double hood_axis = -Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128*1.5); - if (last_hood_axis == -69.23){ - last_hood_axis = hood_axis; - } - double delta = hood_axis - last_hood_axis; + // } + // else{ + // double hood_axis = -Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128*1.5); + // if (last_hood_axis == -69.23){ + // last_hood_axis = hood_axis; + // } + // double delta = hood_axis - last_hood_axis; - SmartDashboard.putNumber("Hood Axis", Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128)); + // SmartDashboard.putNumber("Hood Axis", Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128)); - if (delta + m_robotContainer.shooterHood.servoAngle() <= 51 && delta + m_robotContainer.shooterHood.servoAngle() >= 10){ - m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta); + // if (delta + m_robotContainer.shooterHood.servoAngle() <= 51 && delta + m_robotContainer.shooterHood.servoAngle() >= 10){ + // m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta); - /* - if (m_robotContainer.shooterHood.servoAngle() >= 51 && delta < 0){ - m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta); - } - else if (m_robotContainer.shooterHood.servoAngle() <= 10 && delta > 0){ - m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta); - } - - if (m_robotContainer.shooterHood.servoAngle() < 51 && m_robotContainer.shooterHood.servoAngle() > 10){ - m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta); - } - */ - } - last_hood_axis = hood_axis; - - - } + + // } + // last_hood_axis = hood_axis; + + + // } // } diff --git a/src/main/java/org/team696/robot/RobotContainer.java b/src/main/java/org/team696/robot/RobotContainer.java index 7375cd7..0df3c73 100644 --- a/src/main/java/org/team696/robot/RobotContainer.java +++ b/src/main/java/org/team696/robot/RobotContainer.java @@ -15,30 +15,42 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import javax.swing.Action; + import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; import org.team696.MathUtils.MathUtils; +import org.team696.robot.Constants.ClimberConstants; import org.team696.robot.Constants.IntakeConstants; import org.team696.robot.Constants.OperatorConstants; import org.team696.robot.Constants.SpindexerConstants; -import org.team696.robot.commands.ATCForCommand; -import org.team696.robot.commands.ATCRevCommand; +// import org.team696.robot.commands.ATCForCommand; +// import org.team696.robot.commands.ATCRevCommand; import org.team696.robot.commands.AutoIndexKickUp; +import org.team696.robot.commands.ClimberPullCommand; import org.team696.robot.commands.Drive; import org.team696.robot.commands.DriveTimer; import org.team696.robot.commands.FireCommand; +import org.team696.robot.commands.FullShoot; +import org.team696.robot.commands.IntakeCommand; +import org.team696.robot.commands.IntakeDownCommand; import org.team696.robot.commands.IntakeTimerCommand; +import org.team696.robot.commands.IntakeUpCommand; +import org.team696.robot.commands.OmniKickDown; import org.team696.robot.commands.OmniKickUp; import org.team696.robot.commands.OmniKickUpTimer; import org.team696.robot.commands.ShooterCommand; import org.team696.robot.commands.ShooterHoodCommand; import org.team696.robot.commands.ShooterPowerCommand; import org.team696.robot.commands.ShooterPrep; -import org.team696.robot.commands.SpinToPocket; +// import org.team696.robot.commands.SpinToPocket; import org.team696.robot.commands.SpindexerLoading; import org.team696.robot.commands.TurretLockOn; import org.team696.robot.commands.TurretManual; +import org.team696.robot.commands.UnlockClimber; +import org.team696.robot.subsystems.Climber; +import org.team696.robot.subsystems.ClimberServos; import org.team696.robot.subsystems.Drivetrain; import org.team696.robot.subsystems.Intake; import org.team696.robot.subsystems.Limelight; @@ -64,7 +76,10 @@ public class RobotContainer { public final TurretSubsystem turretSubsystem = new TurretSubsystem(); public static final Limelight limelight = new Limelight(); public final Intake intake = new Intake(); + public final ClimberServos climberServos = new ClimberServos(); + public final Climber climber = new Climber(); + // Joysticks public final XboxController driverController = new XboxController(OperatorConstants.driverJoystickPort); public static final Joystick operatorPanel = new Joystick(OperatorConstants.operatorPanelPort); @@ -95,6 +110,13 @@ public class RobotContainer { private final JoystickButton automaticShootPrep = new JoystickButton(operatorPanel, OperatorConstants.driveAssistButton); private final JoystickButton automaticIndex = new JoystickButton(operatorPanel, OperatorConstants.colorWheelPositionControl); + private final JoystickButton intakeDownButton = new JoystickButton(operatorPanel, OperatorConstants.intakeDownButton); + private final JoystickButton intakeUpButton = new JoystickButton(operatorPanel, OperatorConstants.intakeUpButton); + + private final JoystickButton driverIntakeButton = new JoystickButton(driverController, 2); + + + public double getDriveSpeed() { // Math to correct for nonlinearities in the controller should happen here. double jsVal = -driverController.getY(Hand.kLeft); @@ -120,9 +142,13 @@ public double turretManual() { } public RobotContainer() { + + Command driveCommand = new Drive(() -> getDriveSpeed(), () -> getDriveTurn(), drivetrain); drivetrain.setDefaultCommand(driveCommand); configureButtonBindings(); + + climberServos.setDefaultCommand(new UnlockClimber(climberServos, ClimberConstants.climberServoLock)); } /** @@ -130,27 +156,34 @@ public RobotContainer() { */ private void configureButtonBindings() { // rpm is 3900 - spinUpButton.whenPressed(new ShooterCommand(shooter, shooter.shootRPM, true)); - spinUpButton.whenReleased(new ShooterPowerCommand(shooter, 0, false)); + spinUpButton.whenPressed(new ShooterCommand(shooter, shooter.shootRPM, true, intake, IntakeConstants.intakeMidPosition, shooterHood, 46)); + spinUpButton.whenReleased(new ShooterPowerCommand(shooter, 0, false, intake, IntakeConstants.intakeUpPosition, shooterHood, -5)); + + // spinUpButton.whenPressed(new FullShoot(turretSubsystem, shooter, intake, shooterHood, limelight)); + // spinUpButton.whenReleased(new ShooterPowerCommand(shooter, 0, false, intake, IntakeConstants.intakeUpPosition)); + turretAutoButton.whileHeld(new TurretLockOn(turretSubsystem, limelight)); turretAutoButton.whenReleased(new TurretManual(turretSubsystem, limelight)); - SpindexerLoading loadingOff = new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.stopIntakePower); - intakeOnButton.whileHeld( - new SpindexerLoading(spindexer, intake, SpindexerConstants.loadingDrumPower, IntakeConstants.intakePower)); - intakeOnButton.whenReleased(loadingOff); - intakeOffButton.whileHeld( - new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.outtakePower)); - intakeOffButton.whenReleased(loadingOff); + SpindexerLoading loadingOff = new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.stopIntakePower, IntakeConstants.intakeUpPosition); + // intakeOnButton.whileHeld( + // new SpindexerLoading(spindexer, intake, SpindexerConstants.loadingDrumPower, IntakeConstants.intakePower, IntakeConstants.intakeDownPosition)); + // intakeOnButton.whenReleased(loadingOff); + + driverIntakeButton.whileHeld(new SpindexerLoading(spindexer, intake, SpindexerConstants.loadingDrumPower, IntakeConstants.intakePower, IntakeConstants.intakeDownPosition)); + driverIntakeButton.whenReleased(loadingOff); + // intakeOffButton.whileHeld( + // new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.outtakePower)); + // intakeOffButton.whenReleased(loadingOff); // maybe have this button cancel the human player loading sequence continuousButton.whileHeld(new SpindexerLoading(spindexer, intake, SpindexerConstants.continuousShootDrumPower, - IntakeConstants.stopIntakePower)); + IntakeConstants.stopIntakePower, IntakeConstants.intakeMidPosition)); continuousButton.whenReleased(loadingOff); automaticShootPrep.whenPressed(new ShooterPrep(shooter, turretSubsystem)); - automaticShootPrep.whenReleased(new ShooterPowerCommand(shooter, 0, false)); + automaticShootPrep.whenReleased(new ShooterPowerCommand(shooter, 0, false, intake, IntakeConstants.intakeUpPosition, shooterHood, -5)); automaticShootPrep.whenReleased(new TurretManual(turretSubsystem, limelight)); automaticIndex.whenPressed(new AutoIndexKickUp(spindexer, intake, SpindexerConstants.continuousShootDrumPower, 0)); @@ -159,20 +192,43 @@ private void configureButtonBindings() { fireButton.whenPressed(new FireCommand(shooter, spindexer, SpindexerConstants.KickMotorSpeed)); fireButton.whenReleased(new FireCommand(shooter, spindexer, SpindexerConstants.stopDrumPower)); - OmniKickUp reverseOmni = new OmniKickUp(spindexer, SpindexerConstants.KickMotorReverseSpeed); - omniReverse.whenPressed(reverseOmni); - omniReverse.whenReleased(new OmniKickUp(spindexer, SpindexerConstants.stopDrumPower)); + // OmniKickUp reverseOmni = new OmniKickUp(spindexer, SpindexerConstants.KickMotorReverseSpeed); + // OmniKickDown reverseOmni = new OmniKickDown(spindexer, -0.1, shooter, -0.5); + omniReverse.whenPressed(new OmniKickDown(spindexer, -0.1, shooter, -0.5)); + omniReverse.whenReleased(new OmniKickDown(spindexer, 0.0, shooter, 0.0)); + + // ATCForButton.toggleWhenPressed(new UnlockClimber(climberServos, ClimberConstants.climberServoUnlock)); + ATCForButton.whileHeld(new UnlockClimber(climberServos, ClimberConstants.climberServoUnlock)); + ATCForButton.whenReleased(new UnlockClimber(climberServos, ClimberConstants.climberServoLock)); + // ATCForButton.whenInactive(command) + + + ATCRevButton.whileHeld(new ClimberPullCommand(climber, 0.5)); + ATCRevButton.whenReleased(new ClimberPullCommand(climber, 0)); + + hoodAutoButton.whileHeld(new ShooterHoodCommand(shooterHood, 46 )); + hoodAutoButton.whenReleased(new ShooterHoodCommand(shooterHood, -5)); + + // intakeDownButton.whenPressed(new IntakeDownCommand(intake)); + // intakeUpButton.whenInactive(new IntakeUpCommand(intake, IntakeConstants.intakeMidPosition)); + // intakeDownButton.whenInactive(new IntakeUpCommand(intake, IntakeConstants.intakeMidPosition)); + // intakeUpButton.whenPressed(new IntakeUpCommand(intake, IntakeConstants.intakeUpPosition)); +// spinUpButton.whenReleased(new IntakeUpCommand(intake, IntakeConstants.intakeMidPosition)); +// intakeOnButton.whenReleased(new IntakeUpCommand(intake, IntakeConstants.intakeMidPosition)); +// intakeOnButton.whenInactive(new IntakeUpCommand(intake, IntakeConstants.intakeUpPosition)); - ATCForCommand ATCfor = new ATCForCommand(spindexer); - ATCRevCommand ATCrev = new ATCRevCommand(spindexer); + // intakeOnButton.whenPressed(new IntakeCommand(intake)); - ATCForButton.whenPressed(ATCfor); - ATCForButton.cancelWhenPressed(ATCrev); + // ATCForCommand ATCfor = new ATCForCommand(spindexer); + // ATCRevCommand ATCrev = new ATCRevCommand(spindexer); - ATCRevButton.whenPressed(ATCrev); - ATCRevButton.cancelWhenPressed(ATCfor); + // ATCForButton.whenPressed(ATCfor); + // ATCForButton.cancelWhenPressed(ATCrev); - intakeOnButton.whenReleased(ATCfor); + // ATCRevButton.whenPressed(ATCrev); + // ATCRevButton.cancelWhenPressed(ATCfor); + + // intakeOnButton.whenReleased(ATCfor); } @@ -184,36 +240,40 @@ private void configureButtonBindings() { @SuppressWarnings("checkstyle:magicnumber") public Command getAutonomousCommand() { - return new SequentialCommandGroup( - - new ParallelCommandGroup( - new TurretLockOn(turretSubsystem, limelight), - new ShooterCommand(shooter, 3900, true), - - new SequentialCommandGroup( - new ShooterHoodCommand(shooterHood, 51), - // new IntakeTimerCommand(intake, 50); - new SpinToPocket(spindexer, 1), - new OmniKickUpTimer(spindexer, true, 25), - new SpinToPocket(spindexer, 2), - new OmniKickUpTimer(spindexer, true, 25), - new SpinToPocket(spindexer, 3), - new OmniKickUpTimer(spindexer, true, 25) - ) - ), - - new DriveTimer(drivetrain, -0.5, 0, 50), - new DriveTimer(drivetrain, 0, 0.7, 40), - new DriveTimer(drivetrain, 0.5, 0, 25), - new ParallelCommandGroup( - new DriveTimer(drivetrain, 0.5, 0, 35), - new IntakeTimerCommand(intake, 35) - ) + return new IntakeUpCommand(intake, IntakeConstants.intakeUpPosition).withTimeout(1).andThen(new DriveTimer(drivetrain, 0.25, 0, 50000).withTimeout(2.5)); + // } + // return new SequentialCommandGroup( + + // new ParallelCommandGroup( + // // new TurretLockOn(turretSubsystem, limelight), + // // new ShooterCommand(shooter, 3900, true, intake, IntakeConstants.intakeMidPosition), + + // // new SequentialCommandGroup( + // // new ShooterHoodCommand(shooterHood, 51), + // // new IntakeTimerCommand(intake, 50); + // // new SpinToPocket(spindexer, 1), + // // new OmniKickUpTimer(spindexer, true, 25), + // // new SpinToPocket(spindexer, 2), + // // new OmniKickUpTimer(spindexer, true, 25), + // // new SpinToPocket(spindexer, 3), + // // new OmniKickUpTimer(spindexer, true, 25) + // ) + // ), + + // new DriveTimer(drivetrain, -0.5, 0, 50), + // new DriveTimer(drivetrain, 0, 0.7, 40), + // new DriveTimer(drivetrain, 0.5, 0, 25), + // new ParallelCommandGroup( + // new DriveTimer(drivetrain, 0.5, 0, 35), + // new IntakeTimerCommand(intake, 35) + // ) + + - - ); - } + // ); + + public boolean getDriveMode() { return driveMode.get(); diff --git a/src/main/java/org/team696/robot/commands/ATCForCommand.java b/src/main/java/org/team696/robot/commands/ATCForCommand.java deleted file mode 100644 index d7d6ed1..0000000 --- a/src/main/java/org/team696/robot/commands/ATCForCommand.java +++ /dev/null @@ -1,70 +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 org.team696.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; - -import org.team696.robot.Constants; -import org.team696.robot.subsystems.Spindexer; - -public class ATCForCommand extends CommandBase { - /** - * Creates a new SpinToIndex. - */ - private static final Logger logger = LogManager.getLogger(ATCForCommand.class); - Spindexer spindexer; - - int oldSpindexerPocket = 0; - - public ATCForCommand(Spindexer spindexer) { - this.spindexer = spindexer; - addRequirements(spindexer); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - oldSpindexerPocket = spindexer.getTargetPocket(); - int targetPocket; - if (spindexer.getTargetPocket() == Constants.SpindexerConstants.nPockets) { - targetPocket = 1; - } else { - targetPocket = spindexer.getTargetPocket() + 1; - } - logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket)); - spindexer.setTargetPocket(targetPocket); - } - - // 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) { - if(interrupted){ - logger.info("Spindexer command interrupted."); - } else { - logger.info("Spindexer on target."); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return spindexer.isOnTarget(); - } -} diff --git a/src/main/java/org/team696/robot/commands/ATCRevCommand.java b/src/main/java/org/team696/robot/commands/ATCRevCommand.java deleted file mode 100644 index beb0e54..0000000 --- a/src/main/java/org/team696/robot/commands/ATCRevCommand.java +++ /dev/null @@ -1,63 +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 org.team696.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; - -import org.team696.robot.Constants; -import org.team696.robot.subsystems.Spindexer; - -public class ATCRevCommand extends CommandBase { - private static final Logger logger = LogManager.getLogger(ATCRevCommand.class); - Spindexer spindexer; - int oldSpindexerPocket; - - public ATCRevCommand(Spindexer spindexer) { - this.spindexer = spindexer; - addRequirements(spindexer); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - oldSpindexerPocket = spindexer.getTargetPocket(); - int targetPocket; - if (spindexer.getTargetPocket() == 1) { - targetPocket = Constants.SpindexerConstants.nPockets; - } else { - targetPocket = spindexer.getTargetPocket() - 1; - } - logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket)); - spindexer.setTargetPocket(targetPocket); - } - - // 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) { - if(interrupted){ - logger.info("Spindexer command interrupted."); - } else { - logger.info("Spindexer on target."); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return spindexer.isOnTarget(); - } -} diff --git a/src/main/java/org/team696/robot/commands/ClimberLift.java b/src/main/java/org/team696/robot/commands/ClimberLift.java deleted file mode 100644 index 18d54c8..0000000 --- a/src/main/java/org/team696/robot/commands/ClimberLift.java +++ /dev/null @@ -1,69 +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 org.team696.robot.commands; - - -// import org.team696.robot.subsystems.ClimberSubsystem; -// import org.team696.robot.RobotContainer; -// import org.team696.robot.Constants.OperatorConstants; -// import org.team696.robot.subsystems.ClimberServos; - -// import edu.wpi.first.wpilibj2.command.CommandBase; - -// public class ClimberLift extends CommandBase { -// private ClimberSubsystem climberSubsystem; -// private ClimberServos climberServos; -// /** -// * Creates a new ClimberPullUp. -// */ -// public ClimberLift(ClimberSubsystem climberSubsystem, ClimberServos climberServos) { -// this.climberSubsystem = climberSubsystem; -// this.climberServos = climberServos; -// addRequirements(climberSubsystem); -// addRequirements(climberServos); -// } - -// // 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() { -// // servoSubsystem.closeServo(); -// // if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton)){ - -// climberSubsystem.setClimberPower(); -// // System.out.println(climberSubsystem.climberTargetPower); - -// // } -// // else{ -// // climberSubsystem.setClimberPower(0); -// // // System.out.println(climberSubsystem.climberTargetPower); - -// // } - - -// } - - - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) { -// // servoSubsystem.CloseServo(); - -// } - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } diff --git a/src/main/java/org/team696/robot/commands/ClimberPullCommand.java b/src/main/java/org/team696/robot/commands/ClimberPullCommand.java new file mode 100644 index 0000000..afd2ba5 --- /dev/null +++ b/src/main/java/org/team696/robot/commands/ClimberPullCommand.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.commands; + +import org.team696.robot.subsystems.Climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ClimberPullCommand extends CommandBase { + Climber climber; + double percent; + + + /** Creates a new ClimberPullCommand. */ + public ClimberPullCommand(Climber climber, double percent ) { + this.climber = climber; + this.percent = percent; + addRequirements(climber); + // 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() { + climber.RunClimber(percent); + } + + // 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/org/team696/robot/commands/ContinuousShoot.java b/src/main/java/org/team696/robot/commands/ContinuousShoot.java index e131fcd..56b927f 100644 --- a/src/main/java/org/team696/robot/commands/ContinuousShoot.java +++ b/src/main/java/org/team696/robot/commands/ContinuousShoot.java @@ -30,7 +30,7 @@ public ContinuousShoot(Spindexer spindexer, double drumSpeed, boolean kickUpStat // Called when the command is initially scheduled. @Override public void initialize() { - timer = 0; + // timer = 0;/ } // Called every time the scheduler runs while the command is scheduled. @@ -41,15 +41,20 @@ public void execute() { spindexer.spindexerLoadingAntiJam(drumSpeed); } - spindexer.setKickMotor(1); + while(spindexer.kickupIsOn == true){ + spindexer.kickMotor.setInverted(true); + spindexer.setKickMotor(.05); + } + // spindexer.setKickMotor(1); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + spindexer.kickMotor.setInverted(false); spindexer.setKickMotor(0); - timer = 0; + // timer = 0; } diff --git a/src/main/java/org/team696/robot/commands/FireCommand.java b/src/main/java/org/team696/robot/commands/FireCommand.java index 00e140e..06872f0 100644 --- a/src/main/java/org/team696/robot/commands/FireCommand.java +++ b/src/main/java/org/team696/robot/commands/FireCommand.java @@ -32,22 +32,24 @@ public FireCommand(Shooter shooter, Spindexer spindexer, double kickMotorPower) // Called when the command is initially scheduled. @Override public void initialize() { + spindexer.kickMotor.setInverted(false); + spindexer.kickupIsOn = true; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(shooter.isUpToSpeed()){ + // if(shooter.isUpToSpeed()){ // shooter.setAcceleratorPower(true); spindexer.setKickMotor(power); // spindexer.spindexerLoadingAntiJam(0.5, 20); - } - else{ - spindexer.setKickMotor(0); + // // } + // else{ + // spindexer.setKickMotor(0); - } + // } } @@ -55,6 +57,8 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + spindexer.setKickMotor(0); + spindexer.kickupIsOn = false; } // Returns true when the command should end. diff --git a/src/main/java/org/team696/robot/commands/FullShoot.java b/src/main/java/org/team696/robot/commands/FullShoot.java index 1c02161..7d6ef8a 100644 --- a/src/main/java/org/team696/robot/commands/FullShoot.java +++ b/src/main/java/org/team696/robot/commands/FullShoot.java @@ -1,44 +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. */ -/*----------------------------------------------------------------------------*/ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. package org.team696.robot.commands; +import org.team696.robot.Constants.IntakeConstants; +import org.team696.robot.subsystems.Intake; import org.team696.robot.subsystems.Limelight; +import org.team696.robot.subsystems.Shooter; import org.team696.robot.subsystems.ShooterHood; -import org.team696.robot.subsystems.Spindexer; import org.team696.robot.subsystems.TurretSubsystem; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.Constants; // 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 FullShoot extends SequentialCommandGroup { - /** - * Creates a new FullShoot. - */ - public FullShoot(ShooterHood shooterHood, Spindexer spindexer, TurretSubsystem turretSubsystem, Limelight limelight) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - super( - new ShooterHoodCommand(shooterHood, 51), - new SpinToPocket(spindexer, 1), - new TurretLockOn(turretSubsystem, limelight), - new OmniKickUpTimer(spindexer, true, 50), - new SpinToPocket(spindexer, 2), - new OmniKickUpTimer(spindexer, true, 50), - new SpinToPocket(spindexer, 3), - new OmniKickUpTimer(spindexer, true, 50), - new SpinToPocket(spindexer, 4), - new OmniKickUpTimer(spindexer, true, 50), - new SpinToPocket(spindexer, 5), - new OmniKickUpTimer(spindexer, true, 50), - new ShooterHoodCommand(shooterHood, 10) +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class FullShoot extends ParallelCommandGroup { + // TurretSubsystem turretSubsystem; + // Shooter shooter; + // Intake intake; + // ShooterHood shooterHood; + // Limelight limelight; - ); + // /** Creates a new FullShoot. */ + // public FullShoot(TurretSubsystem turretSubsystem, Shooter shooter, Intake intake, ShooterHood shooterHood, Limelight limelight) { + // this.turretSubsystem = turretSubsystem; + // this.shooter = shooter; + // this.intake = intake; + // this.shooterHood = shooterHood; + // this.limelight = limelight; + + // addRequirements(turretSubsystem, shooter, intake, shooterHood, limelight); + // // Add your commands in the addCommands() call, e.g. + // // addCommands(new FooCommand(), new BarCommand()); + // addCommands(new ShooterHoodCommand(shooterHood, 51), new TurretLockOn(turretSubsystem, limelight), new ShooterCommand(shooter, shooter.shootRPM, true, intake, IntakeConstants.intakeMidPosition) ); } -} +// } diff --git a/src/main/java/org/team696/robot/commands/IntakeCommand.java b/src/main/java/org/team696/robot/commands/IntakeCommand.java index c5781c2..00f5e48 100644 --- a/src/main/java/org/team696/robot/commands/IntakeCommand.java +++ b/src/main/java/org/team696/robot/commands/IntakeCommand.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; +import org.team696.robot.Constants.IntakeConstants; import org.team696.robot.subsystems.Intake; public class IntakeCommand extends CommandBase { @@ -18,12 +19,10 @@ public class IntakeCommand extends CommandBase { */ private Intake intake; - private double intakePower; - public IntakeCommand(Intake intake, double intakePower) { + public IntakeCommand(Intake intake) { // Use addRequirements() here to declare subsystem dependencies. this.intake = intake; - this.intakePower = intakePower; addRequirements(intake); } @@ -35,7 +34,9 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - intake.runIntake(intakePower); + intake.runIntake(IntakeConstants.rollerVelocity); + intake.moveIntake(IntakeConstants.intakeDownPosition); + intake.MoveRightIntake(IntakeConstants.intakeDownPosition); // spindexer.spindexerLoadingAntiJam(power, current); // System.out.println("loading"); } @@ -43,6 +44,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + intake.runIntake(0); } // Returns true when the command should end. diff --git a/src/main/java/org/team696/robot/commands/IntakeDownCommand.java b/src/main/java/org/team696/robot/commands/IntakeDownCommand.java new file mode 100644 index 0000000..7306bcc --- /dev/null +++ b/src/main/java/org/team696/robot/commands/IntakeDownCommand.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.commands; + +import org.team696.robot.Constants.IntakeConstants; +import org.team696.robot.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.CommandBase; +// import frc.robot.Constants; + +public class IntakeDownCommand extends CommandBase { + Intake intake; + /** Creates a new IntakeDownCommand. */ + public IntakeDownCommand(Intake intake) { + this.intake = intake; + addRequirements(intake); + // 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() { + intake.MoveRightIntake(IntakeConstants.intakeDownPosition); + intake.moveIntake(IntakeConstants.intakeDownPosition); + } + + // 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/org/team696/robot/commands/IntakeUpCommand.java b/src/main/java/org/team696/robot/commands/IntakeUpCommand.java new file mode 100644 index 0000000..d55d392 --- /dev/null +++ b/src/main/java/org/team696/robot/commands/IntakeUpCommand.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.commands; + +import org.team696.robot.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeUpCommand extends CommandBase { + Intake intake; + double position; + + /** Creates a new IntakeUpCommand. */ + public IntakeUpCommand(Intake intake, double position) { + this.intake = intake; + this.position = position; + addRequirements(intake); + // 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() { + intake.moveIntake(position); + intake.MoveRightIntake(position); + } + + // 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/org/team696/robot/commands/OmniKickDown.java b/src/main/java/org/team696/robot/commands/OmniKickDown.java new file mode 100644 index 0000000..5ac69f8 --- /dev/null +++ b/src/main/java/org/team696/robot/commands/OmniKickDown.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.commands; + +import org.team696.robot.subsystems.Shooter; +import org.team696.robot.subsystems.Spindexer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class OmniKickDown extends CommandBase { + Spindexer spindexer; + Shooter shooter; + + + double power; + double acceleratorPower; + + + /** Creates a new OmniKickDown. */ + public OmniKickDown( Spindexer spindexer, double power, Shooter shooter, double acceleratorPower ) { + this.spindexer = spindexer; + this.shooter = shooter; + + addRequirements(spindexer, shooter); + + // 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() { + spindexer.setKickMotor(power); + shooter.setAcceleratorPower(acceleratorPower); + + } + + // 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/org/team696/robot/commands/ShooterCommand.java b/src/main/java/org/team696/robot/commands/ShooterCommand.java index de47a1b..dc6763d 100644 --- a/src/main/java/org/team696/robot/commands/ShooterCommand.java +++ b/src/main/java/org/team696/robot/commands/ShooterCommand.java @@ -9,12 +9,16 @@ import edu.wpi.first.wpilibj2.command.CommandBase; + import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; import org.team696.robot.RobotContainer; +import org.team696.robot.Constants.IntakeConstants; import org.team696.robot.Constants.OperatorConstants; import org.team696.robot.Constants.ShooterConstants; +import org.team696.robot.subsystems.Intake; import org.team696.robot.subsystems.Shooter; +import org.team696.robot.subsystems.ShooterHood; public class ShooterCommand extends CommandBase { /** @@ -22,26 +26,41 @@ public class ShooterCommand extends CommandBase { */ private static final Logger logger = LogManager.getLogger(ShooterCommand.class); private Shooter shooter; + private Intake intake; + private ShooterHood shooterHood; + double RPM; boolean state; - public ShooterCommand(Shooter shooter, double RPM, boolean state) { + double position; + double angle; + double timer; + public ShooterCommand(Shooter shooter, double RPM, boolean state, Intake intake, double position, ShooterHood shooterHood, double angle) { this.shooter = shooter; this.state = state; this.RPM = RPM; + this.intake = intake; + this.position = position; + this.shooterHood = shooterHood; + this.angle = angle; + addRequirements(shooter); + addRequirements(intake); + addRequirements(shooterHood); } // Called when the command is initially scheduled. @Override public void initialize() { + timer = 0; logger.info(String.format("Spinning up shooter to %f RPM", RPM)); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + timer++; // if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterAutoButton)){ shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(shooter.getShootRPM())); // } @@ -54,6 +73,11 @@ public void execute() { // } shooter.setAcceleratorPower(state); + intake.moveIntake(position); + intake.MoveRightIntake(position); + + shooterHood.moveServoAngle(angle); + //TODO: make another command to run off of feedforward gains } diff --git a/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java b/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java index 41fc4c5..45c7d22 100644 --- a/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java +++ b/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java @@ -9,21 +9,39 @@ import edu.wpi.first.wpilibj2.command.CommandBase; +import org.team696.robot.Constants.IntakeConstants; +import org.team696.robot.subsystems.Intake; import org.team696.robot.subsystems.Shooter; +import org.team696.robot.subsystems.ShooterHood; public class ShooterPowerCommand extends CommandBase { /** * Creates a new ShooterPowerCommand. */ private Shooter shooter; + private Intake intake; + private ShooterHood shooterHood; + private double angle; private double power; private boolean state; - public ShooterPowerCommand(Shooter shooter, double power, boolean state) { + private double position; + public ShooterPowerCommand(Shooter shooter, double power, boolean state, Intake intake, double position, ShooterHood shooterHood , double angle) { // Use addRequirements() here to declare subsystem dependencies. this.shooter = shooter; + this.intake = intake; this.state = state; this.power = power; + this.position = position; + this.shooterHood = shooterHood; + this.angle = angle; + + + + + addRequirements(shooter); + addRequirements(intake); + addRequirements(shooterHood); } // Called when the command is initially scheduled. @@ -36,6 +54,11 @@ public void initialize() { public void execute() { shooter.setShooterPower(power); shooter.setAcceleratorPower(state); + intake.MoveRightIntake(position); + intake.moveIntake(position); + shooterHood.moveServoAngle(angle); + + } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/team696/robot/commands/SpinToPocket.java b/src/main/java/org/team696/robot/commands/SpinToPocket.java deleted file mode 100644 index 5ff11af..0000000 --- a/src/main/java/org/team696/robot/commands/SpinToPocket.java +++ /dev/null @@ -1,71 +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 org.team696.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; - -import org.team696.robot.Constants; -import org.team696.robot.subsystems.Spindexer; - -public class SpinToPocket extends CommandBase { - /** - * Creates a new SpinToIndex. - */ - private static final Logger logger = LogManager.getLogger(ATCForCommand.class); - Spindexer spindexer; - - int targetPocket; - - public SpinToPocket(Spindexer spindexer, int targetPocket) { - this.spindexer = spindexer; - this.targetPocket = targetPocket; - addRequirements(spindexer); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - // oldSpindexerPocket = spindexer.getTargetPocket(); - // int targetPocket; - // if (spindexer.getTargetPocket() == Constants.SpindexerConstants.nPockets) { - // targetPocket = 1; - // } else { - // targetPocket = spindexer.getTargetPocket() + 1; - // } - // logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket)); - // spindexer.setTargetPocket(targetPocket); - spindexer.setTargetPocket(targetPocket); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - //System.out.println("spinning to index stuff"); - //System.out.println(spindexer.getCurrentPocket()); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - if(interrupted){ - logger.info("Spindexer command interrupted."); - } else { - logger.info("Spindexer on target."); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return spindexer.getCurrentPocket()==targetPocket; - } -} diff --git a/src/main/java/org/team696/robot/commands/SpindexerLoading.java b/src/main/java/org/team696/robot/commands/SpindexerLoading.java index bad50e9..e005cca 100644 --- a/src/main/java/org/team696/robot/commands/SpindexerLoading.java +++ b/src/main/java/org/team696/robot/commands/SpindexerLoading.java @@ -8,6 +8,7 @@ package org.team696.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; import org.team696.robot.subsystems.Intake; import org.team696.robot.subsystems.Spindexer; @@ -20,12 +21,14 @@ public class SpindexerLoading extends CommandBase { Intake intake; double spindexerPower; double intakePower; - public SpindexerLoading(Spindexer spindexer, Intake intake, double spindexerPower, double intakePower) { + double position; + public SpindexerLoading(Spindexer spindexer, Intake intake, double spindexerPower, double intakePower, double position) { // Use addRequirements() here to declare subsystem dependencies. this.spindexerPower = spindexerPower; this.intakePower = intakePower; this.spindexer = spindexer; this.intake = intake; + this.position = position; addRequirements(spindexer); addRequirements(intake); } @@ -40,6 +43,9 @@ public void initialize() { public void execute() { spindexer.spindexerLoadingAntiJam(spindexerPower); intake.runIntake(intakePower); + intake.moveIntake(position); + intake.MoveRightIntake(position); + // System.out.println("running the loading stuff"); } diff --git a/src/main/java/org/team696/robot/commands/UnlockClimber.java b/src/main/java/org/team696/robot/commands/UnlockClimber.java new file mode 100644 index 0000000..92202a4 --- /dev/null +++ b/src/main/java/org/team696/robot/commands/UnlockClimber.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.commands; + +import org.team696.robot.subsystems.ClimberServos; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class UnlockClimber extends CommandBase { + ClimberServos climberServos; + double angle; + /** Creates a new UnlockClimber. */ + public UnlockClimber(ClimberServos climberServos, double angle) { + this.climberServos = climberServos; + this.angle = angle; + addRequirements(climberServos); + // 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() { + climberServos.UnlockClimber(angle); + } + + // 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/org/team696/robot/subsystems/Climber.java b/src/main/java/org/team696/robot/subsystems/Climber.java new file mode 100644 index 0000000..fc53f15 --- /dev/null +++ b/src/main/java/org/team696/robot/subsystems/Climber.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import org.team696.robot.Constants.ClimberConstants; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Climber extends SubsystemBase { + WPI_TalonFX rightClimberMotor; + WPI_TalonFX leftClimberMotor; + + /** Creates a new Climber. */ + public Climber() { + + leftClimberMotor = new WPI_TalonFX(30); + rightClimberMotor = new WPI_TalonFX(20); + + leftClimberMotor.configFactoryDefault(); + leftClimberMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, ClimberConstants.climberTimeout); + leftClimberMotor.setSensorPhase(true);; + leftClimberMotor.setInverted(true);; + leftClimberMotor.configNominalOutputForward(0, ClimberConstants.climberTimeout); + leftClimberMotor.configNominalOutputReverse(0, ClimberConstants.climberTimeout); + leftClimberMotor.configPeakOutputForward(1, ClimberConstants.climberTimeout); + leftClimberMotor.configPeakOutputReverse(-1, ClimberConstants.climberTimeout); + leftClimberMotor.configAllowableClosedloopError(ClimberConstants.climberPIDSlot, 1, ClimberConstants.climberTimeout); + leftClimberMotor.config_kF(ClimberConstants.climberPIDSlot, ClimberConstants.climberkF, ClimberConstants.climberTimeout); + leftClimberMotor.config_kP(ClimberConstants.climberPIDSlot, ClimberConstants.climberkP, ClimberConstants.climberTimeout); + leftClimberMotor.config_kI(ClimberConstants.climberPIDSlot, ClimberConstants.climberkI, ClimberConstants.climberTimeout); + leftClimberMotor.config_kD(ClimberConstants.climberPIDSlot, ClimberConstants.climberkD, ClimberConstants.climberTimeout); + + rightClimberMotor.configFactoryDefault(); + rightClimberMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, ClimberConstants.climberTimeout); + rightClimberMotor.setSensorPhase(true); + rightClimberMotor.setInverted(true); + rightClimberMotor.follow(leftClimberMotor); + rightClimberMotor.configNominalOutputForward(0, ClimberConstants.climberTimeout); + rightClimberMotor.configNominalOutputReverse(0, ClimberConstants.climberTimeout); + rightClimberMotor.configPeakOutputForward(1, ClimberConstants.climberTimeout); + rightClimberMotor.configPeakOutputReverse(-1, ClimberConstants.climberTimeout); + rightClimberMotor.configAllowableClosedloopError(ClimberConstants.climberPIDSlot, 1, ClimberConstants.climberTimeout); + rightClimberMotor.config_kF(ClimberConstants.climberPIDSlot, ClimberConstants.climberkF, ClimberConstants.climberTimeout); + rightClimberMotor.config_kP(ClimberConstants.climberPIDSlot, ClimberConstants.climberkP, ClimberConstants.climberTimeout); + rightClimberMotor.config_kI(ClimberConstants.climberPIDSlot, ClimberConstants.climberkI, ClimberConstants.climberTimeout); + rightClimberMotor.config_kD(ClimberConstants.climberPIDSlot, ClimberConstants.climberkD, ClimberConstants.climberTimeout); + + } + + public void RunClimber(double per){ + leftClimberMotor.set(TalonFXControlMode.PercentOutput, per); + // rightClimberMotor.set(TalonFXControlMode.PercentOutput, per); + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/team696/robot/subsystems/ClimberServos.java b/src/main/java/org/team696/robot/subsystems/ClimberServos.java index 8d370de..8ba3b1a 100644 --- a/src/main/java/org/team696/robot/subsystems/ClimberServos.java +++ b/src/main/java/org/team696/robot/subsystems/ClimberServos.java @@ -1,54 +1,31 @@ -/*----------------------------------------------------------------------------*/ -/* 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 org.team696.robot.subsystems; - -// import edu.wpi.first.wpilibj.Servo; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// import org.team696.robot.Constants; -// import org.team696.robot.Constants.ClimberConstants; - -// public class ClimberServos extends SubsystemBase { -// private Servo leftClimberServo; -// private Servo rightClimberServo; - -// /** -// * Constructor for ServoSubsystem, defines servos -// */ -// public ClimberServos() { -// leftClimberServo = new Servo(ClimberConstants.leftClimberServoPort); -// rightClimberServo = new Servo(ClimberConstants.rightClimberServoPort); - -// } - -// /** -// * Opens the servos to allow the climber to move down and lift the robot up -// */ -// public void openServo(){ -// leftClimberServo.set(ClimberConstants.leftServoOpen); -// rightClimberServo.set(ClimberConstants.rightServoOpen); -// } - - -// public void closeServo(){ -// leftClimberServo.set(ClimberConstants.leftServoClosed); -// rightClimberServo.set(ClimberConstants.rightServoClosed); -// } - -// public void moveClimberServos(double angle){ -// // leftClimberServo.setAngle(angle); -// rightClimberServo.setAngle(angle); -// System.out.println("running servo to "+angle); - -// } - -// @Override -// public void periodic() { -// // This method will be called once per scheduler run -// } -// } +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.team696.robot.subsystems; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimberServos extends SubsystemBase { + private Servo climbServo; + /** Creates a new ClimberServos. */ + public ClimberServos() { + climbServo = new Servo(4); + } + + public void UnlockClimber(double angle){ + climbServo.setAngle(angle); + // climbServo.setPosition(0.5 ); + } + + public double getServoAngle(){ + return climbServo.getAngle(); + } + + @Override + public void periodic() { + // climbServo.setAngle(degrees); + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/team696/robot/subsystems/ClimberSubsystem.java b/src/main/java/org/team696/robot/subsystems/ClimberSubsystem.java deleted file mode 100644 index c21fe71..0000000 --- a/src/main/java/org/team696/robot/subsystems/ClimberSubsystem.java +++ /dev/null @@ -1,224 +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 org.team696.robot.subsystems; - -// import com.revrobotics.CANEncoder; -// import com.revrobotics.CANSparkMax; -// import com.revrobotics.CANSparkMax.FaultID; -// import com.revrobotics.CANSparkMax.IdleMode; -// import com.revrobotics.CANSparkMaxLowLevel.MotorType; -// import com.revrobotics.EncoderType; -// import com.revrobotics.Rev2mDistanceSensor; -// import com.revrobotics.Rev2mDistanceSensor.Port; -// import com.revrobotics.Rev2mDistanceSensor.Unit; - -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// import org.team696.robot.Constants; -// import org.team696.robot.Constants.ClimberConstants; -// import org.team696.robot.Constants.OperatorConstants; -// import org.team696.robot.RobotContainer; - -// public class ClimberSubsystem extends SubsystemBase { -// /** -// * Creates a new ClimberSubsystem. -// */ - -// public CANSparkMax leftClimberMotor; -// private CANSparkMax rightClimberMotor; - -// private CANEncoder leftClimbEncoder; -// private CANEncoder rightClimbEncoder; - -// // private ElevatorFeedforward climberFeedForward; -// // private Rev2mDistanceSensor climberLatchSensor; - -// public double climberTargetPower = 0; - -// private enum OperatorClimberStates { -// REACH, LIFT, OFF, BOTH_ON -// } - -// private OperatorClimberStates climberStates = OperatorClimberStates.OFF; - -// public ClimberSubsystem() { -// leftClimberMotor = new CANSparkMax(Constants.ClimberConstants.leftClimberPort, MotorType.kBrushed); - -// // leftClimbEncoder.setPositionConversionFactor(Constants.ClimberConstants.positionConversionFactor); -// // leftClimbEncoder.setVelocityConversionFactor(Constants.ClimberConstants.velocityConversionFactor); - -// rightClimberMotor = new CANSparkMax(Constants.ClimberConstants.rightClimberPort, MotorType.kBrushed); -// configRightClimber(); -// configLeftClimber(); - -// leftClimbEncoder = new CANEncoder(leftClimberMotor, EncoderType.kNoSensor, 10); -// rightClimbEncoder = new CANEncoder(rightClimberMotor, EncoderType.kNoSensor, 10); -// // rightClimbEncoder.setPositionConversionFactor(Constants.ClimberConstants.positionConversionFactor); -// // rightClimbEncoder.setVelocityConversionFactor(Constants.ClimberConstants.velocityConversionFactor); - -// // climberFeedForward = new ElevatorFeedforward(Constants.ClimberConstants.kS, -// // Constants.ClimberConstants.kG, Constants.ClimberConstants.kV, -// // Constants.ClimberConstants.kA); - -// // climberLatchSensor = new Rev2mDistanceSensor(Port.kOnboard); -// } - -// /** -// * Initializes climber subsystem. -// */ -// public void initialize() { -// leftClimbEncoder.setPosition(0); -// rightClimbEncoder.setPosition(0); -// } - -// // public double distanceRange() { -// // return climberLatchSensor.getRange(Unit.kMillimeters); -// // } - -// public double leftEncoderPosition() { -// return leftClimbEncoder.getPosition(); -// } - -// public double rightEncoderPosition() { -// return rightClimbEncoder.getPosition(); -// } - -// public double leftEncoderVelocity() { -// return leftClimbEncoder.getVelocity(); -// } - -// public double rightEncoderVelocity() { -// return rightClimbEncoder.getVelocity(); -// } - -// public double leftCurrent() { -// return leftClimberMotor.getOutputCurrent(); -// } - -// public double rightCurrent() { -// return rightClimberMotor.getOutputCurrent(); -// } - -// public double climberAppliedPower(){ -// return leftClimberMotor.getAppliedOutput(); -// } - -// /** -// * Sets climberTargetPower based on climber state. -// */ -// public void updateClimberPower() { -// switch (climberStates) { -// case REACH: -// climberTargetPower = ClimberConstants.climberReachTargetPower; -// break; - -// case LIFT: -// climberTargetPower = ClimberConstants.climberLiftTargetPower; -// break; - -// case OFF: -// climberTargetPower = 0; -// break; - -// case BOTH_ON: -// climberTargetPower = 0; -// break; - -// default: -// climberTargetPower = 0; -// break; - -// } - -// } -// /** -// * Sets open-loop climber power -// */ -// public void setClimberPower() { -// leftClimberMotor.set(climberTargetPower); -// System.out.println(climberTargetPower); -// // rightClimberMotor.set(climberTargetPower); - -// } - -// /** -// * Sets open-loop climber power. -// * @param power Throttle value -// */ -// public void setClimberPower(double power) { -// leftClimberMotor.set(power); -// System.out.println(power); -// // rightClimberMotor.set(power); - -// } - -// // might add sparkmax built in pid velocity control TBD with testing -// public void setClimberVelocity() { - -// } - -// @Override -// public void periodic() { - -// if (leftClimberMotor.getFault(FaultID.kHasReset)) { -// configLeftClimber(); -// } -// if (rightClimberMotor.getFault(FaultID.kHasReset)) { -// configRightClimber(); -// } - -// if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberDownButton) -// && !RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton)) { -// climberStates = OperatorClimberStates.REACH; -// } else if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton) -// && !RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberDownButton)) { -// climberStates = OperatorClimberStates.LIFT; -// } else if (!RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberDownButton) -// && !RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton)) { -// climberStates = OperatorClimberStates.OFF; -// } else if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberDownButton) -// && RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton)) { -// climberStates = OperatorClimberStates.BOTH_ON; -// } else { -// climberStates = OperatorClimberStates.OFF; -// } - -// updateClimberPower(); - -// } - -// /** -// * Sets up left climber motor. -// */ -// public void configLeftClimber() { -// leftClimberMotor.restoreFactoryDefaults(); -// leftClimberMotor.setInverted(ClimberConstants.leftClimberInverted); -// leftClimberMotor.setIdleMode(IdleMode.kBrake); -// leftClimberMotor.setMotorType(MotorType.kBrushed); -// // leftClimberMotor.setSmartCurrentLimit(80); - -// // leftClimberMotor.setOpenLoopRampRate(ClimberConstants.openLoopRampRate); -// // leftClimberMotor.follow(rightClimberMotor, ClimberConstants.leftClimberInverted); -// } - -// /** -// * Sets up right climber motor. -// */ -// public void configRightClimber() { -// rightClimberMotor.restoreFactoryDefaults(); -// rightClimberMotor.setInverted(ClimberConstants.leftClimberInverted); - -// rightClimberMotor.setIdleMode(IdleMode.kBrake); -// rightClimberMotor.setMotorType(MotorType.kBrushed); -// // rightClimberMotor.setOpenLoopRampRate(ClimberConstants.openLoopRampRate); - -// rightClimberMotor.follow(leftClimberMotor, false); - -// } -// } diff --git a/src/main/java/org/team696/robot/subsystems/Intake.java b/src/main/java/org/team696/robot/subsystems/Intake.java index 6f275c8..51e110f 100644 --- a/src/main/java/org/team696/robot/subsystems/Intake.java +++ b/src/main/java/org/team696/robot/subsystems/Intake.java @@ -7,7 +7,13 @@ package org.team696.robot.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import org.team696.robot.Constants; @@ -15,26 +21,92 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { + public CANSparkMax leftIntDepMotor; + public CANSparkMax rightIntDepMotor; + + public CANPIDController leftIntPID; + public CANPIDController rightIntPID; + + public WPI_TalonFX intakeRollerMotor; + /** * Creates a new IntakeSubsystem. */ - private CANSparkMax intakeMotor; + // private CANSparkMax intakeMotor; public Intake() { - intakeMotor = new CANSparkMax(Constants.IntakeConstants.IntakeMotorPort, MotorType.kBrushless); - intakeMotor.restoreFactoryDefaults(); - intakeMotor.setInverted(Constants.IntakeConstants.IntakeInverted); - } + leftIntDepMotor = new CANSparkMax(50, MotorType.kBrushless); + rightIntDepMotor = new CANSparkMax(30, MotorType.kBrushless); + // rightIntDepMotor.getEncoder().setPosition(0); + // leftIntDepMotor.getEncoder().setPosition(0); + + leftIntPID = leftIntDepMotor.getPIDController(); + rightIntPID = rightIntDepMotor.getPIDController(); + + + intakeRollerMotor = new WPI_TalonFX(50); + // rightEncoder = new CANEncoder(rightIntDepMotor); + + leftIntDepMotor.restoreFactoryDefaults(); + leftIntDepMotor.setInverted(true); + leftIntPID.setP(Constants.IntakeConstants.leftIntkP); + leftIntPID.setI(Constants.IntakeConstants.leftIntkI); + leftIntPID.setD(Constants.IntakeConstants.leftIntkD); + leftIntPID.setIZone(Constants.IntakeConstants.leftIntIZone); + leftIntPID.setFF(Constants.IntakeConstants.leftIntFF); + leftIntPID.setOutputRange(Constants.IntakeConstants.leftIntMin, Constants.IntakeConstants.leftIntMax); - public void runIntake(double power){ - intakeMotor.set(power); - // System.out.println("intaking"); + rightIntDepMotor.restoreFactoryDefaults(); + rightIntDepMotor.setInverted(false); + // rightEncoder.setPosition(0); + // rightIntDepMotor.follow(leftIntDepMotor); + rightIntPID.setP(Constants.IntakeConstants.rightIntkP); + rightIntPID.setI(Constants.IntakeConstants.rightIntkI); + rightIntPID.setD(Constants.IntakeConstants.rightIntkD); + rightIntPID.setIZone(Constants.IntakeConstants.rightIntIZone); + rightIntPID.setFF(Constants.IntakeConstants.rightIntFF); + rightIntPID.setOutputRange(Constants.IntakeConstants.rightIntMin, Constants.IntakeConstants.rightIntMax); + + intakeRollerMotor.configFactoryDefault(); + intakeRollerMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + intakeRollerMotor.setSensorPhase(true); + intakeRollerMotor.setInverted(true); + intakeRollerMotor.setNeutralMode(NeutralMode.Coast); + intakeRollerMotor.configAllowableClosedloopError(Constants.IntakeConstants.rollerPIDSlot, 10); + intakeRollerMotor.configNominalOutputForward(0, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.configNominalOutputReverse(0, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.configPeakOutputForward(1, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.configPeakOutputReverse(-1, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.config_kF(Constants.IntakeConstants.rollerPIDSlot, Constants.IntakeConstants.rollerkF, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.config_kP(Constants.IntakeConstants.rollerPIDSlot, Constants.IntakeConstants.rollerkP, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.config_kI(Constants.IntakeConstants.rollerPIDSlot, Constants.IntakeConstants.rollerkI, Constants.IntakeConstants.intakeTimeout); + intakeRollerMotor.config_kD(Constants.IntakeConstants.rollerPIDSlot, Constants.IntakeConstants.rollerkD, Constants.IntakeConstants.intakeTimeout); + + // intakeMotor = new CANSparkMax(Constants.IntakeConstants.IntakeMotorPort, MotorType.kBrushless); + // intakeMotor.restoreFactoryDefaults(); + // intakeMotor.setInverted(Constants.IntakeConstants.IntakeInverted); } + public void runIntake(double percent){ + intakeRollerMotor.set(TalonFXControlMode.PercentOutput, percent); + + + } + public void moveIntake(double intakePosition){ + leftIntPID.setReference(intakePosition, ControlType.kPosition); + // rightIntPID.setReference(intakePosition, ControlType.kPosition); + } - public double intakeCurrent(){ - return intakeMotor.getOutputCurrent(); + public void MoveRightIntake(double pos){ + rightIntPID.setReference(pos, ControlType.kPosition); } + + + + + // public double intakeCurrent(){ + // return intakeMotor.getOutputCurrent(); + // } @Override diff --git a/src/main/java/org/team696/robot/subsystems/ShooterHood.java b/src/main/java/org/team696/robot/subsystems/ShooterHood.java index 8b5054b..c16014d 100644 --- a/src/main/java/org/team696/robot/subsystems/ShooterHood.java +++ b/src/main/java/org/team696/robot/subsystems/ShooterHood.java @@ -48,6 +48,7 @@ public void moveServoPosition(double position) { */ public double servoAngle(){ return leftShooterHoodServo.getAngle(); + // return leftShooterHoodServo.getPosition(); } diff --git a/src/main/java/org/team696/robot/subsystems/Spindexer.java b/src/main/java/org/team696/robot/subsystems/Spindexer.java index a3669fb..38c93db 100644 --- a/src/main/java/org/team696/robot/subsystems/Spindexer.java +++ b/src/main/java/org/team696/robot/subsystems/Spindexer.java @@ -33,14 +33,16 @@ public class Spindexer extends SubsystemBase { private static final Logger logger = LogManager.getLogger(Spindexer.class); private boolean isMoving; - private int targetPocket = 1; - private boolean isOnTarget; + // private int targetPocket = 1; + // private boolean isOnTarget; private static Boolean isJammed = false; - private boolean[] ballOccupancy = new boolean[SpindexerConstants.nPockets]; + // private boolean[] ballOccupancy = new boolean[SpindexerConstants.nPockets]; private int forwardTimer; private int backTimer; + public boolean kickupIsOn; + public enum SpindexerLoadingStates { FORWARD_TIMER, FORWARD_OK, BACK_TIMER, BACK_OK } @@ -56,14 +58,14 @@ public enum SpindexerLoadingStates { private final DutyCycle encoder = new DutyCycle(encoderDI); // Ball color sensors - TCA9548A mux = new TCA9548A(Port.kOnboard, SpindexerConstants.ColorSensorsMuxAddress); - private final BallSensors sensors = new BallSensors(mux, - new byte[] { SpindexerConstants.ColorSensor1MuxChannel, SpindexerConstants.ColorSensor2MuxChannel, - SpindexerConstants.ColorSensor3MuxChannel, SpindexerConstants.ColorSensor4MuxChannel, - SpindexerConstants.ColorSensor5MuxChannel }); + // TCA9548A mux = new TCA9548A(Port.kOnboard, SpindexerConstants.ColorSensorsMuxAddress); + // private final BallSensors sensors = new BallSensors(mux, + // new byte[] { SpindexerConstants.ColorSensor1MuxChannel, SpindexerConstants.ColorSensor2MuxChannel, + // SpindexerConstants.ColorSensor3MuxChannel, SpindexerConstants.ColorSensor4MuxChannel, + // SpindexerConstants.ColorSensor5MuxChannel }); // Kick motor - private final CANSparkMax kickMotor = new CANSparkMax(SpindexerConstants.KickMotorCANID, MotorType.kBrushless); + public final CANSparkMax kickMotor = new CANSparkMax(SpindexerConstants.KickMotorCANID, MotorType.kBrushless); public Spindexer() { spindexerMotor.restoreFactoryDefaults(); @@ -71,21 +73,21 @@ public Spindexer() { spindexerMotor.setClosedLoopRampRate(SpindexerConstants.closedLoopRampRate); spindexerMotor.setOpenLoopRampRate(SpindexerConstants.openLoopRampRate); motorPID.setP(SpindexerConstants.P); - sensors.initSensors(); + // sensors.initSensors(); kickMotor.setIdleMode(IdleMode.kBrake); } - public int getTargetPocket() { - return targetPocket; - } + // public int getTargetPocket() { + // return targetPocket; + // } public boolean isMoving() { return isMoving; } - public boolean isOnTarget() { - return isOnTarget; - } + // public boolean isOnTarget() { + // return isOnTarget; + // } /** * Sets brake mode of spindexer motor. @@ -103,16 +105,16 @@ public void setBrake(boolean isBrake){ * Safely sets target pocket. * * @param pocket Intended target pocket (bounds-checked) - */ - public void setTargetPocket(int pocket) { - if (1 <= pocket && pocket <= SpindexerConstants.nPockets) { - // logger.info(String.format("Setting spindexer target to pocket %d.", pocket)); - targetPocket = pocket; - } - else{ - // logger.error(String.format("Received request to set spindexer target to pocket %d. Ignoring.", pocket)); - } - } + // */ + // public void setTargetPocket(int pocket) { + // if (1 <= pocket && pocket <= SpindexerConstants.nPockets) { + // // logger.info(String.format("Setting spindexer target to pocket %d.", pocket)); + // targetPocket = pocket; + // } + // else{ + // // logger.error(String.format("Received request to set spindexer target to pocket %d. Ignoring.", pocket)); + // } + // } public double getMotorPosition() { return motorEncoder.getPosition(); @@ -129,9 +131,9 @@ public double getVelocity() { /**Gets ball occupancy array. * @return Which pockets have balls */ - public boolean[] getBallOccupancy() { - return ballOccupancy; - } + // public boolean[] getBallOccupancy() { + // return ballOccupancy; + // } @Override public void periodic() { @@ -147,48 +149,48 @@ public void periodic() { isMoving = (motorEncoder.getVelocity() > SpindexerConstants.VelocityTolerance); // Check if on target - if ((Math.abs(SpindexerConstants.PocketPositions[targetPocket - 1] - - getEncoderPosition()) < SpindexerConstants.PositionTolerance) && !isMoving) { - isOnTarget = true; - // updateBallOccupancy(); - } else { - isOnTarget = false; - - if(isJammed){ - // logger.warn(String.format("Jam while advancing to pocket %d. Reverting to previous pocket.")); - if (getVelocity() > 0) { - if (targetPocket == 1) { - setTargetPocket(5); - } else { - setTargetPocket(targetPocket - 1); - } - } else { - if (targetPocket == 5) { - setTargetPocket(1); - } else { - setTargetPocket(targetPocket + 1); - } - } - } - - motorPID.setReference(getMotorPositionForPocket(targetPocket), ControlType.kPosition); + // if ((Math.abs(SpindexerConstants.PocketPositions[targetPocket - 1] + // - getEncoderPosition()) < SpindexerConstants.PositionTolerance) && !isMoving) { + // isOnTarget = true; + // // updateBallOccupancy(); + // } else { + // isOnTarget = false; + + // if(isJammed){ + // // logger.warn(String.format("Jam while advancing to pocket %d. Reverting to previous pocket.")); + // if (getVelocity() > 0) { + // if (targetPocket == 1) { + // setTargetPocket(5); + // } else { + // setTargetPocket(targetPocket - 1); + // } + // } else { + // if (targetPocket == 5) { + // setTargetPocket(1); + // } else { + // setTargetPocket(targetPocket + 1); + // } + // // } + // } + + // motorPID.setReference(getMotorPositionForPocket(targetPocket), ControlType.kPosition); - } + // } } /** * Gets the currently-indexed pocket. * * @return Current pocket (1-5), or 0 if not on a pocket - */ - public int getCurrentPocket() { - if (isOnTarget) { - return targetPocket; - } else { - // TODO: Need a better way to represent "not at a pocket" - return 0; - } - } + // */ + // public int getCurrentPocket() { + // if (isOnTarget) { + // return targetPocket; + // } else { + // // TODO: Need a better way to represent "not at a pocket" + // return 0; + // } + // } /** * Gets the position of the spindexer, based on the absolute encoder. @@ -212,18 +214,18 @@ public double getEncoderPosition() { * @param pocket The index of the pocket to go to (1, ..., 5) * @return The required motor position, taking the current motor position into * account - */ - @SuppressWarnings("checkstyle:magicnumber") - private double getMotorPositionForPocket(int pocket) { - double currentPos = getEncoderPosition(); - double targetPos = SpindexerConstants.PocketPositions[pocket - 1]; - double toGo = computeWrappedDistance(currentPos, targetPos); - if ((0.5 - Math.abs(toGo)) < 0.1) { - toGo = Math.abs(toGo); - } - // System.out.printf("toGo: %f\n", toGo); - return getMotorPosition() + (toGo * SpindexerConstants.MotorTurnsPerSpindexerTurn); - } + // */ + // @SuppressWarnings("checkstyle:magicnumber") + // private double getMotorPositionForPocket(int pocket) { + // double currentPos = getEncoderPosition(); + // double targetPos = SpindexerConstants.PocketPositions[pocket - 1]; + // double toGo = computeWrappedDistance(currentPos, targetPos); + // if ((0.5 - Math.abs(toGo)) < 0.1) { + // toGo = Math.abs(toGo); + // } + // // System.out.printf("toGo: %f\n", toGo); + // return getMotorPosition() + (toGo * SpindexerConstants.MotorTurnsPerSpindexerTurn); + // } /** * Helper function to compute shortest wrapped angular distance. @@ -232,31 +234,31 @@ private double getMotorPositionForPocket(int pocket) { * @param to To point, on (0, 1) * @return Shortest distance, on (-0.5, 0.5) */ - @SuppressWarnings("checkstyle:magicnumber") - private double computeWrappedDistance(double from, double to) { - // Adapted from https://stackoverflow.com/a/28037434 - double diff = (to - from + 0.5) % 1 - 0.5; - return diff < -0.5 ? diff + 1 : diff; - } + // @SuppressWarnings("checkstyle:magicnumber") + // private double computeWrappedDistance(double from, double to) { + // // Adapted from https://stackoverflow.com/a/28037434 + // double diff = (to - from + 0.5) % 1 - 0.5; + // return diff < -0.5 ? diff + 1 : diff; + // } /** * Finds the pocket closest to the current position. * @return Index of closest pocket, in the range (1, SpindexerConstants.nPockets) */ - public int findNearestPocket(){ - double currentPos = getEncoderPosition(); - int i; - int nearest=0; - double nearestDistance = 1; - for(i=0; i