diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e7d4846..20db0397 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; @@ -19,11 +18,14 @@ import frc.robot.commands.drive.DriveCommand; import frc.robot.commands.drive.FollowSwerveSampleCommand; import frc.robot.commands.elevator.ManualElevator; +import frc.robot.commands.elevator.ZeroElevator; +import frc.robot.commands.intake.Eject; import frc.robot.extras.util.AllianceFlipper; import frc.robot.extras.util.JoystickUtil; import frc.robot.sim.SimWorld; import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem; import frc.robot.subsystems.algaePivot.PhysicalAlgaePivot; +import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorInterface; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.elevator.PhysicalElevator; @@ -57,7 +59,6 @@ * project. */ public class Robot extends LoggedRobot { - private final VisionSubsystem visionSubsystem; private final SwerveDrive swerveDrive; private final ElevatorSubsystem elevatorSubsystem; @@ -69,7 +70,7 @@ public class Robot extends LoggedRobot { new AlgaePivotSubsystem(new PhysicalAlgaePivot()); private final SimWorld simWorld; - + // private final ZeroElevator zeroElevator = new ZeroElevator(); public AutoFactory autoFactory; public final AutoChooser autoChooser; public Autos autos; @@ -170,7 +171,9 @@ public Robot() { new ModuleInterface() {}, new ModuleInterface() {}, new ModuleInterface() {}); - elevatorSubsystem = new ElevatorSubsystem(new ElevatorInterface() {}); + elevatorSubsystem = new ElevatorSubsystem(new ElevatorInterface() { + + }); simWorld = null; } } @@ -179,7 +182,7 @@ public Robot() { autoFactory = new AutoFactory( swerveDrive::getEstimatedPose, // A function that returns the current robot pose - swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to the + swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to (SwerveSample sample) -> { FollowSwerveSampleCommand followCommand = new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample); @@ -307,17 +310,37 @@ private void configureDriverController() { } private void configureOperatorController() { - operatorController.b().whileTrue(Commands.none()); - operatorController.y().whileTrue(Commands.none()); - operatorController.x().whileTrue(Commands.none()); + // operatorController.b().whileTrue(Commands.none()); + // operatorController.y().whileTrue(Commands.none()); + // operatorController.x().whileTrue(Commands.none()); operatorController .a() .whileTrue(new ManualElevator(elevatorSubsystem, () -> operatorController.getLeftY())); + // Manual zero + operatorController.leftBumper().whileTrue(new ZeroElevator(elevatorSubsystem)); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + // Elevator Safety + if (Math.abs(swerveDrive.getRoll()) >= SwerveConstants.MAX_ROLL_ANGLE // gyro safety + || Math.abs(swerveDrive.getPitch()) + >= SwerveConstants + .MAX_PITCH_ANGLE) + { + new Eject(intakeSubsystem).schedule(); + elevatorSubsystem.setElevatorPosition(0); + + + } + //extra elevator safety + + //homing + elevatorSubsystem.homing(); + + } + /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/frc/robot/commands/elevator/ManualElevator.java b/src/main/java/frc/robot/commands/elevator/ManualElevator.java index 4a9f5867..2714ea03 100644 --- a/src/main/java/frc/robot/commands/elevator/ManualElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ManualElevator.java @@ -8,12 +8,17 @@ import frc.robot.subsystems.elevator.ElevatorSubsystem; import java.util.function.DoubleSupplier; -/* 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 ManualElevator extends Command { ElevatorSubsystem elevatorSubsystem; DoubleSupplier joystickY; - /** Creates a new ManualElevator. */ + /** + * Creates a new ManualElevator. + * + * @param elevatorSubsystem Elevator subsystem + * @param position Position in meters + * @param joystickY = Double Supplier for the joystick + */ public ManualElevator(ElevatorSubsystem elevatorSubsystem, DoubleSupplier joystickY) { this.elevatorSubsystem = elevatorSubsystem; this.joystickY = joystickY; diff --git a/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java b/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java index 320637c8..71a99940 100644 --- a/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java +++ b/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java @@ -12,7 +12,12 @@ public class SetElevatorPosition extends Command { ElevatorSubsystem elevatorSubsystem; double position; - /** Creates a new SetElevatorPosition. */ + /** + * Creates a new SetElevatorPosition. + * + * @param elevatorSubsystem Elevator subsystem + * @param position Position in meters + */ public SetElevatorPosition(ElevatorSubsystem elevatorSubsystem, double position) { this.elevatorSubsystem = elevatorSubsystem; this.position = position; diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java new file mode 100644 index 00000000..9545d4b7 --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorConstants; +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 ZeroElevator extends Command { + /** Creates a new ZeroElevator. */ + ElevatorSubsystem elevatorSubsystem; + + public ZeroElevator(ElevatorSubsystem elevatorSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevatorSubsystem.setElevatorPosition(ElevatorConstants.MIN_HEIGHT); // zeroes it + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevatorSubsystem.setVolts(0); //stop it and stuff + } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 40dbb0ce..9a3bcdfc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -11,7 +11,7 @@ public class ElevatorConstants { public static final int ELEVATOR_LEADER_MOTOR_ID = 0; public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 28; - public static final double ELEVATOR_P = 1; + public static final double ELEVATOR_P = 2; public static final double ELEVATOR_I = 0; public static final double ELEVATOR_D = 0; public static final double ELEVATOR_S = 0; @@ -26,11 +26,13 @@ public class ElevatorConstants { public static final double MAX_HEIGHT = 3; public static final double INCLINE_ANGLE_RADIANS = Units.degreesToRadians(8); public static final boolean SIMULATE_GRAVITY = true; - + //stator and supply public static final double STATOR_CURRENT_LIMIT = 0; public static final double SUPPLY_CURRENT_LIMIT = 0; public static final boolean STATOR_CURRENT_LIMIT_ENABLE = false; public static final boolean SUPPLY_CURRENT_LIMIT_ENABLE = false; + public static final double MAX_STATOR_THRESHOLD = 59343494; + public static final double MIN_STATOR_THRESHOLD = 59343494; public static final double MOTION_MAGIC_MAX_ACCELERATION = 2; public static final double MOTION_MAGIC_CRUISE_VELOCITY = 2; @@ -40,4 +42,5 @@ public class ElevatorConstants { public static final boolean LIMIT_ENABLE = false; public static final double REVERSE_LIMIT = 0.15; public static final boolean REVRESE_LIMIT_ENABLE = false; + } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java index 24faf1a0..0358770f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java @@ -18,7 +18,10 @@ public static class ElevatorInputs { // For values public double followerDutyCycle = 0.0; public double desiredPosition = 0.0; } - + + public default double elevatorStatorCurrent() { + return 0.0; + } public default void updateInputs(ElevatorInputs inputs) {} public default double getElevatorPosition() { @@ -32,4 +35,5 @@ public default void setVolts(double volts) {} public default double getVolts() { return 0.0; } + public default void homing(){} } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 7bc5838c..6723e7a2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -31,6 +31,14 @@ public void setElevatorPosition(double position) { public void setVolts(double volts) { elevatorInterface.setVolts(volts); } + + public double elevatorStatorCurrent() { + return elevatorInterface.elevatorStatorCurrent(); + } + + public void homing() { + elevatorInterface.homing(); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java index ff3eae30..6a90b3ea 100644 --- a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.elevator; +import static edu.wpi.first.units.Units.Rotations; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -13,6 +15,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.HardwareConstants; @@ -30,6 +33,7 @@ public class PhysicalElevator implements ElevatorInterface { private final StatusSignal followerAppliedVoltage; private final StatusSignal followerDutyCycle; private final StatusSignal leaderDutyCycle; + private final StatusSignal elevatorStatorCurrent; private double desiredPosition; @@ -87,7 +91,7 @@ public PhysicalElevator() { followerAppliedVoltage = followerMotor.getMotorVoltage(); followerDutyCycle = followerMotor.getDutyCycle(); leaderDutyCycle = leaderMotor.getDutyCycle(); - + elevatorStatorCurrent = leaderMotor.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( HardwareConstants.STATUS_SIGNAL_FREQUENCY, leaderPosition, @@ -98,6 +102,29 @@ public PhysicalElevator() { followerDutyCycle); desiredPosition = 0.0; + } + /** + * Homing to prevent stalling. If there is too much stator current, that shows stalling, so we just set that to the max/or min positions and stop the motor + * + */ + public void homing() { + if (leaderMotor.getStatorCurrent().getValueAsDouble() >= ElevatorConstants.MAX_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble() >= 0.0) { //below stator threshold AND going in positive direction (going down) + leaderMotor.setPosition(0);//set encoder back to 0 + followerMotor.setPosition(0); // set encoder back to 0 + //leaderMotor.setPosition(Rotations.of(0.0)); + followerMotor.set(0); + leaderMotor.set(0);} + + else if (leaderMotor.getStatorCurrent().getValueAsDouble() <= ElevatorConstants.MIN_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble() <= 0.0) { //above stator threshold AND going in negative direction (going up) + leaderMotor.setPosition(ElevatorConstants.MAX_HEIGHT);//set encoder to max height + followerMotor.setPosition(ElevatorConstants.MAX_HEIGHT); // set encoder to max height + //leaderMotor.setPosition(Rotations.of(0.0)); + followerMotor.set(0); + leaderMotor.set(0); + } + + + } @Override @@ -144,4 +171,5 @@ public void setVolts(double volts) { public double getVolts() { return leaderAppliedVoltage.getValueAsDouble(); } + } diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java index f7fbae2f..b7805ee2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -77,4 +77,7 @@ public void setVolts(double volts) { public double getVolts() { return currentVolts; } + + + } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index 294fdfc1..b25b3963 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -181,4 +181,11 @@ public record ModuleConfig( SensorDirectionValue encoderReversed, InvertedValue turnReversed, InvertedValue driveReversed) {} + + + // The limits for the gyro + public static final double MAX_ROLL_ANGLE = 30; // degrees + public static final double MAX_PITCH_ANGLE = 20; // degrees + // public static final double MAX_ACCEL_X = 69; + // public static final double MAX_ACCEL_Y = 432423; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index f76379ab..930d376f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -309,6 +309,14 @@ public boolean getZeroedSpeeds(ChassisSpeeds speeds) { public double getHeading() { return gyroInputs.yawDegrees; } + /** Returns the pitch of the robot as double*/ + public double getPitch() { + return gyroInputs.pitchDegrees; + } + + public double getRoll() { + return gyroInputs.rollDegrees; + } /** * Gets the rate of rotation of the robot. diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index 4ffef1f6..6d8794c8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java @@ -7,7 +7,8 @@ public interface GyroInterface { @AutoLog public static class GyroInputs { public boolean isConnected = false; - + public double rollDegrees = 0.0; + public double pitchDegrees = 0.0; public double yawDegrees = 0.0; public double yawVelocityDegreesPerSecond = 0.0; public double accelX = 0.0; diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java index 129384c0..6bfcab6e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java @@ -20,6 +20,8 @@ public void updateInputs(GyroInputs inputs) { inputs.yawDegrees = -gyro.getAngle(); inputs.accelX = -gyro.getWorldLinearAccelX(); inputs.accelY = -gyro.getWorldLinearAccelY(); + inputs.pitchDegrees = gyro.getPitch(); + inputs.rollDegrees = gyro.getRoll(); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index cc47b016..afaf640c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java @@ -24,6 +24,8 @@ public SimulatedGyro(SimGyro simGyro) { public void updateInputs(GyroInputs inputs) { inputs.isConnected = false; inputs.yawDegrees = -yawRads; + inputs.rollDegrees = 0.0; + inputs.pitchDegrees = 0.0; inputs.yawVelocityDegreesPerSecond = -yawVelRadsPerSec; inputs.accelX = accelX; inputs.accelY = accelY;