diff --git a/2025ProjBotCode b/2025ProjBotCode new file mode 160000 index 0000000..257e8c6 --- /dev/null +++ b/2025ProjBotCode @@ -0,0 +1 @@ +Subproject commit 257e8c6f2bc70eb03fe167fd6cbb7ce3c74da9f1 diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..5628a9e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -88,5 +93,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 0bb79b1..f037649 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -12,20 +12,20 @@ "robotMass": 74.088, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.047625, + "driveGearing": 6.75, "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", + "driveMotorType": "NEO", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.311151, + "flModuleY": 0.3115, + "frModuleX": 0.311, + "frModuleY": -0.311, + "blModuleX": -0.311, + "blModuleY": 0.311, + "brModuleX": -0.311, + "brModuleY": -0.311, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 64e678c..fe85238 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -18,7 +18,7 @@ } }, "rampRate": { - "drive": 1, + "drive": 0.6, "angle": 0.15 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ce151e6..df35a2b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,6 +46,34 @@ public static class DrivetrainConfig { public static final PIDConstants DRIVE_PID = new PIDConstants(0.5, 0, 0); public static final PIDConstants TURN_PID = new PIDConstants(0.5, 0, 0); } + public static class ElevatorConstants{ + public static final double L1Height = 20; + public static final double L2Height = 62; + public static final double L3Height = 108; + public static final double L4Height = 193; + public static final double A1Height = 45; + public static final double A2Height = 72; + } + public static class tiltConstants{ + public static double startingHeight = 15.25; + public static double endingHeight = 59.25; + public static double startingEncoder = 0; + public static double endingEncoder = 110; + + public static double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); + + public static final double tiltOffset = -5.928567; //Please put this in degrees and change it according to what you measure. + + public static final double tiltMinSpeed = 1; + public static final double tiltMaxSpeed = 2.5; + + public static final double tiltL1 = 3.380949; + public static final double tiltL2 = 4.85710; + public static final double tiltL3 = 6.642862; + public static final double tiltL4 = 3.071426; + public static final double tiltA1 = 22.118935; + public static final double tiltA2 = 22.118935; + } public static class SystemConfig { public static final PIDController PIVOT_PID = new PIDController(0.1, 0, 0); @@ -55,7 +83,7 @@ public static class SystemConfig { public static final double SHOOTER_TOLERANCE = 2; // deg public static final double ELEVATOR_TOLERANCE = 0.1; // m - public static final double ELEVATOR_SPEED = 0.5; + public static final double ELEVATOR_SPEED = 0.7; public static final double PIVOT_SPEED = 0.5; public static final double SHOOTER_SPEED = 0.5; public static final double GRABBER_SPEED = 0.5; @@ -74,7 +102,7 @@ public static class SystemConfig { public static final double SHOOTER_INTAKE_TIME = 0.5; public static final double CORAL_INTAKE_TIME = 1.0; public static final double CORAL_OUTTAKE_TIME = 1.0; - public static final double GRABBER_TIME = 3.0; + public static final double GRABBER_TIME = 1.0; public static final double CLIMBER_DEFAULT_POSITION = 0; public static final double CLIMBER_CLIMB_POSITION = 0.5; @@ -98,6 +126,7 @@ public static final class DrivebaseConstants public static class OperatorConstants { + // Joystick Deadband public static final double DEADBAND = 0.1; public static final double LEFT_Y_DEADBAND = 0.1; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f8da5da..c78ec6b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -53,7 +53,9 @@ public void robotInit() disabledTimer = new Timer(); //start streaming the camera feed - CameraServer.startAutomaticCapture(); + CameraServer.startAutomaticCapture(0); + CameraServer.startAutomaticCapture(1); + if (isSimulation()) { @@ -106,14 +108,14 @@ public void disabledPeriodic() @Override public void autonomousInit() { - // m_robotContainer.setMotorBrake(true); - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // // schedule the autonomous command (example) - // if (m_autonomousCommand != null) - // { - // m_autonomousCommand.schedule(); - // } + m_robotContainer.setMotorBrake(true); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) + { + m_autonomousCommand.schedule(); + } return; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e689f77..659bab9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,9 +37,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; // import frc.robot.subsystems.drive.DriveSubsystem; @@ -58,6 +60,7 @@ public class RobotContainer public static ClimberSubsystem climber = new ClimberSubsystem(); public static ElevatorSubsystem elevator = new ElevatorSubsystem(); public static EndEffectorSubsystem endEffector = new EndEffectorSubsystem(); + public static Intake intake = new Intake(); //Define Controllers public static CommandXboxController driverController = new CommandXboxController(0); @@ -72,10 +75,12 @@ public class RobotContainer SwerveInputStream driveAngularVelocity = SwerveInputStream.of(driveTrain.getSwerveDrive(), () -> driverController.getLeftY() * -1, () -> driverController.getLeftX() * -1) - .withControllerRotationAxis(driverController::getRightX) + .withControllerRotationAxis(() -> driverController.getRightX()*-1) .deadband(OperatorConstants.DEADBAND) .scaleTranslation(0.8) - .allianceRelativeControl(true); + .allianceRelativeControl(true) + .cubeTranslationControllerAxis(true) + .cubeRotationControllerAxis(true); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -86,21 +91,74 @@ public RobotContainer() configureBindings(); DriverStation.silenceJoystickConnectionWarning(true); autoChooser = AutoBuilder.buildAutoChooser("Pass The Line Auto"); - + SmartDashboard.putData("Auto Mode", autoChooser); Command driveFieldOrientedAnglularVelocity = driveTrain.driveFieldOriented(driveAngularVelocity); driveTrain.setDefaultCommand(driveFieldOrientedAnglularVelocity); + climber.setDefaultCommand(Commands.run(() -> { + // Driver Controls + + + if(driverController.rightTrigger().getAsBoolean()){ + if(climber.lowerLimitReachedWinch()){ + climber.setSpeed(0); + climber.resetEncoder(); + } + else{ + climber.setSpeed(3); + } + } + else if(driverController.leftTrigger().getAsBoolean()){ + climber.setSpeed(-3); + } + else{ + climber.setSpeed(0); + } + - climber.setDefaultCommand(new RunCommand(() -> { }, climber)); - elevator.setDefaultCommand(new RunCommand(() -> { - elevator.setSpeed(-operatorController.getLeftY()*.1); //Multiply by .1 for testing + + intake.setDefaultCommand(new InstantCommand(() -> {}, intake)); + + elevator.setDefaultCommand(new InstantCommand(() -> { + double joystick = -operatorController.getLeftY(); + + if (Math.abs(joystick) < 0.1) + { + elevator.goToHeight(elevator.getLastSetpoint()); + } + else + { + //if the lower limit's been reached don't allow them to go down but allow them to go up. + if(elevator.lowerLimitReached() == true && joystick <= 0) { + //Multiply by .1 for testing + elevator.resetEncoder(); + elevator.setSpeed(0); + }else + { + // elevator.setSpeed(joystick); + joystick = handleDeadband(joystick, 0.1); + elevator.goToHeight(elevator.getLastSetpoint() + joystick * Constants.ElevatorConstants.L2Height / 50.0 / 1.0); + } + } }, elevator)); - endEffector.setDefaultCommand(new RunCommand(() -> { - endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1); //We are using this to test, the .1 is to make it go slow + endEffector.setDefaultCommand(new InstantCommand(() -> { + if(elevator.lowerLimitReached() == true /*&& -operatorController.getLeftY() <= 0*/) { + //if((endEffector.endEffectorTilt.getEncoder().getPosition() <= -.8)||(endEffector.endEffectorTilt.getEncoder().getPosition() >= 0 )){ + //if(endEffector.atSetPoint() == false){ + endEffector.goToTilt(-.4); + //}else{ + // endEffector.setSpeedEndEffectorTilt(0); + //} + //} + }else if((elevator.elevator1.getEncoder().getPosition() <= 30)) { + endEffector.goToTilt(6); + }else{ + endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1); //We are using this to test, the .1 is to make it go slow //this is the value to make sure the end effector clears the funnel + } }, endEffector)); } @@ -117,25 +175,71 @@ private void configureBindings() //To do: Set Buttons, Set Speeds, Verify Directions // Operator Controls + Command heightL1PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L1Height,elevator); + Command heightL2PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L2Height,elevator); + Command heightL3PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L3Height,elevator); + Command heightL4PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.L4Height,elevator); + Command heightA1PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.A1Height,elevator); + Command heightA2PresetCommand = new ElevatorPreset(Constants.ElevatorConstants.A2Height,elevator); + + Command tiltL1PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL1, endEffector); + Command tiltL2PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL2, endEffector); + Command tiltL3PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL3, endEffector); + Command tiltL4PresetCommand = new TiltPreset(Constants.tiltConstants.tiltL4, endEffector); + Command tiltA1PresetCommand = new TiltPreset(Constants.tiltConstants.tiltA1, endEffector); + Command tiltA2PresetCommand = new TiltPreset(Constants.tiltConstants.tiltA2, endEffector); - operatorController.rightTrigger().whileTrue(new ShootCommand(-.1)); - operatorController.leftTrigger().whileTrue(new ShootCommand(.1)); + SequentialCommandGroup goToPresetL1 = new SequentialCommandGroup(heightL1PresetCommand, tiltL1PresetCommand); + SequentialCommandGroup goToPresetL2 = new SequentialCommandGroup(heightL2PresetCommand, tiltL2PresetCommand); + SequentialCommandGroup goToPresetL3 = new SequentialCommandGroup(heightL3PresetCommand, tiltL3PresetCommand); + SequentialCommandGroup goToPresetL4 = new SequentialCommandGroup(heightL4PresetCommand, tiltL4PresetCommand); + SequentialCommandGroup goToPresetA1 = new SequentialCommandGroup(tiltA1PresetCommand, heightA1PresetCommand); + SequentialCommandGroup goToPresetA2 = new SequentialCommandGroup(tiltA2PresetCommand, heightA2PresetCommand); - operatorController.rightBumper().whileTrue(new IntakeCommand(-.1)); - operatorController.leftBumper().whileTrue(new IntakeCommand(.1)); + // operatorController.rightTrigger().whileTrue(new ShootCommand(-2.5)); + // operatorController.leftTrigger().whileTrue(new ShootCommand(2.5)); + // operatorController.rightBumper().whileTrue(new IntakeCommand(-.2)); + // operatorController.leftBumper().whileTrue(new IntakeCommand(.2)); - - // Driver Controls - driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); - driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); + operatorController.rightTrigger() + .onTrue(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(-2.5), endEffector)) + .onFalse(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(0.0), endEffector)); - driverController.rightTrigger().whileTrue(new ExtendWinchCommand(-.1)); - driverController.leftTrigger().whileTrue(new ExtendWinchCommand(.1)); + operatorController.leftTrigger() + .onTrue(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(2.5), endEffector)) + .onFalse(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(0.0), endEffector)); - driverController.rightBumper().whileTrue(new GrabCageCommand(.1)); - driverController.leftBumper().whileTrue(new GrabCageCommand(-1.)); + + operatorController.rightBumper() + .onTrue(new InstantCommand(() -> intake.intake(-0.2), intake)) + .onFalse(new InstantCommand(() -> intake.intake(0.0), intake)); + + operatorController.leftBumper() + .onTrue(new InstantCommand(() -> intake.intake(0.2), intake)) + .onFalse(new InstantCommand(() -> intake.intake(0.0), intake)); + + operatorController.x().whileTrue(goToPresetL1); + operatorController.y().whileTrue(goToPresetL2); + operatorController.b().whileTrue(goToPresetL3); + operatorController.a().whileTrue(goToPresetL4); + + + operatorController.povDown().whileTrue(goToPresetA1); + operatorController.povUp().whileTrue(goToPresetA2); + + + driverController.rightBumper() + .onTrue(new InstantCommand(() -> climber.Grab(1.00), climber)) + .onFalse(new InstantCommand(() -> climber.Grab(0.0), climber)); + + driverController.leftBumper() + .onTrue(new InstantCommand(() -> climber.Grab(-1.00), climber)) + .onFalse(new InstantCommand(() -> climber.Grab(0.0), climber)); + + driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); + driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); } public void setMotorBrake(boolean brake) { @@ -150,6 +254,19 @@ public Command getAutonomousCommand() { // Pass the auto line for points return autoChooser.getSelected(); + // return driveTrain.getAutonomousCommand("Pass The Line Auto"); + } + + + public static double handleDeadband(double value, double deadband) + { + deadband = Math.abs(deadband); + if (deadband == 1) + { + return 0; + } + double scaledValue = (value + (value < 0 ? deadband : -deadband)) / (1 - deadband); + return (Math.abs(value) > Math.abs(deadband)) ? scaledValue : 0; } } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java index f47beb7..6d246fc 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java @@ -14,7 +14,9 @@ public ExtendWinchCommand(Double _speed) } @Override - public void initialize() {} + public void initialize() { + RobotContainer.climber.Winch(0); + } @Override public void execute() { @@ -22,7 +24,9 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.climber.Winch(0); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java index b10aa1d..9397662 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java @@ -14,7 +14,9 @@ public GrabCageCommand(Double _speed) } @Override - public void initialize() {} + public void initialize() { + RobotContainer.climber.Grab(0); + } @Override public void execute() { @@ -22,7 +24,9 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.climber.Grab(0); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorPreset.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorPreset.java new file mode 100644 index 0000000..90785cc --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorPreset.java @@ -0,0 +1,47 @@ +// 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.ElevatorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorSubsystem; + + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ElevatorPreset extends Command { + private final double height; + private final ElevatorSubsystem elevator; + /** Creates a new ElevatorPreset. */ + public ElevatorPreset(double height, ElevatorSubsystem elevator) { + this.height = height; + this.elevator = elevator; + + addRequirements(elevator); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.setVoltage(0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevator.goToHeight(height); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevator.setVoltage(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java index 1560887..f77f867 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -3,30 +3,31 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; + public class IntakeCommand extends Command { private final double speed; public IntakeCommand(Double _speed) { - addRequirements(RobotContainer.endEffector); + addRequirements(RobotContainer.intake); speed = _speed; } @Override public void initialize() { - RobotContainer.endEffector.setSpeedEndEffectorIntake(0); + RobotContainer.intake.intake(0); } @Override public void execute() { - RobotContainer.endEffector.setSpeedEndEffectorIntake(speed); + RobotContainer.intake.intake(speed); } @Override public void end(boolean interrupted) { - RobotContainer.endEffector.setSpeedEndEffectorIntake(0); + RobotContainer.intake.intake(0); } @Override diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java index 79d2c13..c5d738e 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java @@ -14,7 +14,9 @@ public ShootCommand(Double _speed) } @Override - public void initialize() {} + public void initialize() { + RobotContainer.endEffector.setSpeedEndEffectorMotor(0); + } @Override public void execute() @@ -23,7 +25,9 @@ public void execute() } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.endEffector.setSpeedEndEffectorMotor(0); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/TiltPreset.java b/src/main/java/frc/robot/commands/EndEffectorCommands/TiltPreset.java new file mode 100644 index 0000000..955b906 --- /dev/null +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/TiltPreset.java @@ -0,0 +1,47 @@ +// 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.EndEffectorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.endeffector.EndEffectorSubsystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class TiltPreset extends Command { + private EndEffectorSubsystem endEffector; + private double tilt; + + /** Creates a new TiltPreset. */ + public TiltPreset(double tilt, EndEffectorSubsystem endEffector) { + this.tilt = tilt; + this.endEffector = endEffector; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(endEffector); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + endEffector.setSpeedEndEffectorTilt(0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + endEffector.goToTilt(tilt); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + endEffector.setSpeedEndEffectorTilt(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return endEffector.atSetPoint(); + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java index a084acb..b6c6be9 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java @@ -55,6 +55,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -65,12 +66,16 @@ public class ClimberSubsystem extends SubsystemBase { + DigitalInput winchLowerSwitch; + private final SparkMax climberWinch; private final SparkMax climberGrab; private Servo climberServo = new Servo(0); public ClimberSubsystem() { + winchLowerSwitch = new DigitalInput(1); + climberWinch = new SparkMax(CANConfig.CLIMB_WINCH, MotorType.kBrushless); climberGrab = new SparkMax(CANConfig.CLIMB_GRAB, MotorType.kBrushless); climberServo.set(0); @@ -95,11 +100,29 @@ public void UnlatchServo(){ climberServo.set(.25); } + public Boolean lowerLimitReachedWinch() { + if(winchLowerSwitch.get()) + return false; + else{ + return true; + } + } + + @Override public void periodic() { SmartDashboard.putNumber("Climber Servo Position", climberServo.get()); SmartDashboard.putNumber("Climber Winch Position", climberWinch.getEncoder().getPosition()); + SmartDashboard.putBoolean("Winch Limit Switch", lowerLimitReachedWinch()); + } + + public void setSpeed(double speed) { + climberWinch.set(speed); + } + + public void resetEncoder() { + climberWinch.getEncoder().setPosition(0); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 6e07635..8abcfa5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -3,11 +3,16 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -15,9 +20,10 @@ public class ElevatorSubsystem extends SubsystemBase { - private final SparkMax elevator1 = new SparkMax(CANConfig.ELEVATOR_LEFT, MotorType.kBrushless); - private final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless); - private final SparkMax endEffectorTilt = new SparkMax(CANConfig.END_EFFECTOR_TILT, MotorType.kBrushless); + public DigitalInput elevatorLowerSwitch; + + public final SparkMax elevator1 = new SparkMax(CANConfig.ELEVATOR_LEFT, MotorType.kBrushless); + public final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless); private SparkMaxConfig elevator1Config = new SparkMaxConfig(); private SparkMaxConfig elevator2Config = new SparkMaxConfig(); @@ -29,34 +35,55 @@ public class ElevatorSubsystem extends SubsystemBase private double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); - private PIDController elevatorPID = new PIDController(.5,0,0); + private PIDController elevatorPID = new PIDController(1,0,0); + + private double lastSetpoint = 0; public ElevatorSubsystem() { + this.elevatorLowerSwitch = new DigitalInput(2); + elevator1Config.inverted(true); elevator2Config.inverted(false); elevator1Config.idleMode(IdleMode.kBrake); elevator2Config.idleMode(IdleMode.kBrake); + elevator1Config.smartCurrentLimit(40); + elevator2Config.smartCurrentLimit(40); elevator1.configure(elevator1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); elevator2.configure(elevator2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); elevatorPID.setTolerance(1); } public void goToHeight(double height){ - height = height - 15.25; //15.25 is starting height - double voltage = elevatorPID.calculate(this.getHeight(), height); - if(Math.abs(voltage) > 3){ - voltage = (voltage)/Math.abs(voltage)*3; - } + double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); + voltage = MathUtil.clamp(voltage, -4, 4); SmartDashboard.putNumber("elevator PID Voltage", voltage); elevator1.setVoltage(voltage); elevator2.setVoltage(voltage); + lastSetpoint = height; + } + + public double getLastSetpoint() { + return lastSetpoint; + } + + public void resetEncoder(){ + elevator1.getEncoder().setPosition(0); + elevator2.getEncoder().setPosition(0); } public double getHeight(){ - return elevator1.getEncoder().getPosition()*inchesPerEncoder; + return elevator1.getEncoder().getPosition(); + } + + public void setVoltage(double voltage) { + elevator1.setVoltage(voltage); + elevator2.setVoltage(voltage); } + public boolean atSetpoint(){ + return elevatorPID.atSetpoint(); + } public void setSpeedElevator1(double speed) { @@ -68,16 +95,19 @@ public void setSpeedElevator2(double speed) elevator2.set(speed); } - public void setSpeedEndEffectorTilt(double speed){ - endEffectorTilt.set(speed); - } - public void setSpeed(double speed){ elevator1.set(speed); elevator2.set(speed); } - //elevator positions + + public Boolean lowerLimitReached() { + if(elevatorLowerSwitch.get()) + return false; + else{ + return true; + } + } @Override @@ -85,5 +115,6 @@ public void periodic() { SmartDashboard.putNumber("Elevator Motor1 Position", elevator1.getEncoder().getPosition()); SmartDashboard.putNumber("Elevator Motor2 Position", elevator2.getEncoder().getPosition()); + SmartDashboard.putBoolean("Elevator Limit Switch", lowerLimitReached()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index a36126c..6ba347e 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -3,22 +3,25 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import frc.robot.Constants; import frc.robot.Constants.CANConfig; +import frc.robot.Constants.tiltConstants; public class EndEffectorSubsystem extends SubsystemBase { - private final SparkMax endEffectorMotor; - private final SparkMax endEffectorTilt; - private final SparkMax endEffectorIntake; + public final SparkMax endEffectorMotor; + public final SparkMax endEffectorTilt; + + private PIDController tiltPIDController = new PIDController(.06, 0.0, 0.0); public EndEffectorSubsystem() { endEffectorMotor = new SparkMax(CANConfig.END_EFFECTOR_MOTOR, MotorType.kBrushless); endEffectorTilt = new SparkMax(CANConfig.END_EFFECTOR_TILT, MotorType.kBrushless); - endEffectorIntake = new SparkMax(CANConfig.END_EFFECTOR_INTAKE, MotorType.kBrushless); + tiltPIDController.setTolerance(.5); } @@ -31,17 +34,40 @@ public void setSpeedEndEffectorTilt(double speed) { endEffectorTilt.set(speed); } - - public void setSpeedEndEffectorIntake(double speed){ - endEffectorIntake.set(speed); + + public void setPIDGains(double P, double I, double D){ + tiltPIDController.setPID(P, I, D); } + public void goToTilt(double encoders){ + double speed = tiltPIDController.calculate(endEffectorTilt.getEncoder().getPosition(), encoders); + if(speed > .85){ + speed = (speed/Math.abs(speed))*.85; + } + endEffectorTilt.set(speed); + } + /* + * public void goToHeight(double height){ + double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); + if(Math.abs(voltage) > 3){ + voltage = (voltage)/Math.abs(voltage)*3; + } + SmartDashboard.putNumber("elevator PID Voltage", voltage); + elevator1.setVoltage(voltage); + elevator2.setVoltage(voltage); + + } + */ + @Override public void periodic() { SmartDashboard.putNumber("End Effector Encoder", endEffectorMotor.getEncoder().getPosition()); SmartDashboard.putNumber("End Effector Tilt", endEffectorTilt.getEncoder().getPosition()); SmartDashboard.putNumber("End Effector Tilt Absolute Encoder", endEffectorTilt.getAlternateEncoder().getPosition()); - SmartDashboard.putNumber("End Effector Intake", endEffectorIntake.getEncoder().getPosition()); + } + + public boolean atSetPoint() { + return tiltPIDController.atSetpoint(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/endeffector/Intake.java b/src/main/java/frc/robot/subsystems/endeffector/Intake.java new file mode 100644 index 0000000..c4604a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/endeffector/Intake.java @@ -0,0 +1,28 @@ +// 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.endeffector; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.CANConfig; + +public class Intake extends SubsystemBase { + private final SparkMax endEffectorIntake = new SparkMax(CANConfig.END_EFFECTOR_INTAKE, MotorType.kBrushless); + + /** Creates a new Intake. */ + public Intake() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + public void intake(double speed) { + endEffectorIntake.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 8aa5173..a490d04 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -171,11 +171,11 @@ public void setupPathPlanner() final boolean enableFeedforward = true; // Configure AutoBuilder last AutoBuilder.configure( - this::getPose, + swerveDrive::getPose, // Robot pose supplier - this::resetOdometry, + swerveDrive::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotVelocity, + swerveDrive::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speedsRobotRelative, moduleFeedForwards) -> { if (enableFeedforward)