From e23cb975a9d3b309fe2689bca1410bcee49befe0 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sat, 15 Mar 2025 14:02:26 -0500 Subject: [PATCH 01/19] intake --- .../robot/subsystems/endeffector/Intake.java | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/endeffector/Intake.java 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); + } +} From 4b856480a0c32f22d670b97bb021acfb195e7482 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sat, 15 Mar 2025 14:03:50 -0500 Subject: [PATCH 02/19] more intake --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- .../robot/commands/EndEffectorCommands/IntakeCommand.java | 7 ++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e689f77..528316c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,6 +58,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); @@ -94,6 +95,7 @@ public RobotContainer() climber.setDefaultCommand(new RunCommand(() -> { }, climber)); + intake.setDefaultCommand(new RunCommand(() -> {}, intake)); elevator.setDefaultCommand(new RunCommand(() -> { elevator.setSpeed(-operatorController.getLeftY()*.1); //Multiply by .1 for testing @@ -123,8 +125,8 @@ private void configureBindings() operatorController.rightBumper().whileTrue(new IntakeCommand(-.1)); operatorController.leftBumper().whileTrue(new IntakeCommand(.1)); - - + + // Driver Controls driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java index 1560887..5d137e2 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; + public class IntakeCommand extends Command { private final double speed; @@ -15,18 +16,18 @@ public IntakeCommand(Double _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 From b2bd71c276965977dbd81a61f7785a139247bbe6 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sat, 15 Mar 2025 16:47:39 -0500 Subject: [PATCH 03/19] today's work --- 2025ProjBotCode | 1 + src/main/deploy/pathplanner/settings.json | 22 ++++----- src/main/java/frc/robot/Constants.java | 13 +++++ src/main/java/frc/robot/RobotContainer.java | 21 +++++---- .../ClimberCommands/ExtendWinchCommand.java | 8 +++- .../ClimberCommands/GrabCageCommand.java | 8 +++- .../ElevatorCommands/ElevatorPreset.java | 47 +++++++++++++++++++ .../EndEffectorCommands/IntakeCommand.java | 2 +- .../EndEffectorCommands/ShootCommand.java | 8 +++- .../elevator/ElevatorSubsystem.java | 17 +++---- .../endeffector/EndEffectorSubsystem.java | 6 --- 11 files changed, 112 insertions(+), 41 deletions(-) create mode 160000 2025ProjBotCode create mode 100644 src/main/java/frc/robot/commands/ElevatorCommands/ElevatorPreset.java 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/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/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ce151e6..919009a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,6 +46,19 @@ 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 startingHeight = 15.25; + public static final double endingHeight = 59.25; + public static final double startingEncoder = 0; + public static final double endingEncoder = 110; + + public static final double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); + public static final double L1Height = 0; + public static final double L2Height = 57; + public static final double L3Height = 98; + public static final double L4Height = 135; + } public static class SystemConfig { public static final PIDController PIVOT_PID = new PIDController(0.1, 0, 0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 528316c..5313e83 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,6 +37,7 @@ 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; @@ -87,7 +88,7 @@ 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); @@ -120,23 +121,25 @@ private void configureBindings() // Operator Controls - operatorController.rightTrigger().whileTrue(new ShootCommand(-.1)); - operatorController.leftTrigger().whileTrue(new ShootCommand(.1)); + operatorController.rightTrigger().whileTrue(new ShootCommand(-1.25)); + operatorController.leftTrigger().whileTrue(new ShootCommand(1.25)); operatorController.rightBumper().whileTrue(new IntakeCommand(-.1)); operatorController.leftBumper().whileTrue(new IntakeCommand(.1)); - - + operatorController.x().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L1Height,elevator)); + operatorController.y().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L2Height,elevator)); + operatorController.b().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L3Height,elevator)); + operatorController.a().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L4Height,elevator)); // Driver Controls driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); - driverController.rightTrigger().whileTrue(new ExtendWinchCommand(-.1)); - driverController.leftTrigger().whileTrue(new ExtendWinchCommand(.1)); + driverController.leftTrigger().whileTrue(new ExtendWinchCommand(-.2)); + driverController.rightTrigger().whileTrue(new ExtendWinchCommand(.2)); - driverController.rightBumper().whileTrue(new GrabCageCommand(.1)); - driverController.leftBumper().whileTrue(new GrabCageCommand(-1.)); + driverController.rightBumper().whileTrue(new GrabCageCommand(1.0)); + driverController.leftBumper().whileTrue(new GrabCageCommand(-1.0)); } public void setMotorBrake(boolean brake) 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 5d137e2..f77f867 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -10,7 +10,7 @@ public class IntakeCommand extends Command public IntakeCommand(Double _speed) { - addRequirements(RobotContainer.endEffector); + addRequirements(RobotContainer.intake); speed = _speed; } 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/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 6e07635..4dec770 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -17,7 +17,6 @@ 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); private SparkMaxConfig elevator1Config = new SparkMaxConfig(); private SparkMaxConfig elevator2Config = new SparkMaxConfig(); @@ -29,7 +28,7 @@ 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(.2,0,0); public ElevatorSubsystem() { @@ -43,8 +42,7 @@ public ElevatorSubsystem() } public void goToHeight(double height){ - height = height - 15.25; //15.25 is starting height - double voltage = elevatorPID.calculate(this.getHeight(), height); + double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); if(Math.abs(voltage) > 3){ voltage = (voltage)/Math.abs(voltage)*3; } @@ -56,7 +54,14 @@ public void goToHeight(double height){ public double getHeight(){ return elevator1.getEncoder().getPosition()*inchesPerEncoder; } + public void setVoltage(double voltage) { + elevator1.setVoltage(voltage); + elevator2.setVoltage(voltage); + } + public boolean atSetpoint(){ + return elevatorPID.atSetpoint(); + } public void setSpeedElevator1(double speed) { @@ -68,10 +73,6 @@ 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); diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index a36126c..8968efc 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -12,13 +12,11 @@ public class EndEffectorSubsystem extends SubsystemBase { private final SparkMax endEffectorMotor; private final SparkMax endEffectorTilt; - private final SparkMax endEffectorIntake; 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); } @@ -32,9 +30,6 @@ public void setSpeedEndEffectorTilt(double speed) endEffectorTilt.set(speed); } - public void setSpeedEndEffectorIntake(double speed){ - endEffectorIntake.set(speed); - } @Override public void periodic() @@ -42,6 +37,5 @@ 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()); } } \ No newline at end of file From f42a4d9d53f53e09f2335dfd971d9e20b9cd213a Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sun, 16 Mar 2025 17:30:08 -0500 Subject: [PATCH 04/19] today's work --- src/main/java/frc/robot/Constants.java | 7 ---- src/main/java/frc/robot/RobotContainer.java | 42 ++++++++++++++----- .../subsystems/climb/ClimberSubsystem.java | 23 ++++++++++ .../elevator/ElevatorSubsystem.java | 21 +++++++++- 4 files changed, 75 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 919009a..1aded8f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,13 +47,6 @@ public static class DrivetrainConfig { public static final PIDConstants TURN_PID = new PIDConstants(0.5, 0, 0); } public static class ElevatorConstants{ - - public static final double startingHeight = 15.25; - public static final double endingHeight = 59.25; - public static final double startingEncoder = 0; - public static final double endingEncoder = 110; - - public static final double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); public static final double L1Height = 0; public static final double L2Height = 57; public static final double L3Height = 98; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5313e83..6dc84d6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,11 +95,41 @@ public RobotContainer() climber.setDefaultCommand(new RunCommand(() -> { + // Driver Controls + driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); + driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); + + + if(driverController.rightTrigger().getAsBoolean()){ + if(climber.lowerLimitReachedWinch()){ + climber.setSpeed(0); + climber.resetEncoder(); + } + climber.setSpeed(.2); + } + else if(driverController.leftTrigger().getAsBoolean()){ + climber.setSpeed(-.2); + } + driverController.rightBumper().whileTrue(new GrabCageCommand(1.0)); + driverController.leftBumper().whileTrue(new GrabCageCommand(-1.0)); + }, climber)); + + intake.setDefaultCommand(new RunCommand(() -> {}, intake)); elevator.setDefaultCommand(new RunCommand(() -> { - elevator.setSpeed(-operatorController.getLeftY()*.1); //Multiply by .1 for testing + //if the lower limit's been reached don't allow them to go down but allow them to go up. + if(elevator.lowerLimitReached() == true && -operatorController.getLeftY() <= 0) { + //Multiply by .1 for testing + elevator.resetEncoder(); + elevator.setSpeed(0); + + } + else{ + elevator.setSpeed(-operatorController.getLeftY()*.2); + } + }, elevator)); endEffector.setDefaultCommand(new RunCommand(() -> { @@ -131,15 +161,7 @@ private void configureBindings() operatorController.b().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L3Height,elevator)); operatorController.a().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L4Height,elevator)); - // Driver Controls - driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); - driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); - - driverController.leftTrigger().whileTrue(new ExtendWinchCommand(-.2)); - driverController.rightTrigger().whileTrue(new ExtendWinchCommand(.2)); - - driverController.rightBumper().whileTrue(new GrabCageCommand(1.0)); - driverController.leftBumper().whileTrue(new GrabCageCommand(-1.0)); + } public void setMotorBrake(boolean brake) 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 4dec770..ac119f5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -3,11 +3,14 @@ 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.controller.PIDController; +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,6 +18,8 @@ public class ElevatorSubsystem extends SubsystemBase { + DigitalInput elevatorLowerSwitch; + private final SparkMax elevator1 = new SparkMax(CANConfig.ELEVATOR_LEFT, MotorType.kBrushless); private final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless); @@ -32,6 +37,8 @@ public class ElevatorSubsystem extends SubsystemBase public ElevatorSubsystem() { + elevatorLowerSwitch = new DigitalInput(2); + elevator1Config.inverted(true); elevator2Config.inverted(false); elevator1Config.idleMode(IdleMode.kBrake); @@ -51,6 +58,10 @@ public void goToHeight(double height){ elevator2.setVoltage(voltage); } + public void resetEncoder(){ + elevator1.getEncoder().setPosition(0); + elevator2.getEncoder().setPosition(0); + } public double getHeight(){ return elevator1.getEncoder().getPosition()*inchesPerEncoder; } @@ -78,7 +89,14 @@ public void setSpeed(double speed){ elevator2.set(speed); } - //elevator positions + + public Boolean lowerLimitReached() { + if(elevatorLowerSwitch.get()) + return false; + else{ + return true; + } + } @Override @@ -86,5 +104,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 From 0a6d081d4507b0f176533dae823bc1a346f86da8 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Mon, 17 Mar 2025 20:59:32 -0500 Subject: [PATCH 05/19] 3/17 --- src/main/java/frc/robot/Constants.java | 26 ++++++++-- src/main/java/frc/robot/RobotContainer.java | 47 +++++++++++++++---- .../EndEffectorCommands/TiltPreset.java | 47 +++++++++++++++++++ .../endeffector/EndEffectorSubsystem.java | 40 ++++++++++++++-- 4 files changed, 142 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/robot/commands/EndEffectorCommands/TiltPreset.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1aded8f..f42922a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,10 +47,28 @@ public static class DrivetrainConfig { public static final PIDConstants TURN_PID = new PIDConstants(0.5, 0, 0); } public static class ElevatorConstants{ - public static final double L1Height = 0; - public static final double L2Height = 57; - public static final double L3Height = 98; - public static final double L4Height = 135; + public static final double L1Height = 20; + public static final double L2Height = 183; + public static final double L3Height = 329; + public static final double L4Height = 528; + } + 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 = 1.285715; + public static final double tiltL3 = .928572; + public static final double tiltL4 = .785715; } public static class SystemConfig { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6dc84d6..8d81e36 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,6 +41,7 @@ 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; @@ -109,6 +110,9 @@ public RobotContainer() } else if(driverController.leftTrigger().getAsBoolean()){ climber.setSpeed(-.2); + } + else{ + climber.setSpeed(0); } driverController.rightBumper().whileTrue(new GrabCageCommand(1.0)); driverController.leftBumper().whileTrue(new GrabCageCommand(-1.0)); @@ -124,10 +128,19 @@ else if(driverController.leftTrigger().getAsBoolean()){ //Multiply by .1 for testing elevator.resetEncoder(); elevator.setSpeed(0); + if((endEffector.endEffectorTilt.getEncoder().getPosition() >= -1.2)||(endEffector.endEffectorTilt.getEncoder().getPosition() <= .4 )){ + new TiltPreset(-.4,endEffector); + } } else{ - elevator.setSpeed(-operatorController.getLeftY()*.2); + if(endEffector.endEffectorTilt.getEncoder().getPosition() > 5.6) + { + elevator.setSpeed(-operatorController.getLeftY()*.6); + } + else{ + endEffector.goToTilt(5.7); + } } }, elevator)); @@ -150,16 +163,30 @@ 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 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); - operatorController.rightTrigger().whileTrue(new ShootCommand(-1.25)); - operatorController.leftTrigger().whileTrue(new ShootCommand(1.25)); - - operatorController.rightBumper().whileTrue(new IntakeCommand(-.1)); - operatorController.leftBumper().whileTrue(new IntakeCommand(.1)); - operatorController.x().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L1Height,elevator)); - operatorController.y().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L2Height,elevator)); - operatorController.b().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L3Height,elevator)); - operatorController.a().whileTrue(new ElevatorPreset(Constants.ElevatorConstants.L4Height,elevator)); + SequentialCommandGroup goToPresetL1 = new SequentialCommandGroup(tiltL1PresetCommand, heightL1PresetCommand); + SequentialCommandGroup goToPresetL2 = new SequentialCommandGroup(tiltL2PresetCommand, heightL2PresetCommand); + SequentialCommandGroup goToPresetL3 = new SequentialCommandGroup(tiltL3PresetCommand, heightL3PresetCommand); + SequentialCommandGroup goToPresetL4 = new SequentialCommandGroup(tiltL4PresetCommand, heightL4PresetCommand); + + 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)); + operatorController.x().whileTrue(goToPresetL1); + operatorController.y().whileTrue(goToPresetL2); + operatorController.b().whileTrue(goToPresetL3); + operatorController.a().whileTrue(goToPresetL4); 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/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index 8968efc..656ce85 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -3,15 +3,19 @@ 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; + public final SparkMax endEffectorMotor; + public final SparkMax endEffectorTilt; + + private PIDController tiltPIDController = new PIDController(.1, 0.0, 0.0); public EndEffectorSubsystem() { @@ -29,7 +33,31 @@ public void setSpeedEndEffectorTilt(double speed) { endEffectorTilt.set(speed); } - + + public void setPIDGains(double P, double I, double D){ + tiltPIDController.setPID(P, I, D); + } + + public void goToTilt(double encoders){ + encoders = encoders - tiltConstants.tiltOffset; + double speed = tiltPIDController.calculate(endEffectorTilt.getEncoder().getPosition(), encoders); + if(speed > .5){ + speed = (speed/Math.abs(speed))*.5; + } + 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() @@ -38,4 +66,8 @@ public void periodic() SmartDashboard.putNumber("End Effector Tilt", endEffectorTilt.getEncoder().getPosition()); SmartDashboard.putNumber("End Effector Tilt Absolute Encoder", endEffectorTilt.getAlternateEncoder().getPosition()); } + + public boolean atSetPoint() { + return tiltPIDController.atSetpoint(); + } } \ No newline at end of file From 18c009046768094eca85f8b6f2e495066c2948bf Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Tue, 18 Mar 2025 18:10:55 -0500 Subject: [PATCH 06/19] 3/18 --- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/RobotContainer.java | 54 ++++++++++++------- .../elevator/ElevatorSubsystem.java | 8 +-- .../endeffector/EndEffectorSubsystem.java | 6 +-- 4 files changed, 47 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f42922a..0fe32cf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -51,6 +51,8 @@ public static class ElevatorConstants{ public static final double L2Height = 183; public static final double L3Height = 329; public static final double L4Height = 528; + public static final double A1Height = 104; + public static final double A2Height = 165; } public static class tiltConstants{ public static double startingHeight = 15.25; @@ -69,6 +71,8 @@ public static class tiltConstants{ public static final double tiltL2 = 1.285715; public static final double tiltL3 = .928572; public static final double tiltL4 = .785715; + public static final double tiltA1 = 22.118935; + public static final double tiltA2 = 22.118935; } public static class SystemConfig { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d81e36..4258a41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -126,27 +126,36 @@ else if(driverController.leftTrigger().getAsBoolean()){ //if the lower limit's been reached don't allow them to go down but allow them to go up. if(elevator.lowerLimitReached() == true && -operatorController.getLeftY() <= 0) { //Multiply by .1 for testing - elevator.resetEncoder(); - elevator.setSpeed(0); - if((endEffector.endEffectorTilt.getEncoder().getPosition() >= -1.2)||(endEffector.endEffectorTilt.getEncoder().getPosition() <= .4 )){ - new TiltPreset(-.4,endEffector); - } - + elevator.resetEncoder(); + elevator.setSpeed(0); } else{ - if(endEffector.endEffectorTilt.getEncoder().getPosition() > 5.6) - { - elevator.setSpeed(-operatorController.getLeftY()*.6); - } - else{ - endEffector.goToTilt(5.7); - } + // if(endEffector.endEffectorTilt.getEncoder().getPosition() > 5.6) + // { + elevator.setSpeed(-operatorController.getLeftY()*.9); + // } + // else{ + // endEffector.goToTilt(5.7); + // } } }, elevator)); endEffector.setDefaultCommand(new RunCommand(() -> { - endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1); //We are using this to test, the .1 is to make it go slow + 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()>= 152) { + endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1); //We are using this to test, the .1 is to make it go slow + }else{ + endEffector.goToTilt(5.7); //this is the value to make sure the end effector clears the funnel + } }, endEffector)); } @@ -167,16 +176,22 @@ private void configureBindings() 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); - SequentialCommandGroup goToPresetL1 = new SequentialCommandGroup(tiltL1PresetCommand, heightL1PresetCommand); - SequentialCommandGroup goToPresetL2 = new SequentialCommandGroup(tiltL2PresetCommand, heightL2PresetCommand); - SequentialCommandGroup goToPresetL3 = new SequentialCommandGroup(tiltL3PresetCommand, heightL3PresetCommand); - SequentialCommandGroup goToPresetL4 = new SequentialCommandGroup(tiltL4PresetCommand, heightL4PresetCommand); + 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.rightTrigger().whileTrue(new ShootCommand(-2.5)); operatorController.leftTrigger().whileTrue(new ShootCommand(2.5)); @@ -187,6 +202,9 @@ private void configureBindings() operatorController.y().whileTrue(goToPresetL2); operatorController.b().whileTrue(goToPresetL3); operatorController.a().whileTrue(goToPresetL4); + //operatorController.povDown().whileTrue(goToPresetA1); + //operatorController.povUp().whileTrue(goToPresetA2); + diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index ac119f5..9b83abe 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -18,10 +18,10 @@ public class ElevatorSubsystem extends SubsystemBase { - DigitalInput elevatorLowerSwitch; + public DigitalInput elevatorLowerSwitch; - private final SparkMax elevator1 = new SparkMax(CANConfig.ELEVATOR_LEFT, MotorType.kBrushless); - private final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless); + 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(); @@ -37,7 +37,7 @@ public class ElevatorSubsystem extends SubsystemBase public ElevatorSubsystem() { - elevatorLowerSwitch = new DigitalInput(2); + this.elevatorLowerSwitch = new DigitalInput(2); elevator1Config.inverted(true); elevator2Config.inverted(false); diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index 656ce85..72717b2 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -15,12 +15,13 @@ public class EndEffectorSubsystem extends SubsystemBase public final SparkMax endEffectorMotor; public final SparkMax endEffectorTilt; - private PIDController tiltPIDController = new PIDController(.1, 0.0, 0.0); + private PIDController tiltPIDController = new PIDController(.025, 0.0, 0.0); public EndEffectorSubsystem() { endEffectorMotor = new SparkMax(CANConfig.END_EFFECTOR_MOTOR, MotorType.kBrushless); endEffectorTilt = new SparkMax(CANConfig.END_EFFECTOR_TILT, MotorType.kBrushless); + tiltPIDController.setTolerance(.5); } @@ -38,8 +39,7 @@ public void setPIDGains(double P, double I, double D){ tiltPIDController.setPID(P, I, D); } - public void goToTilt(double encoders){ - encoders = encoders - tiltConstants.tiltOffset; + public void goToTilt(double encoders){ double speed = tiltPIDController.calculate(endEffectorTilt.getEncoder().getPosition(), encoders); if(speed > .5){ speed = (speed/Math.abs(speed))*.5; From 1cb933228300918cdf429e265bb169a9b39ed0c3 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Tue, 18 Mar 2025 20:04:45 -0500 Subject: [PATCH 07/19] 3/18 --- .../swerve/modules/physicalproperties.json | 2 +- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 19 +++++++++---------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 64e678c..6580255 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.5, "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 0fe32cf..eda8903 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,9 +48,9 @@ public static class DrivetrainConfig { } public static class ElevatorConstants{ public static final double L1Height = 20; - public static final double L2Height = 183; + public static final double L2Height = 171; public static final double L3Height = 329; - public static final double L4Height = 528; + public static final double L4Height = 506; public static final double A1Height = 104; public static final double A2Height = 165; } @@ -68,9 +68,9 @@ public static class tiltConstants{ public static final double tiltMaxSpeed = 2.5; public static final double tiltL1 = 3.380949; - public static final double tiltL2 = 1.285715; + public static final double tiltL2 = 4.85710; public static final double tiltL3 = .928572; - public static final double tiltL4 = .785715; + public static final double tiltL4 = 3.499997; public static final double tiltA1 = 22.118935; public static final double tiltA2 = 22.118935; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4258a41..80f76c2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,18 +143,17 @@ else if(driverController.leftTrigger().getAsBoolean()){ endEffector.setDefaultCommand(new RunCommand(() -> { if(elevator.lowerLimitReached() == true /*&& -operatorController.getLeftY() <= 0*/) { - if((endEffector.endEffectorTilt.getEncoder().getPosition() <= -.8)||(endEffector.endEffectorTilt.getEncoder().getPosition() >= 0 )){ - if(endEffector.atSetPoint() == false){ + //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()>= 152) { - endEffector.setSpeedEndEffectorTilt(-operatorController.getRightY()*.1); //We are using this to test, the .1 is to make it go slow + //}else{ + // endEffector.setSpeedEndEffectorTilt(0); + //} + //} + }else if((elevator.elevator1.getEncoder().getPosition() <= 155)) { + endEffector.goToTilt(5.7); }else{ - endEffector.goToTilt(5.7); //this is the value to make sure the end effector clears the funnel + 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)); } From 2bb426c19c494f37e6351699ee6bdc2246c475b7 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Tue, 18 Mar 2025 20:08:10 -0500 Subject: [PATCH 08/19] 3/18 --- .../java/frc/robot/subsystems/elevator/ElevatorSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 9b83abe..075adf7 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -50,8 +50,8 @@ public ElevatorSubsystem() public void goToHeight(double height){ double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); - if(Math.abs(voltage) > 3){ - voltage = (voltage)/Math.abs(voltage)*3; + if(Math.abs(voltage) > 6){ + voltage = (voltage)/Math.abs(voltage)*6; } SmartDashboard.putNumber("elevator PID Voltage", voltage); elevator1.setVoltage(voltage); From 863f092a41f1b71d0b9c1b39fdd8d3b390c9fa06 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Thu, 20 Mar 2025 10:26:26 -0500 Subject: [PATCH 09/19] 3/20 --- simgui-ds.json | 11 +++++++++++ src/main/java/frc/robot/Robot.java | 16 ++++++++-------- src/main/java/frc/robot/RobotContainer.java | 1 + .../subsystems/swervedrive/SwerveSubsystem.java | 6 +++--- 4 files changed, 23 insertions(+), 11 deletions(-) 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/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f8da5da..bb5c6e9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -106,14 +106,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 80f76c2..ab08ce1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -221,6 +221,7 @@ public Command getAutonomousCommand() { // Pass the auto line for points return autoChooser.getSelected(); + // return driveTrain.getAutonomousCommand("Pass The Line Auto"); } } 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) From 743898e5af1f5b50bf65e1f21dec32e796fb4760 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Thu, 20 Mar 2025 11:23:52 -0500 Subject: [PATCH 10/19] 3/20 --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eda8903..a4fb372 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,9 +48,9 @@ public static class DrivetrainConfig { } public static class ElevatorConstants{ public static final double L1Height = 20; - public static final double L2Height = 171; - public static final double L3Height = 329; - public static final double L4Height = 506; + public static final double L2Height = 152; + public static final double L3Height = 287; + public static final double L4Height = 484; public static final double A1Height = 104; public static final double A2Height = 165; } From 8ebb02cc1c082e9a428e74c1b2f9f90c4a19fc1e Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Thu, 20 Mar 2025 17:18:49 -0500 Subject: [PATCH 11/19] 3/20 --- .../deploy/swerve/modules/physicalproperties.json | 2 +- src/main/java/frc/robot/Constants.java | 11 ++++++----- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../robot/subsystems/elevator/ElevatorSubsystem.java | 6 +++--- .../subsystems/endeffector/EndEffectorSubsystem.java | 6 +++--- 5 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 6580255..fe85238 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -18,7 +18,7 @@ } }, "rampRate": { - "drive": 0.5, + "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 a4fb372..01320b6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,9 +48,9 @@ public static class DrivetrainConfig { } public static class ElevatorConstants{ public static final double L1Height = 20; - public static final double L2Height = 152; - public static final double L3Height = 287; - public static final double L4Height = 484; + public static final double L2Height = 173; + public static final double L3Height = 302; + public static final double L4Height = 507; public static final double A1Height = 104; public static final double A2Height = 165; } @@ -83,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; @@ -102,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; @@ -126,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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab08ce1..f7e989f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -106,10 +106,10 @@ public RobotContainer() climber.setSpeed(0); climber.resetEncoder(); } - climber.setSpeed(.2); + climber.setSpeed(3); } else if(driverController.leftTrigger().getAsBoolean()){ - climber.setSpeed(-.2); + climber.setSpeed(-3); } else{ climber.setSpeed(0); @@ -132,7 +132,7 @@ else if(driverController.leftTrigger().getAsBoolean()){ else{ // if(endEffector.endEffectorTilt.getEncoder().getPosition() > 5.6) // { - elevator.setSpeed(-operatorController.getLeftY()*.9); + elevator.setSpeed(-operatorController.getLeftY()*3); // } // else{ // endEffector.goToTilt(5.7); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 075adf7..8881e34 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -33,7 +33,7 @@ public class ElevatorSubsystem extends SubsystemBase private double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); - private PIDController elevatorPID = new PIDController(.2,0,0); + private PIDController elevatorPID = new PIDController(.8,0,0); public ElevatorSubsystem() { @@ -50,8 +50,8 @@ public ElevatorSubsystem() public void goToHeight(double height){ double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); - if(Math.abs(voltage) > 6){ - voltage = (voltage)/Math.abs(voltage)*6; + if(Math.abs(voltage) > 12){ + voltage = (voltage)/Math.abs(voltage)*12; } SmartDashboard.putNumber("elevator PID Voltage", voltage); elevator1.setVoltage(voltage); diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index 72717b2..6ba347e 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -15,7 +15,7 @@ public class EndEffectorSubsystem extends SubsystemBase public final SparkMax endEffectorMotor; public final SparkMax endEffectorTilt; - private PIDController tiltPIDController = new PIDController(.025, 0.0, 0.0); + private PIDController tiltPIDController = new PIDController(.06, 0.0, 0.0); public EndEffectorSubsystem() { @@ -41,8 +41,8 @@ public void setPIDGains(double P, double I, double D){ public void goToTilt(double encoders){ double speed = tiltPIDController.calculate(endEffectorTilt.getEncoder().getPosition(), encoders); - if(speed > .5){ - speed = (speed/Math.abs(speed))*.5; + if(speed > .85){ + speed = (speed/Math.abs(speed))*.85; } endEffectorTilt.set(speed); } From 800982be8a8c1f9a3c1b12eb5317069ecd59fb74 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Fri, 21 Mar 2025 13:11:51 -0500 Subject: [PATCH 12/19] Jacob S. CSA Code Fix --- src/main/java/frc/robot/RobotContainer.java | 41 +++++++++++-------- .../EndEffectorCommands/IntakeCommand.java | 2 +- .../EndEffectorCommands/ShootCommand.java | 2 +- 3 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7e989f..7d3af7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,11 +94,8 @@ public RobotContainer() driveTrain.setDefaultCommand(driveFieldOrientedAnglularVelocity); - - climber.setDefaultCommand(new RunCommand(() -> { + climber.setDefaultCommand(Commands.run(() -> { // Driver Controls - driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();})); - driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();})); if(driverController.rightTrigger().getAsBoolean()){ @@ -114,15 +111,14 @@ else if(driverController.leftTrigger().getAsBoolean()){ else{ climber.setSpeed(0); } - driverController.rightBumper().whileTrue(new GrabCageCommand(1.0)); - driverController.leftBumper().whileTrue(new GrabCageCommand(-1.0)); + }, climber)); - intake.setDefaultCommand(new RunCommand(() -> {}, intake)); + intake.setDefaultCommand(new InstantCommand(() -> {}, intake)); - elevator.setDefaultCommand(new RunCommand(() -> { + elevator.setDefaultCommand(new InstantCommand(() -> { //if the lower limit's been reached don't allow them to go down but allow them to go up. if(elevator.lowerLimitReached() == true && -operatorController.getLeftY() <= 0) { //Multiply by .1 for testing @@ -141,7 +137,7 @@ else if(driverController.leftTrigger().getAsBoolean()){ }, elevator)); - endEffector.setDefaultCommand(new RunCommand(() -> { + 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){ @@ -192,21 +188,32 @@ private void configureBindings() //SequentialCommandGroup goToPresetA1 = new SequentialCommandGroup(tiltA1PresetCommand, heightA1PresetCommand); //SequentialCommandGroup goToPresetA2 = new SequentialCommandGroup(tiltA2PresetCommand, heightA2PresetCommand); - operatorController.rightTrigger().whileTrue(new ShootCommand(-2.5)); - operatorController.leftTrigger().whileTrue(new ShootCommand(2.5)); + // 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)); + // operatorController.x().whileTrue(goToPresetL1); + // operatorController.y().whileTrue(goToPresetL2); + // operatorController.b().whileTrue(goToPresetL3); + // operatorController.a().whileTrue(goToPresetL4); + - operatorController.rightBumper().whileTrue(new IntakeCommand(-.2)); - operatorController.leftBumper().whileTrue(new IntakeCommand(.2)); - 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) { diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java index f77f867..1103883 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -32,6 +32,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return false; + return true; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java index c5d738e..a1e45f0 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java @@ -31,6 +31,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return false; + return true; } } \ No newline at end of file From 0f3052b072d53c1aa2e2b91eec74b11fe27f7368 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Fri, 21 Mar 2025 13:12:24 -0500 Subject: [PATCH 13/19] JS CSA Fr this time --- src/main/java/frc/robot/RobotContainer.java | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7d3af7e..fbe30fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -188,19 +188,19 @@ private void configureBindings() //SequentialCommandGroup goToPresetA1 = new SequentialCommandGroup(tiltA1PresetCommand, heightA1PresetCommand); //SequentialCommandGroup goToPresetA2 = new SequentialCommandGroup(tiltA2PresetCommand, heightA2PresetCommand); - // operatorController.rightTrigger().whileTrue(new ShootCommand(-2.5)); - // operatorController.leftTrigger().whileTrue(new ShootCommand(2.5)); + 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)); - // operatorController.x().whileTrue(goToPresetL1); - // operatorController.y().whileTrue(goToPresetL2); - // operatorController.b().whileTrue(goToPresetL3); - // operatorController.a().whileTrue(goToPresetL4); + operatorController.rightBumper().whileTrue(new IntakeCommand(-.2)); + operatorController.leftBumper().whileTrue(new IntakeCommand(.2)); + operatorController.x().whileTrue(goToPresetL1); + operatorController.y().whileTrue(goToPresetL2); + operatorController.b().whileTrue(goToPresetL3); + operatorController.a().whileTrue(goToPresetL4); - //operatorController.povDown().whileTrue(goToPresetA1); - //operatorController.povUp().whileTrue(goToPresetA2); + // operatorController.povDown().whileTrue(goToPresetA1); + // operatorController.povUp().whileTrue(goToPresetA2); driverController.rightBumper() From e1e2240cb1b14386e5694fe9dc3f79f4991ea3c1 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sat, 22 Mar 2025 19:19:49 -0500 Subject: [PATCH 14/19] 3/22 --- src/main/java/frc/robot/Constants.java | 4 +-- src/main/java/frc/robot/RobotContainer.java | 30 +++++++++++++++---- .../EndEffectorCommands/IntakeCommand.java | 2 +- .../EndEffectorCommands/ShootCommand.java | 2 +- .../elevator/ElevatorSubsystem.java | 2 +- 5 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 01320b6..5bb64b0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,8 +69,8 @@ public static class tiltConstants{ public static final double tiltL1 = 3.380949; public static final double tiltL2 = 4.85710; - public static final double tiltL3 = .928572; - public static final double tiltL4 = 3.499997; + public static final double tiltL3 = 6.642862; + public static final double tiltL4 = 5.904757; public static final double tiltA1 = 22.118935; public static final double tiltA2 = 22.118935; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbe30fd..c4f4196 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,8 +103,10 @@ public RobotContainer() climber.setSpeed(0); climber.resetEncoder(); } + else{ climber.setSpeed(3); - } + } + } else if(driverController.leftTrigger().getAsBoolean()){ climber.setSpeed(-3); } @@ -188,11 +190,29 @@ private void configureBindings() //SequentialCommandGroup goToPresetA1 = new SequentialCommandGroup(tiltA1PresetCommand, heightA1PresetCommand); //SequentialCommandGroup goToPresetA2 = new SequentialCommandGroup(tiltA2PresetCommand, heightA2PresetCommand); - operatorController.rightTrigger().whileTrue(new ShootCommand(-2.5)); - operatorController.leftTrigger().whileTrue(new ShootCommand(2.5)); + // 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)); + + operatorController.rightTrigger() + .onTrue(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(-2.5), endEffector)) + .onFalse(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(0.0), endEffector)); + + operatorController.leftTrigger() + .onTrue(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(2.5), endEffector)) + .onFalse(new InstantCommand(() -> endEffector.setSpeedEndEffectorMotor(0.0), endEffector)); + + + 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.rightBumper().whileTrue(new IntakeCommand(-.2)); - operatorController.leftBumper().whileTrue(new IntakeCommand(.2)); operatorController.x().whileTrue(goToPresetL1); operatorController.y().whileTrue(goToPresetL2); operatorController.b().whileTrue(goToPresetL3); diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java index 1103883..f77f867 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -32,6 +32,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return true; + return false; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java index a1e45f0..c5d738e 100644 --- a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java @@ -31,6 +31,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return true; + return false; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 8881e34..2776445 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -33,7 +33,7 @@ public class ElevatorSubsystem extends SubsystemBase private double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); - private PIDController elevatorPID = new PIDController(.8,0,0); + private PIDController elevatorPID = new PIDController(1,0,0); public ElevatorSubsystem() { From 9367bb21ffc51df5b635605ac8ab6097da04d6b7 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sun, 23 Mar 2025 14:47:48 -0500 Subject: [PATCH 15/19] 3/23 --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 10 +++++----- .../robot/subsystems/elevator/ElevatorSubsystem.java | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5bb64b0..21801a1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,9 +48,9 @@ public static class DrivetrainConfig { } public static class ElevatorConstants{ public static final double L1Height = 20; - public static final double L2Height = 173; - public static final double L3Height = 302; - public static final double L4Height = 507; + public static final double L2Height = 76; + public static final double L3Height = 132; + public static final double L4Height = 222; public static final double A1Height = 104; public static final double A2Height = 165; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c4f4196..b8e05a9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -126,11 +126,11 @@ else if(driverController.leftTrigger().getAsBoolean()){ //Multiply by .1 for testing elevator.resetEncoder(); elevator.setSpeed(0); - } - else{ + + }else // // if(endEffector.endEffectorTilt.getEncoder().getPosition() > 5.6) - // { - elevator.setSpeed(-operatorController.getLeftY()*3); + { + elevator.setSpeed(-operatorController.getLeftY()); // } // else{ // endEffector.goToTilt(5.7); @@ -148,7 +148,7 @@ else if(driverController.leftTrigger().getAsBoolean()){ // endEffector.setSpeedEndEffectorTilt(0); //} //} - }else if((elevator.elevator1.getEncoder().getPosition() <= 155)) { + }else if((elevator.elevator1.getEncoder().getPosition() <= 68)) { endEffector.goToTilt(5.7); }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 diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 2776445..2ad5e70 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -50,8 +50,8 @@ public ElevatorSubsystem() public void goToHeight(double height){ double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); - if(Math.abs(voltage) > 12){ - voltage = (voltage)/Math.abs(voltage)*12; + if(Math.abs(voltage) > 2){ + voltage = (voltage)/Math.abs(voltage)*2; } SmartDashboard.putNumber("elevator PID Voltage", voltage); elevator1.setVoltage(voltage); From 0a4b25a2a901dfb21aa179bc4e7d0bf8489a6383 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Sun, 23 Mar 2025 15:30:40 -0500 Subject: [PATCH 16/19] 3/23 --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 21801a1..b808fea 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,9 +48,9 @@ public static class DrivetrainConfig { } public static class ElevatorConstants{ public static final double L1Height = 20; - public static final double L2Height = 76; - public static final double L3Height = 132; - public static final double L4Height = 222; + public static final double L2Height = 67; + public static final double L3Height = 117; + public static final double L4Height = 190; public static final double A1Height = 104; public static final double A2Height = 165; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b8e05a9..bf95dd8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,9 @@ public class RobotContainer .withControllerRotationAxis(driverController::getRightX) .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. From cecaf1b94b51f7be444011e9bae630a03dbd817b Mon Sep 17 00:00:00 2001 From: Poppeseed <651841+Poppeseed@users.noreply.github.com> Date: Sun, 23 Mar 2025 15:44:36 -0500 Subject: [PATCH 17/19] Elevator maintain height --- src/main/java/frc/robot/RobotContainer.java | 46 +++++++++++++------ .../elevator/ElevatorSubsystem.java | 17 +++++-- 2 files changed, 44 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bf95dd8..335eb05 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,22 +123,26 @@ else if(driverController.leftTrigger().getAsBoolean()){ intake.setDefaultCommand(new InstantCommand(() -> {}, intake)); elevator.setDefaultCommand(new InstantCommand(() -> { - //if the lower limit's been reached don't allow them to go down but allow them to go up. - if(elevator.lowerLimitReached() == true && -operatorController.getLeftY() <= 0) { - //Multiply by .1 for testing - elevator.resetEncoder(); - elevator.setSpeed(0); - - }else // - // if(endEffector.endEffectorTilt.getEncoder().getPosition() > 5.6) - { - elevator.setSpeed(-operatorController.getLeftY()); - // } - // else{ - // endEffector.goToTilt(5.7); - // } + 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 / 2.0); + } } - }, elevator)); endEffector.setDefaultCommand(new InstantCommand(() -> { @@ -252,5 +256,17 @@ public Command getAutonomousCommand() 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/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 2ad5e70..847f0d8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -8,7 +8,9 @@ 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; @@ -34,6 +36,8 @@ public class ElevatorSubsystem extends SubsystemBase private double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder); private PIDController elevatorPID = new PIDController(1,0,0); + + private double lastSetpoint = 0; public ElevatorSubsystem() { @@ -50,21 +54,26 @@ public ElevatorSubsystem() public void goToHeight(double height){ double voltage = elevatorPID.calculate(elevator1.getEncoder().getPosition(), height); - if(Math.abs(voltage) > 2){ - voltage = (voltage)/Math.abs(voltage)*2; - } + 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); From 37478b8f6f3bb46451e502b82a7244362941544d Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Tue, 25 Mar 2025 17:42:18 -0500 Subject: [PATCH 18/19] 3/24 --- src/main/java/frc/robot/Constants.java | 12 +++++------ src/main/java/frc/robot/RobotContainer.java | 24 ++++++++++----------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b808fea..df35a2b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,11 +48,11 @@ public static class DrivetrainConfig { } public static class ElevatorConstants{ public static final double L1Height = 20; - public static final double L2Height = 67; - public static final double L3Height = 117; - public static final double L4Height = 190; - public static final double A1Height = 104; - public static final double A2Height = 165; + 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; @@ -70,7 +70,7 @@ public static class tiltConstants{ 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 = 5.904757; + public static final double tiltL4 = 3.071426; public static final double tiltA1 = 22.118935; public static final double tiltA2 = 22.118935; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 335eb05..659bab9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,7 +75,7 @@ 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) @@ -140,7 +140,7 @@ else if(driverController.leftTrigger().getAsBoolean()){ { // elevator.setSpeed(joystick); joystick = handleDeadband(joystick, 0.1); - elevator.goToHeight(elevator.getLastSetpoint() + joystick * Constants.ElevatorConstants.L2Height / 50.0 / 2.0); + elevator.goToHeight(elevator.getLastSetpoint() + joystick * Constants.ElevatorConstants.L2Height / 50.0 / 1.0); } } }, elevator)); @@ -154,8 +154,8 @@ else if(driverController.leftTrigger().getAsBoolean()){ // endEffector.setSpeedEndEffectorTilt(0); //} //} - }else if((elevator.elevator1.getEncoder().getPosition() <= 68)) { - endEffector.goToTilt(5.7); + }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 } @@ -179,22 +179,22 @@ private void configureBindings() 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 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); + Command tiltA1PresetCommand = new TiltPreset(Constants.tiltConstants.tiltA1, endEffector); + Command tiltA2PresetCommand = new TiltPreset(Constants.tiltConstants.tiltA2, endEffector); 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); + SequentialCommandGroup goToPresetA1 = new SequentialCommandGroup(tiltA1PresetCommand, heightA1PresetCommand); + SequentialCommandGroup goToPresetA2 = new SequentialCommandGroup(tiltA2PresetCommand, heightA2PresetCommand); // operatorController.rightTrigger().whileTrue(new ShootCommand(-2.5)); // operatorController.leftTrigger().whileTrue(new ShootCommand(2.5)); @@ -225,8 +225,8 @@ private void configureBindings() operatorController.a().whileTrue(goToPresetL4); - // operatorController.povDown().whileTrue(goToPresetA1); - // operatorController.povUp().whileTrue(goToPresetA2); + operatorController.povDown().whileTrue(goToPresetA1); + operatorController.povUp().whileTrue(goToPresetA2); driverController.rightBumper() From ae20b176d80e0d8fbb9c84cce19f2fadbc839649 Mon Sep 17 00:00:00 2001 From: QC-Elite-648 Date: Thu, 27 Mar 2025 16:35:26 -0500 Subject: [PATCH 19/19] Add current limit --- src/main/java/frc/robot/Robot.java | 4 +++- .../java/frc/robot/subsystems/elevator/ElevatorSubsystem.java | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bb5c6e9..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()) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 847f0d8..8abcfa5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -47,6 +47,8 @@ public ElevatorSubsystem() 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);