From 66a62fddf626ed094f7895c42b13c81b0d09fef1 Mon Sep 17 00:00:00 2001 From: bbdriverstation2 Date: Sat, 8 Mar 2025 09:06:08 -0800 Subject: [PATCH] maybe maybe maybe --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../FieldDriveElevatorLimitedCommand.java | 108 ++++++++++++++++++ .../frc/robot/constants/ArmConstants.java | 6 +- .../frc/robot/constants/DriveConstants.java | 2 +- .../robot/constants/ElevatorConstants.java | 2 +- 5 files changed, 117 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveCommands/FieldDriveElevatorLimitedCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d218216..bd7a84b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -279,15 +279,15 @@ void loadCommands() { driveSubsystem::getRotation )); - DoubleSupplier dPadX = () -> { + DoubleSupplier dPadY = () -> { if (driveController.povLeft().getAsBoolean()) { - return -1.0; - } else if (driveController.povRight().getAsBoolean()) { return 1.0; + } else if (driveController.povRight().getAsBoolean()) { + return -1.0; } else { return 0.0;} }; - DoubleSupplier dPadY = () -> { + DoubleSupplier dPadX = () -> { if (driveController.povDown().getAsBoolean()) { return -1.0; } else if (driveController.povUp().getAsBoolean()) { diff --git a/src/main/java/frc/robot/commands/DriveCommands/FieldDriveElevatorLimitedCommand.java b/src/main/java/frc/robot/commands/DriveCommands/FieldDriveElevatorLimitedCommand.java new file mode 100644 index 0000000..0896722 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands/FieldDriveElevatorLimitedCommand.java @@ -0,0 +1,108 @@ +package frc.robot.commands.DriveCommands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.OperatorInput; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.DriveSubsystem.DriveSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class FieldDriveElevatorLimitedCommand extends Command { + private final DriveSubsystem drive; + private final DoubleSupplier xSupplier; + private final DoubleSupplier ySupplier; + private final DoubleSupplier omegaSupplier; + private final ElevatorSubsystem elevator; + private final Supplier headingSupplier; + + public FieldDriveElevatorLimitedCommand(DriveSubsystem drive, ElevatorSubsystem elevatorSubsystem, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier, Supplier headingSupplier) { + this.drive = drive; + this.xSupplier = xSupplier; + this.ySupplier = ySupplier; + this.omegaSupplier = omegaSupplier; + this.headingSupplier = headingSupplier; + this.elevator = elevatorSubsystem; + } + + + public void execute() { + // Apply deadband + double DEADBAND = 0.1; + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + + + double speedFactor; + if (OperatorInput.slowModeHold.getAsBoolean()) { + speedFactor = DriveConstants.slowSpeed; + } else if (OperatorInput.turboModeHold.getAsBoolean()) { + speedFactor = DriveConstants.turboSpeed; + } else { + speedFactor = DriveConstants.normalSpeed; + } + + speedFactor *= speedRamp(elevator.getLoadHeight(), + ElevatorConstants.minHeight + 0.2, ElevatorConstants.maxHeight, 0, 1); + + + ChassisSpeeds speeds_robotOriented = new ChassisSpeeds( + linearVelocity.getX() * speedFactor, //4.5 is the experimentally determined max velocity + linearVelocity.getY() * speedFactor, + omega * Math.PI * 1.5 + ); + + Rotation2d headingFlipped = headingSupplier.get(); + + if (isFlipped) { + headingFlipped = headingFlipped.plus(Rotation2d.fromDegrees(180)); + } + + drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds_robotOriented, headingFlipped)); + } + + + // returns a ramped speed value + // a value between an min max, gets mapped to a different linear scale + private double speedRamp(double value, double inputMin, double inputMax, double outputMin, double outputMax) { + if (!(value <= inputMax && value >= inputMin)) {return 1;} + double range1 = inputMax - inputMin; + double diff1 = value - inputMin; + double firstScalar = diff1 / range1; + + double range2 = outputMax - outputMin; + return range2 * firstScalar; + }; + + public void end(boolean interrupted) { + drive.stop(); + } +} diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 66ae3cf..42475ce 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -13,10 +13,10 @@ public class ArmConstants { public static final double kSSim = 0; public static double kASim = 0; - public static double kS = 0; - public static double kV = 2; + public static double kS = 0.26427; + public static double kV = 1.5096; public static double kG = 0; - public static double kA = 0; + public static double kA = 0.33715; public static double kP = 0; public static double kI = 0; public static double kD = 0; diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index a81df56..7a10d5d 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -36,7 +36,7 @@ public class DriveConstants { public static final double maxSpeedMetersPerSec = 4.8; public static final double turboSpeed = maxSpeedMetersPerSec; public static final double normalSpeed = maxSpeedMetersPerSec - 1; - public static final double slowSpeed = maxSpeedMetersPerSec - 2; + public static final double slowSpeed = maxSpeedMetersPerSec - 3; public static final double alignmentSpeed = 1; public static final double odometryFrequency = 100.0; // Hz diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index b2e0839..3c42602 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -49,7 +49,7 @@ public class ElevatorConstants { public static double L4 = 0; public static double carriageMass = 20.0; public static double maxHeight = 0.571; - public static double minHeight = -0.340; + public static double minHeight = -0.240; public static double maxVelocity = 1; public static double maxAcceleration = 1;