From 968e73122b2fd892f24c3816159b2faeaa655678 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Fri, 15 Mar 2024 20:15:36 -0400 Subject: [PATCH] better wrist alg and simple ff on aimbot alignment --- src/main/java/frc/robot/RobotContainer.java | 7 +++ .../frc/robot/commands/AimbotCommand.java | 44 +++++++++++++------ .../robot/subsystems/arm/ArmSubsystem.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 4 +- .../subsystems/vision/VisionSubsystem.java | 8 ++-- src/main/java/lib/utils/AimbotUtils.java | 3 +- 6 files changed, 47 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22023469..87189773 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -225,6 +225,13 @@ private void configureButtonBindings() { .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + controller.rightTrigger().whileTrue(m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get())); + controller.leftTrigger().whileTrue(m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get())); + controller.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE)); + + // 96.240234375 + // 60.029296875 + // 2250 m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index 3b586bc5..41a75888 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -2,6 +2,7 @@ import com.gos.lib.properties.pid.PidProperty; import com.gos.lib.properties.pid.WpiPidPropertyBuilder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -36,6 +37,8 @@ public class AimbotCommand extends Command { private boolean m_runKicker; + private static final double TOLERENCE_DEGREES = 10.0; + public AimbotCommand(ArmSubsystem armSubsystem, DriveSubsystem driveSubsystem, ShooterSubsystem shooterSubsystem, @@ -49,6 +52,9 @@ public AimbotCommand(ArmSubsystem armSubsystem, m_smallController = new PIDController(0.0, 0.0, 0.0); m_fastController = new PIDController(0.0, 0.0, 0.0); + m_smallController.enableContinuousInput(-180, 180); + m_fastController.enableContinuousInput(-180, 180); + m_smallProperty = new WpiPidPropertyBuilder("Drive/Aimbot Small", false, m_smallController) .addP(1.0) .addI(0.0) @@ -82,18 +88,19 @@ public void execute() { FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); // TODO make an actual equation for shot time based on distance - double shotTime = 0.5; + double distance = AimbotUtils.getDistanceFromSpeaker(m_driveSubsystem.getVisionPose()); + double shotTime = 1.05; Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d(); Translation2d movingTarget = new Translation2d(); // loop over movement calcs to better adjust for acceleration - if (false) { + if (true) { for (int i = 0; i < 5; i++) { double virtualGoalX = target.getX() - - shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + + shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); double virtualGoalY = target.getY() - - shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + + shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); movingTarget = new Translation2d(virtualGoalX, virtualGoalY); } @@ -103,46 +110,53 @@ public void execute() { // get our desired rotation and error from it double desiredRotationDegs = - AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), - new Translation3d(movingTarget.getX(), movingTarget.getY(), 0.0)) + AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()) .getDegrees(); double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); // if we're far from our setpoint, move faster - double omega; - if (error > 15.0) { + double omega;// = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);; + if (error > 5.0) { omega = m_fastController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); } else { omega = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); } + // add a feedforward component to compensate for horizontal movement + Translation2d linearFieldVelocity = new Translation2d(fieldRelativeSpeed.vx, fieldRelativeSpeed.vy); + Translation2d tangentalVelocity = linearFieldVelocity + .rotateBy(Rotation2d.fromDegrees(-desiredRotationDegs).unaryMinus()); + double tangentalComponent = tangentalVelocity.getX(); + double x = -DriveCommands.setSensitivity(-m_driverController.getLeftY(), 0.25); double y = -DriveCommands.setSensitivity(-m_driverController.getLeftX(), 0.25); + x = MathUtil.applyDeadband(x, 0.1); + y = MathUtil.applyDeadband(y, 0.1); Rotation2d heading; // if red change heading goal if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); - } else { heading = m_driveSubsystem.getRotation(); + } else { + heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); } // Convert to field relative speeds & send command m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( x * Constants.DriveConstants.MAX_LINEAR_SPEED, y * Constants.DriveConstants.MAX_LINEAR_SPEED, - omega * Constants.DriveConstants.MAX_ANGULAR_SPEED, + omega + tangentalComponent * 0.5, heading )); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); - m_shooterSubsystem.runShooterVelocity(m_runKicker); + m_shooterSubsystem.runShooterVelocity(m_runKicker).execute(); // set shooter speeds and rumble controller - if (m_shooterSubsystem.atSpeed() && error < 10.0) { + if (m_shooterSubsystem.atSpeed() && error < 15.0) { m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0); } else { m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); @@ -152,7 +166,9 @@ public void execute() { @Override public void end(boolean interrupted) { m_driveSubsystem.stopWithX(); - m_shooterSubsystem.runShooterVelocity(false, 0.0, 0.0); + m_shooterSubsystem.setShooterPowerLeft(0.0); + m_shooterSubsystem.setShooterPowerRight(0.0); + m_shooterSubsystem.setKickerPower(0.0); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 2268c93c..9757d7ef 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -152,8 +152,8 @@ public void handleState() { m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0; } case ANTI_DEFENSE -> { - m_desiredArmPoseDegs = 45.0; - m_desiredWristPoseDegs = 35.0; + m_desiredArmPoseDegs = 68.0; + m_desiredWristPoseDegs = 65.0; } case INTAKE -> { m_armVelocityMult = 1.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index ad713901..2cde3bc1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -170,7 +170,7 @@ public void setupLogging() { } public boolean atSpeed() { - return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75 - && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75; + return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 100 + && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 100; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index d2acceb2..faad2dd4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -33,11 +33,11 @@ public class VisionSubsystem { private AprilTagFieldLayout m_aprilTagFieldLayout; private final String m_name; - private final double xyStdDevCoefficient = Units.inchesToMeters(4.0); - private final double thetaStdDevCoefficient = Units.degreesToRadians(14.0); + private final double xyStdDevCoefficient = Units.inchesToMeters(3.5); + private final double thetaStdDevCoefficient = Units.degreesToRadians(12.0); - private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(2.0); - private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(7.0); + private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(1.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(5.0); private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index c2a8d5be..a13af2d5 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -47,7 +47,8 @@ public class AimbotUtils { public static double getWristAngle(double distance) { // return m_angleLerpTable.get(distance); // return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); - return 48.903-(0.09001 * distance); +// return 48.903-(0.09001 * distance); + return 50.218 - (0.1108 * distance); } public static double getLeftSpeed(double distance) {