diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2ac3803..0d7dffa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -182,6 +182,11 @@ public static class AlgaeSubsystemConstants { public static class ElevatorSubsystemConstants { public static final int ELEVATOR_LEFT_MOTOR_ID = 11; public static final int ELEVATOR_RIGHT_MOTOR_ID = 12; + + public static final double kSVolts = 0.11356; + public static final double kGVolts = 0.29175; + public static final double kVVoltSecondPerRad = 1.5928; + public static final double kAVoltSecondSquaredPerRad = 0.030171; } public static class ClimberSubsystemConstants { @@ -230,19 +235,19 @@ public static class VisionConstants { public static final ArrayList blueHPTags = new ArrayList<>(Arrays.asList(13, 12)); public static ArrayList HPTags = new ArrayList<>(); - // Standard deviations below are from Team Spectrum 3847’s X-Ray robot + // Standard deviations below are from Team Spectrum 3847s X-Ray robot /** * Standard deviations of model states. Increase these numbers to trust your model's state - * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians, + * estimates less. This matrix is in the form [x, y, theta], with units in meters and radians, * then meters. */ public static final Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 10); /** * Standard deviations of the vision measurements. Increase these numbers to trust global - * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in - * meters and radians. + * measurements from vision less. This matrix is in the form [x, y, theta], with units in meters + * and radians. */ public static final Matrix measurementStdDevs = VecBuilder.fill(5, 5, 500); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 083b423..003b6bf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -150,6 +150,7 @@ public void disabledInit() { RobotContainer.elevatorSubsystem.stop(); RobotContainer.coralHoldAngleSubsystem.stopTilt(); // RobotContainer.climberSubsystem.stop(); + // centerModules.ignoringDisable(true); } @@ -217,6 +218,10 @@ public void autonomousPeriodic() { // have flex create points to follow flexAutoSubsystem.CreatePath(constraints, m_CoralStationChooser.getSelected()); } + // Will constantly get the position of the elevator, + // so that when we go into tele, we have an offset to + // adjust to + RobotContainer.getStart(); } @Override @@ -234,6 +239,11 @@ public void teleopInit() { } else { CommandScheduler.getInstance().cancelAll(); } + // This is for testing- if we already have an offset, + // this will do nothing + if (!RobotContainer.gottenStart()) { + RobotContainer.getStart(); + } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 796bc4a..5a06ea5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -28,6 +29,8 @@ import frc.robot.commands.AlgaeIntakeCommand; import frc.robot.commands.AlgaeShootCommand; import frc.robot.commands.AlignWithNearest; +import frc.robot.commands.ArmLevel2; +import frc.robot.commands.ArmLevel3; import frc.robot.commands.CoralIntakeCommand; import frc.robot.commands.CoralShootCommand; import frc.robot.commands.ElevatorDown; @@ -45,7 +48,6 @@ import frc.robot.commands.algaeStopIntake; import frc.robot.commands.stopCoralIntake; import frc.robot.subsystems.AlgaeSubsystem; -// import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.CoralHoldAngleSubsystem; import frc.robot.subsystems.CoralHoldSubsystem; import frc.robot.subsystems.ElevatorSubsystem; @@ -56,6 +58,10 @@ import java.util.Optional; import swervelib.SwerveInputStream; +// import frc.robot.subsystems.KeyboardInput; + +// import java.util.Scanner; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -66,8 +72,11 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed + public static final CommandXboxController driverXbox = new CommandXboxController(0); final CommandGenericHID genericHID = new CommandGenericHID(1); + final CommandXboxController coralController = new CommandXboxController(1); + // final CommandGenericHID genericHID = new CommandGenericHID(1); // The robot's subsystems and commands are defined here... public static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); @@ -79,6 +88,8 @@ public class RobotContainer { public static PathConstraints Pathconstraints; public static FlexAutoSubsystem flexAutoSubsystem; + // private final KeyboardInput keyboard; + // public Command repeatWristDown = new RepeatCommand(new WristDown(coralHoldAngleSubsystem)); // public Command repeatWristDown = new RepeatCommand(new WristDown(coralHoldAngleSubsystem)); @@ -135,6 +146,7 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + // keyboard = new KeyboardInput(); configureBindings(); flexAutoSubsystem = new FlexAutoSubsystem(); Pathconstraints = @@ -179,6 +191,33 @@ public void Periodic() { m_elevator.setLength(elevatorSubsystem.getPositionEncoder()); m_wrist.setAngle(coralHoldAngleSubsystem.getEncoder()); // m_wrist2.setAngle(climberSubsystem.getEncoder()); + // m_wrist2.setAngle(climberSubsystem.getEncoder()); + + // String key = keyboard.getLastKeyPressed(); + + /*if (key.equalsIgnoreCase("X")) { + new SequentialCommandGroup( + new ParallelCommandGroup( + new ElevatorMoveLevel2(elevatorSubsystem), new ArmLevel2(coralHoldAngleSubsystem)), + new WaitCommand(1), + new CoralShootCommand(coralHoldSubsystem)); + } else if (key.equalsIgnoreCase("S")) { + new SequentialCommandGroup( + new ParallelCommandGroup( + new ElevatorMoveLevel3(elevatorSubsystem), new ArmLevel3(coralHoldAngleSubsystem)), + new WaitCommand(1), + new CoralShootCommand(coralHoldSubsystem)); + } else if (key.equalsIgnoreCase("W")) { + new SequentialCommandGroup( + new ParallelCommandGroup( + new ElevatorMoveLevel4(elevatorSubsystem), new ArmLevel4(coralHoldAngleSubsystem)), + new WaitCommand(1), + new CoralShootCommand(coralHoldSubsystem)); + }*/ + + // m_wrist.setAngle(coralHoldAngleSubsystem.getEncoder()); + // m_wrist2.setAngle(climberSubsystem.getEncoder()); + } /** @@ -204,6 +243,48 @@ private void configureBindings() { drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboard); drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity); + + coralController + .y() + .onTrue( + new ParallelCommandGroup( + new ElevatorMoveLevel3(elevatorSubsystem), new ArmLevel3(coralHoldAngleSubsystem))); + + // DO NOT TOUCH + // NEEDS WORK + // KATHERINE I WILL KILL YOU + // OR AIDEN + // I WILL STAB + + // coralController.povDown().onTrue(new ArmLevel2(coralHoldAngleSubsystem)); + coralController + .b() + .onTrue( + new ParallelCommandGroup( + new ElevatorMoveLevel2(elevatorSubsystem), new ArmLevel2(coralHoldAngleSubsystem))); + + // coralController + // .b() + // .onTrue( + // new SequentialCommandGroup( + // new ParallelCommandGroup( + // new ElevatorIntake(elevatorSubsystem), new + // ArmIntake(coralHoldAngleSubsystem)), + // new WaitCommand(1), + // new CoralIntakeCommand(coralHoldSubsystem))); + // // driverXbox.x().whileTrue(new AlignWithNearest()); + /*driverXbox + .x() + .onTrue( + new SequentialCommandGroup( + new ParallelCommandGroup( + new ElevatorMoveLevel2(elevatorSubsystem), + new ArmLevel2(coralHoldAngleSubsystem) + ), + new WaitCommand(1), + new CoralShootCommand(coralHoldSubsystem) + ) + );*/ driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); driverXbox.a().whileTrue(drivebase.centerModulesCommand()); @@ -213,11 +294,11 @@ private void configureBindings() { driverXbox.y().whileTrue(new AlgaeIntakeCommand(algaeSubsystem)); // driverXbox.y().onFalse(new algaeStopIntake(algaeSubsystem)); - driverXbox.leftBumper().onTrue(new ElevatorMoveLevel3(elevatorSubsystem)); + // driverXbox.leftBumper().onTrue(new ElevatorMoveLevel3(elevatorSubsystem)); driverXbox.rightTrigger().onFalse(new ElevatorStop(elevatorSubsystem)); driverXbox.rightBumper().onTrue(new ElevatorPrevPosition(elevatorSubsystem)); driverXbox.leftTrigger().onFalse(new ElevatorStop(elevatorSubsystem)); - driverXbox.leftTrigger().whileTrue(new ElevatorUp(elevatorSubsystem, 0.65)); + driverXbox.leftTrigger().whileTrue(new ElevatorUp(elevatorSubsystem, 0.64)); driverXbox.rightTrigger().onTrue(new ElevatorDown(elevatorSubsystem, 0.1)); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); driverXbox.povUp().onTrue(new WristUp(coralHoldAngleSubsystem)); @@ -248,6 +329,22 @@ public Command getAutonomousCommand(String pathString) { return drivebase.getAutonomousCommand(pathString); } + // On the disablement of auto, or the start of tele if we + // don't do auto, this will get the starting position of the elevator. + // Basically- through out auto, this will keep the elevators position. + // When it moves to tele, that position is now it's zero. + // The means we now know our offset. + public static void getStart() { + + elevatorSubsystem.getStartPos(); + coralHoldAngleSubsystem.setOGPos(); + elevatorSubsystem.setOgPOSgotten(); + } + + public static boolean gottenStart() { + return elevatorSubsystem.getOgPOSgotten(); + } + public Command getCoralPathCommand(String chooser) { Optional ally = DriverStation.getAlliance(); if (ally.get() == Alliance.Blue) { diff --git a/src/main/java/frc/robot/commands/ArmLevel2.java b/src/main/java/frc/robot/commands/ArmLevel2.java new file mode 100644 index 0000000..d5eaf76 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmLevel2.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CoralHoldAngleSubsystem; +import java.util.logging.Level; +import java.util.logging.Logger; + +public class ArmLevel2 extends Command { + private CoralHoldAngleSubsystem coralHold; + + public ArmLevel2(CoralHoldAngleSubsystem c_Subsystem) { + coralHold = c_Subsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(c_Subsystem); + } + + public void execute() { + coralHold.setLevel(2); + + if (coralHold.atLevel(2)) { + + // Logger.getGlobal().log(Level.INFO, "found level"); + coralHold.stopTilt(); + } + } + + @Override + public boolean isFinished() { + if (coralHold.atLevel(2)) { + Logger.getGlobal().log(Level.INFO, "found level"); + coralHold.stopTilt(); + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmLevel3.java b/src/main/java/frc/robot/commands/ArmLevel3.java new file mode 100644 index 0000000..5c8e3dc --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmLevel3.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CoralHoldAngleSubsystem; +import java.util.logging.Level; +import java.util.logging.Logger; + +public class ArmLevel3 extends Command { + private CoralHoldAngleSubsystem coralHold; + + public ArmLevel3(CoralHoldAngleSubsystem c_Subsystem) { + coralHold = c_Subsystem; + addRequirements(c_Subsystem); + } + + @Override + public void execute() { + coralHold.setLevel(3); + + if (coralHold.atLevel(3)) { + + // Logger.getGlobal().log(Level.INFO, "found level"); + coralHold.stopTilt(); + } + } + + @Override + public boolean isFinished() { + if (coralHold.atLevel(3)) { + Logger.getGlobal().log(Level.INFO, "found level"); + coralHold.stopTilt(); + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmLevel4.java b/src/main/java/frc/robot/commands/ArmLevel4.java new file mode 100644 index 0000000..94d2cf4 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmLevel4.java @@ -0,0 +1,23 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CoralHoldAngleSubsystem; + +public class ArmLevel4 extends Command { + private CoralHoldAngleSubsystem coralHold; + + public ArmLevel4(CoralHoldAngleSubsystem c_Subsystem) { + coralHold = c_Subsystem; + addRequirements(c_Subsystem); + } + + @Override + public void initialize() { + // coralHold.setLevel(); + } + + @Override + public boolean isFinished() { + return coralHold.isFinished(4); + } +} diff --git a/src/main/java/frc/robot/commands/CoralShootCommand.java b/src/main/java/frc/robot/commands/CoralShootCommand.java index 7af4f0a..0d4347f 100644 --- a/src/main/java/frc/robot/commands/CoralShootCommand.java +++ b/src/main/java/frc/robot/commands/CoralShootCommand.java @@ -35,7 +35,8 @@ public void execute() { // Returns true when the command should end. @Override public boolean isFinished() { - // Should come back to this - return true; + // If Milo needs this changed, they can + + return m_Coral.getTrigger(); } } diff --git a/src/main/java/frc/robot/commands/ElevatorMoveLevel2.java b/src/main/java/frc/robot/commands/ElevatorMoveLevel2.java index 41a6cc1..93d4315 100644 --- a/src/main/java/frc/robot/commands/ElevatorMoveLevel2.java +++ b/src/main/java/frc/robot/commands/ElevatorMoveLevel2.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ElevatorSubsystem; +import java.util.logging.Level; +import java.util.logging.Logger; /** An example command that uses an example subsystem. */ public class ElevatorMoveLevel2 extends Command { @@ -19,12 +21,23 @@ public ElevatorMoveLevel2(ElevatorSubsystem subsystem) { } @Override - public void initialize() { - m_subsystem.setLevel(2); + public void execute() { + m_subsystem.goToCoralLevel(2); + + if (m_subsystem.atLevel(2)) { + + // Logger.getGlobal().log(Level.INFO, "found level"); + m_subsystem.stop(); + } } @Override public boolean isFinished() { - return m_subsystem.isFinished(2); + if (m_subsystem.atLevel(2)) { + Logger.getGlobal().log(Level.INFO, "found level"); + m_subsystem.stop(); + return true; + } + return false; } } diff --git a/src/main/java/frc/robot/commands/ElevatorMoveLevel3.java b/src/main/java/frc/robot/commands/ElevatorMoveLevel3.java index a1c04f6..a1bb82f 100644 --- a/src/main/java/frc/robot/commands/ElevatorMoveLevel3.java +++ b/src/main/java/frc/robot/commands/ElevatorMoveLevel3.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ElevatorSubsystem; +import java.util.logging.Level; +import java.util.logging.Logger; /** An example command that uses an example subsystem. */ public class ElevatorMoveLevel3 extends Command { @@ -20,12 +22,23 @@ public ElevatorMoveLevel3(ElevatorSubsystem subsystem) { @Override public void execute() { - m_subsystem.setLevel(3); + m_subsystem.goToCoralLevel(3); + + if (m_subsystem.atLevel(3)) { + + // Logger.getGlobal().log(Level.INFO, "found level"); + m_subsystem.stop(); + } } @Override public boolean isFinished() { - return -16 - 1 >= m_subsystem.getPositionEncoder() - && -16 + 1 <= m_subsystem.getPositionEncoder(); + + if (m_subsystem.atLevel(3)) { + Logger.getGlobal().log(Level.INFO, "found level"); + m_subsystem.stop(); + return true; + } + return false; } } diff --git a/src/main/java/frc/robot/commands/WristDown.java b/src/main/java/frc/robot/commands/WristDown.java index b7ba5a3..1092653 100644 --- a/src/main/java/frc/robot/commands/WristDown.java +++ b/src/main/java/frc/robot/commands/WristDown.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CoralHoldAngleSubsystem; +import java.util.logging.Level; +import java.util.logging.Logger; public class WristDown extends Command { private CoralHoldAngleSubsystem coralHold; @@ -15,6 +17,11 @@ public void initialize() { coralHold.tiltedDown(); } + @Override + public void execute() { + Logger.getGlobal().log(Level.INFO, "down: " + coralHold.getPosition()); + } + @Override public boolean isFinished() { return true; diff --git a/src/main/java/frc/robot/commands/WristUp.java b/src/main/java/frc/robot/commands/WristUp.java index af6a3a5..abf7a90 100644 --- a/src/main/java/frc/robot/commands/WristUp.java +++ b/src/main/java/frc/robot/commands/WristUp.java @@ -2,17 +2,30 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CoralHoldAngleSubsystem; +import java.util.logging.Level; +import java.util.logging.Logger; public class WristUp extends Command { private CoralHoldAngleSubsystem coralHold; public WristUp(CoralHoldAngleSubsystem ca_Subsystem) { coralHold = ca_Subsystem; + + addRequirements(ca_Subsystem); } @Override public void initialize() { + Logger.getGlobal().log(Level.INFO, "UP: " + coralHold.getPosition()); coralHold.tiltUp(); + // if (elevator.getPositionEncoder() != 50) { + // coralHold.tiltUp(); + // } + } + + @Override + public void execute() { + Logger.getGlobal().log(Level.INFO, "UP: " + coralHold.getPosition()); } @Override diff --git a/src/main/java/frc/robot/subsystems/CoralHoldAngleSubsystem.java b/src/main/java/frc/robot/subsystems/CoralHoldAngleSubsystem.java index b50b779..0e976ce 100644 --- a/src/main/java/frc/robot/subsystems/CoralHoldAngleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CoralHoldAngleSubsystem.java @@ -8,6 +8,7 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.controller.ArmFeedforward; @@ -17,6 +18,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ArmConstants; import frc.robot.Constants.CoralHoldAngleSubsystemConstants; +import java.util.logging.Level; +import java.util.logging.Logger; public class CoralHoldAngleSubsystem extends SubsystemBase { @@ -42,18 +45,30 @@ public class CoralHoldAngleSubsystem extends SubsystemBase { // would need to add 'import edu.wpi.first.wpilibj.Encoder;' if we do private RelativeEncoder encoder; + public static final double shooter = 4.82; + public double finalPos; + private double ogPos; + private double setPoint = 21.97; + + private double[] levels = {0, -5.8, -2.9}; + public CoralHoldAngleSubsystem() { motor = new SparkFlex( CoralHoldAngleSubsystemConstants.CORAL_HOLD_ANGLE_MOTOR_ID, MotorType.kBrushless); // Assign motor controller port - motorConfig = new SparkFlexConfig(); + controller = motor.getClosedLoopController(); encoder = motor.getEncoder(); + motorConfig = new SparkFlexConfig(); motorConfig.idleMode(IdleMode.kBrake); - motorConfig.closedLoop.pidf(0f, 0f, 0f, 0f, ClosedLoopSlot.kSlot0); + motorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0f, 0f, 0f, 0f, ClosedLoopSlot.kSlot0); + motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); // encoder = new Encoder(1, 1); // Assign encoder ports wantedPos = encoder.getPosition(); state = new State(wantedPos, 0); @@ -81,6 +96,7 @@ private double setSpeed() { public void tiltedDown() { buttonPressed = true; + Logger.getGlobal().log(Level.INFO, "DOWN " + getPositionEncoder()); motorConfig.inverted(true); motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -106,6 +122,10 @@ public void IntakePosition() { } } + public double getPosition() { + return encoder.getPosition(); + } + /** * Move the arm to L3 place position or 0.73 using the PIDF commands * @@ -121,6 +141,8 @@ public void LPosition() { public void tiltUp() { buttonPressed = true; + Logger.getGlobal().log(Level.INFO, "UP: " + getPositionEncoder()); + motorConfig.inverted(false); motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -149,6 +171,56 @@ public double getEncoder() { } */ + private double getPositionEncoder() { + return encoder.getPosition(); + } + + public void setOGPos() { + ogPos = encoder.getPosition(); + } + + public double getFinalPos() { + return finalPos; + } + + public void setLevel(int levelNeeded) { + // double setPoint = 0; + + // if(ogPos > levels[levelNeeded - 1]){ + // setPoint = levels[levelNeeded - 1] + ogPos; + // } else if (ogPos < levels[levelNeeded - 1]){ + // setPoint = levels[levelNeeded] + ogPos; + // } else{ + // setPoint = levels[levelNeeded - 1]; + // } + + double setPoint = levels[levelNeeded - 1]; + + motorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0.0001f, 0f, 0f, 0.3f, ClosedLoopSlot.kSlot0); + + controller.setReference(setPoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.1); + } + + public boolean atLevel(int levlNeeded) { + Logger.getGlobal().log(Level.INFO, "looking for level"); + Logger.getGlobal().log(Level.INFO, "encoder value " + getPositionEncoder()); + Logger.getGlobal().log(Level.INFO, "target value " + levels[levlNeeded - 1]); + + return getPositionEncoder() < levels[levlNeeded - 1]; + } + + public boolean isFinished(int position) { + + if (shooter - (motor.getEncoder().getPosition()) == 0) { + return true; + } else { + return false; + } + } + /** * Uses the ff calculator to keep the arm in a single place * @@ -168,6 +240,20 @@ public void holdUp(TrapezoidProfile.State setpoint) { /** When you release a button, this is called to stop the tilt. */ public void stopTilt() { motor.set(0); + + motorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0f, 0f, 0f, 0, ClosedLoopSlot.kSlot0); + + motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + State setPoint = new State(getPositionEncoder(), 0); + holdUp(setPoint); + // State setPoint = new State(getPositionEncoder(), 0); + // double ff = feedforward.calculate(setPoint.position * 2 * Math.PI, setPoint.velocity); + // controller.setReference( + // getPositionEncoder(), ControlType.kPosition, ClosedLoopSlot.kSlot0, ff); buttonPressed = false; } } diff --git a/src/main/java/frc/robot/subsystems/CoralHoldSubsystem.java b/src/main/java/frc/robot/subsystems/CoralHoldSubsystem.java index 90b1ed1..6a04f33 100644 --- a/src/main/java/frc/robot/subsystems/CoralHoldSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CoralHoldSubsystem.java @@ -3,7 +3,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.CoralHoldSubsystemConstants; public class CoralHoldSubsystem extends SubsystemBase { @@ -11,6 +13,8 @@ public class CoralHoldSubsystem extends SubsystemBase { private final SparkMax motor; // DigitalInput limitSwitch = new DigitalInput(0); + DigitalInput limitSwitch = new DigitalInput(0); + boolean doingStuff = false; // The endcoder isn't used in the basic form of the subsystem - But we may need it later on // would need to add 'import edu.wpi.first.wpilibj.Encoder;' if we do @@ -53,6 +57,12 @@ public void shoot() { motor.set(-setSpeed() * 1.25); // } + if (!limitSwitch.get()) { + // motorConfig.inverted(false); + // motor.configure(motorConfig, ResetMode.kResetSafeParameters, + // PersistMode.kPersistParameters); + motor.set(-setSpeed() * 1.25); + } } /** @@ -66,10 +76,24 @@ public void intake() { // } else { // motor.set(0); // } + if (limitSwitch.get()) { + motor.set(setSpeed()); + doingStuff = true; + + } else if (limitSwitch.get() && doingStuff == true) { + new WaitCommand(1.2f); + motor.set(0); + doingStuff = false; + } } /** Makes the motor stop. */ public void stop() { motor.set(0); + doingStuff = false; + } + + public boolean getTrigger() { + return limitSwitch.get(); } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 234c5a6..16917c7 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -8,8 +8,10 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ElevatorSubsystemConstants; @@ -23,17 +25,39 @@ public class ElevatorSubsystem extends SubsystemBase { private final SparkFlexConfig leftMotorConfig; private final SparkFlexConfig rightMotorConfig; - @SuppressWarnings("unused") + public boolean levelThere; + + private TrapezoidProfile.State state; + + private boolean gottenOgPos; + private RelativeEncoder encoder; private SparkClosedLoopController closedLoopController; + // private ArmFeedforward armFeedforward = + // new ArmFeedforward( + // ElevatorSubsystemConstants.kSVolts, + // ElevatorSubsystemConstants.kGVolts, + // ElevatorSubsystemConstants.kVVoltSecondPerRad, + // ElevatorSubsystemConstants.kAVoltSecondSquaredPerRad); + // private ElevatorFeedforward feedforward = + // new ElevatorFeedforward( + // ElevatorSubsystemConstants.kSVolts, + // ElevatorSubsystemConstants.kGVolts, + // ElevatorSubsystemConstants.kVVoltSecondPerRad, + // ElevatorSubsystemConstants.kAVoltSecondSquaredPerRad); + // Elevator levels in encoder ticks - public static final double[] levels = {0, -18, -22, -35, -50}; + // 0 - Intake position + // 1 - level 2 + // 2 - level 3 + public static final double[] levels = {0, -15.056, -31.401}; // public static final double[] Angles = {0, 0, 0, 0, 0}; public ElevatorSubsystem() { + leftMotor = new SparkFlex( ElevatorSubsystemConstants.ELEVATOR_LEFT_MOTOR_ID, @@ -55,11 +79,16 @@ public ElevatorSubsystem() { rightMotorConfig.idleMode(IdleMode.kBrake); rightMotorConfig.inverted(false); rightMotorConfig.follow(leftMotor, true); + + leftMotorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0f, 0f, 0f, 0.63f, ClosedLoopSlot.kSlot0); + leftMotor.configure( leftMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); rightMotor.configure( rightMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - leftMotorConfig.closedLoop.pidf(0f, 0f, 0f, 0.63f, ClosedLoopSlot.kSlot0); } public void Periodic() { @@ -97,15 +126,118 @@ public void setLevel(int level) { * -50.1 as reported by the encoder */ public void setSpeed(double speed) { - if (getPositionEncoder() >= 0 && speed > 0) { - leftMotor.set(0); - closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.3); - } else if (getPositionEncoder() <= -50.1 && speed < 0) { - leftMotor.set(0); - closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.3); - } else { - leftMotor.set(speed); + leftMotor.set(speed); + // if (getPositionEncoder() >= 0 && speed > 0) { + // leftMotor.set(0); + // //closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.3); + // } else if (getPositionEncoder() <= -50.1 && speed < 0) { + // leftMotor.set(0); + // //closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.3); + // } else { + // leftMotor.set(speed); + // } + } + + public double finalLevelPos(int levelWanted) { + return levels[levelWanted - 1]; + } + + public void goToCoralLevel(int levelWanted) { + + double setPoint = levels[levelWanted - 1]; + + leftMotorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0.000001f, 0f, 0f, 0.3f, ClosedLoopSlot.kSlot0); + + leftMotor.configure( + leftMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + rightMotor.configure( + rightMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + + closedLoopController.setReference(setPoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.3); + + // double setPoint = levels[levelWanted - 1]; + // // state = new State(setPoint, 0); + + // // double ff = armFeedforward.calculate(state.position * 2 * Math.PI, state.velocity); + // // closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, ff); + + // // double setPoint = levels[levelWanted - 1]; + // // state = new State(setPoint, 0); + // // double ff = feedforward.calculate(state.position, state.velocity); + // // closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, ff); + + // // while (!((setPoint - 1 >= getPositionEncoder()) && (setPoint + 3 <= + // getPositionEncoder()))) { + // // if (setPoint + 3 >= getPositionEncoder()) { + // // leftMotor.set(0.15); + // // //Logger.getGlobal().log(Level.INFO, "Going Down"); + // // } else if (setPoint - 1 <= getPositionEncoder()) { + // // leftMotor.set(-0.3); + // // //Logger.getGlobal().log(Level.INFO, "Going Up"); + // // } + + // // // if (setPoint - 2 >= getPositionEncoder() && setPoint + 2 <= getPositionEncoder()) { + // // // //Logger.getGlobal().log(Level.INFO, "Found L3"); + // // // leftMotor.set(0); + // // // closedLoopController.setReference( + // // // getPositionEncoder(), ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.5); + // // // } + + // // } + // // closedLoopController.setReference( + // // getPositionEncoder(), ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.5); + // if (setPoint + 4 >= getPositionEncoder()) { + // leftMotor.set(0.15); + // //Logger.getGlobal().log(Level.INFO, "Going Down"); + // } else if (setPoint - 1 <= getPositionEncoder()) { + // leftMotor.set(-0.3); + // //Logger.getGlobal().log(Level.INFO, "Going Up"); + // } + + // if (setPoint - 1 >= getPositionEncoder() && setPoint + 4 <= getPositionEncoder()) { + // Logger.getGlobal().log(Level.INFO, "Found L3"); + // leftMotor.set(0); + // closedLoopController.setReference( + // getPositionEncoder(), ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.5); + // } + + // double ff = feedforward.calculate(setpoint.position * 2 * Math.PI, setpoint.velocity); + // closedLoopController.setReference(0, ControlType.kPosition, ClosedLoopSlot.kSlot0, ff); + } + + public boolean atLevel(int levlNeeded) { + Logger.getGlobal().log(Level.INFO, "looking for level"); + Logger.getGlobal().log(Level.INFO, "encoder value " + getPositionEncoder()); + Logger.getGlobal().log(Level.INFO, "target value " + levels[levlNeeded - 1]); + + return getPositionEncoder() < levels[levlNeeded - 1]; + } + + public void getStartPos() { + double ogOffSet = encoder.getPosition(); + levels[1] = levels[1] + ogOffSet; + levels[2] = levels[2] + ogOffSet; + } + + // -35 + // -16 + + public double getLevel() { + if (Robot.isSimulation()) { + return levels[(int) SmartDashboard.getNumber("ElevatorPos", 0)]; } + return leftMotor.getEncoder().getPosition(); + } + + public boolean getOgPOSgotten() { + return gottenOgPos; + } + + public void setOgPOSgotten() { + gottenOgPos = true; } /** @@ -134,10 +266,8 @@ public void goToLevelNoPID(double setPoint) { /** * Move to a position for the elevator to move to using PIDF. Will also update the simulation of * the elevator - * - * @deprecated As of yet the PIDF has not been tuned so this does nothing */ - public void moveToPosition(double position) { + private void moveToPosition(double position) { for (int i = 0; i < levels.length; i++) { if (levels[i] == position) { SmartDashboard.putNumber("ElevatorPos", i); @@ -148,7 +278,18 @@ public void moveToPosition(double position) { /** Stop the elevator from moving and hold the position with a flat 30% feed forward */ public void stop() { - // leftMotor.set(0); + leftMotor.set(0); + + leftMotorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0f, 0f, 0f, 0, ClosedLoopSlot.kSlot0); + + leftMotor.configure( + leftMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + rightMotor.configure( + rightMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + closedLoopController.setReference( getPositionEncoder(), ControlType.kPosition, ClosedLoopSlot.kSlot0, -0.3); } diff --git a/src/main/java/frc/robot/subsystems/KeyboardInput.java b/src/main/java/frc/robot/subsystems/KeyboardInput.java new file mode 100644 index 0000000..7cd2dce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/KeyboardInput.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.awt.event.KeyEvent; +import java.awt.event.KeyListener; +import javax.swing.JFrame; + +public class KeyboardInput extends SubsystemBase { + private String lastKeyPressed = ""; + + public KeyboardInput() { + // Create a JFrame to register the KeyListener + JFrame frame = new JFrame("Key Controller"); + frame.addKeyListener( + new KeyListener() { + @Override + public void keyPressed(KeyEvent e) { + lastKeyPressed = KeyEvent.getKeyText(e.getKeyCode()); + } + + @Override + public void keyReleased(KeyEvent e) { + // Clear the key state when released + lastKeyPressed = ""; + } + + @Override + public void keyTyped(KeyEvent e) { + // Optional: Handle key typed events + } + }); + // frame.setSize(200, 200); // Dummy size + // frame.setVisible(true); + } + + public String getLastKeyPressed() { + return lastKeyPressed; + } +}