This runs after the mode specific periodic functions, but before LiveWindow and + *
+ * 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 + // 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() {} + public void disabledInit() { + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -70,7 +88,8 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void teleopInit() { @@ -85,7 +104,8 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 377c764..9c80d8c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,9 @@ import frc.robot.commands.indexer.ShootIndexer; import frc.robot.commands.indexer.LoadIndexer; import frc.robot.commands.auto.DriveToDistance; +import frc.robot.commands.climber.Calibrate; +import frc.robot.commands.climber.ElevatorDown; +import frc.robot.commands.climber.ElevatorUp; import frc.robot.commands.drive.SimDrive; import frc.robot.commands.intake.IntakeRun; import frc.robot.commands.intake.ToggleIntake; @@ -23,21 +26,19 @@ import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.subsystems.ClimberSubsystem; public class RobotContainer { /* GAMEPADS */ private static final PS4Controller driverGamepad = new PS4Controller( - Constants.RobotMap.kDriverControllerPort - ); + Constants.RobotMap.kDriverControllerPort); private static final PS4Controller operatorGamepad = new PS4Controller( - Constants.RobotMap.kOperatorControllerPort - ); + Constants.RobotMap.kOperatorControllerPort); private static final JoystickButton triangleButton = new JoystickButton( - operatorGamepad, - PS4Controller.Button.kTriangle.value - ); + operatorGamepad, + PS4Controller.Button.kTriangle.value); private static final JoystickButton m_crossButton = new JoystickButton(driverGamepad, PS4Controller.Button.kCross.value); private static final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value); @@ -48,28 +49,33 @@ public class RobotContainer { private final TurretSubsystem m_turretSubsystem = new TurretSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); private final IndexerSubsystem m_indexerSubsystem = new IndexerSubsystem(); + private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final JoystickButton m_circleButton = new JoystickButton( - driverGamepad, - PS4Controller.Button.kCircle.value - ); + driverGamepad, + PS4Controller.Button.kCircle.value); + + private final JoystickButton m_optionButton = new JoystickButton(driverGamepad, PS4Controller.Button.kOptions.value); + private final JoystickButton m_shareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kShare.value); + // private final JoystickButton m_squareButton = new JoystickButton(driverGamepad, PS4Controller.Button.kSquare.value); + private final JoystickButton m_l1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kL1.value); + private final JoystickButton m_r1Button = new JoystickButton(driverGamepad, PS4Controller.Button.kR1.value); public RobotContainer() { configureButtonBindings(); CommandScheduler - .getInstance() - .schedule(new ToggleLED(m_ledSubsystem, true)); + .getInstance() + .schedule(new ToggleLED(m_ledSubsystem, true)); + CommandScheduler.getInstance().schedule(false, new Calibrate(m_climberSubsystem)); m_shooterSubsystem.setDefaultCommand(new ManualShoot(m_shooterSubsystem, 40, 40)); m_driveSubsystem.setDefaultCommand( - new SimDrive( - m_driveSubsystem, - 2, - driverGamepad::getR2Axis, - driverGamepad::getL2Axis, - driverGamepad::getLeftX, - driverGamepad::getRightX - ) - ); + new SimDrive( + m_driveSubsystem, + 2, + driverGamepad::getR2Axis, + driverGamepad::getL2Axis, + driverGamepad::getLeftX, + driverGamepad::getRightX)); m_intakeSubsystem.setDefaultCommand(new IntakeRun(m_intakeSubsystem)); m_indexerSubsystem.setDefaultCommand(new LoadIndexer(m_indexerSubsystem)); } @@ -77,11 +83,16 @@ public RobotContainer() { private void configureButtonBindings() { triangleButton.whileHeld(new AutoAimTurret(m_turretSubsystem, 0.05)); m_circleButton.whenPressed(new ToggleIntake(m_intakeSubsystem)); + // X_BUTTON.whileHeld(new RunIndexer(m_indexerSubsystem)); + m_optionButton.whileHeld(new ElevatorUp(m_climberSubsystem)); // Elevator Up + m_shareButton.whileHeld(new ElevatorDown(m_climberSubsystem)); // Elevator Down + m_l1Button.toggleWhenPressed(new ElevatorDown(m_climberSubsystem)); + m_r1Button.toggleWhenPressed(new ElevatorUp(m_climberSubsystem)); m_crossButton.whileHeld(new ShootIndexer(m_indexerSubsystem)); } public Command getAutonomousCommand() { - return new DriveToDistance(m_driveSubsystem, 2); + return new ElevatorUp(m_climberSubsystem); } diff --git a/src/main/java/frc/robot/commands/climber/Calibrate.java b/src/main/java/frc/robot/commands/climber/Calibrate.java new file mode 100644 index 0000000..6eb6d3b --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/Calibrate.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 frc.robot.commands.climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ClimberSubsystem; + +// for setting soft limits +public class Calibrate extends CommandBase { + /** Creates a new Calibrate. */ + private ClimberSubsystem m_climberSubsystem; + + public Calibrate(ClimberSubsystem climbSubsystem) { + m_climberSubsystem = climbSubsystem; + addRequirements(m_climberSubsystem); + } + + // 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() { + m_climberSubsystem.setSpeed(-0.2); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_climberSubsystem.stop(); + m_climberSubsystem.reset(); + m_climberSubsystem.setSoftLimits(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_climberSubsystem.fullyRetracted(); + } +} diff --git a/src/main/java/frc/robot/commands/climber/ElevatorDown.java b/src/main/java/frc/robot/commands/climber/ElevatorDown.java new file mode 100644 index 0000000..22acec1 --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/ElevatorDown.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 frc.robot.commands.climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ClimberSubsystem; + +// for making the elevator descend +public class ElevatorDown extends CommandBase { + /** Creates a new ElevatorDown. */ + private ClimberSubsystem m_climberSubsystem; + + public ElevatorDown(ClimberSubsystem climberSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_climberSubsystem = climberSubsystem; + addRequirements(m_climberSubsystem); + } + + // 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() { + m_climberSubsystem.descend(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_climberSubsystem.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/climber/ElevatorUp.java b/src/main/java/frc/robot/commands/climber/ElevatorUp.java new file mode 100644 index 0000000..18d725d --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/ElevatorUp.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 frc.robot.commands.climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ClimberSubsystem; + +// for making the elevator raise up +public class ElevatorUp extends CommandBase { + /** Creates a new Elevator. */ + private ClimberSubsystem m_climberSubsystem; + + public ElevatorUp(ClimberSubsystem climbsubsystem) { + m_climberSubsystem = climbsubsystem; + addRequirements(m_climberSubsystem); + } + + // 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() { + m_climberSubsystem.climb(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_climberSubsystem.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..d486139 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,146 @@ +// 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; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Climber; + +public class ClimberSubsystem extends SubsystemBase { + /** Creates a new ClimberSubsystem. */ + private CANSparkMax m_climberMotor; + private DigitalInput m_limitSwitch; + public double m_climbSpeed; + private final RelativeEncoder m_encoder; + + ElevatorSim m_elevatorSimulation; + EncoderSim m_encoderSimulation; + + private final Mechanism2d m_mech2d; + private final MechanismRoot2d m_mech2dRoot; + private final MechanismLigament2d m_elevatorMech2d; + + boolean isBottom; + boolean atTop; + boolean toggleLimit; + + public ClimberSubsystem() { + m_climberMotor = + new CANSparkMax(Climber.kClimberMotor_Port, MotorType.kBrushless); + m_limitSwitch = new DigitalInput(Climber.kLimitSwitch_Port); + + m_encoder = m_climberMotor.getEncoder(); + m_encoder.setPosition(0); + + m_climbSpeed = Climber.kElevatorSpeed; + + m_elevatorSimulation = new ElevatorSim(DCMotor.getNEO(1), 100, 45, + Units.inchesToMeters(2), 0, 1); + + m_encoderSimulation = new EncoderSim(new Encoder(1, 2)); + + m_mech2d = new Mechanism2d(20, 50); + m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 2); + m_elevatorMech2d = m_mech2dRoot.append(new MechanismLigament2d("Elevator", + Units.metersToInches(m_elevatorSimulation.getPositionMeters()), 90)); + + SmartDashboard.putData("Elevator Sim", m_mech2d); + + isBottom = true; + atTop = false; + } + + @Override + public void periodic() { + // setSpeed(0.2); + SmartDashboard.putNumber("Elevator Sim Position", + m_elevatorSimulation.getPositionMeters()); + SmartDashboard.putBoolean("Limit Switch", fullyRetracted()); + } + + public void getPosition() { + if (m_elevatorSimulation.getPositionMeters() == 0.01) { + isBottom = true; + atTop = false; + } else if (m_elevatorSimulation.getPositionMeters() == 1) { + isBottom = false; + atTop = true; + } + } + + public boolean isAtBottom() { + return isBottom; + } + + public boolean isAtTop() { + return atTop; + } + + public boolean fullyRetracted() { + return m_limitSwitch.get(); + } + + public void setSoftLimits() { + m_climberMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_climberMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_climberMotor.setSoftLimit(SoftLimitDirection.kForward, + Climber.kUpperLimit); // 15 + m_climberMotor.setSoftLimit(SoftLimitDirection.kReverse, + Climber.kLowerLimit); // 0 + } + + public void setSpeed(double speed) { + m_climberMotor.set(speed); + } + + + public void reset() { + m_encoder.setPosition(0); + } + + public void climb() { + setSpeed(Climber.kElevatorSpeed); + } + + public void descend() { + setSpeed(-Climber.kElevatorSpeed); + } + + public void stop() { + setSpeed(0); + } + + public void log() { + SmartDashboard.putNumber("Current Position", + Double.valueOf(String.valueOf(m_climberMotor.getEncoder()))); + } + + @Override + public void simulationPeriodic() { + m_elevatorSimulation + .setInput(m_climberMotor.get() * RobotController.getBatteryVoltage()); + m_elevatorSimulation.update(0.020); + m_encoderSimulation.setDistance(m_elevatorSimulation.getPositionMeters()); + m_elevatorMech2d.setLength( + Units.metersToInches(m_elevatorSimulation.getPositionMeters())); + + } +}