diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6d3aea16..543f4f66 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,7 @@ // GNU General Public License for more details. package frc.robot; +import com.fasterxml.jackson.core.SerializableString; import edu.wpi.first.math.geometry.*; import com.gos.lib.properties.GosDoubleProperty; import com.pathplanner.lib.path.PathConstraints; @@ -275,6 +276,9 @@ private ArmSetpoints() { } public static class ShooterConstants { + public static final GosDoubleProperty ACCEL_COMP_FACTOR = + new GosDoubleProperty(false, "Shooter/Acceleration Compensation", 0.100); + private ShooterConstants() { throw new IllegalStateException("Static classes should not be constructed"); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc1dda4e..22023469 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -185,23 +185,26 @@ private void configureButtonBindings() { intakeTrigger.whileTrue(m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); - spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) - .alongWith(DriveCommands.alignmentDrive( - m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) - ))); +// spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) +// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) +// .alongWith(DriveCommands.alignmentDrive( +// m_driveSubsystem, +// () -> -controller.getLeftY(), +// () -> -controller.getLeftX(), +// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) +// ))); + spinUpTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), false)); - shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) - .alongWith(DriveCommands.alignmentDrive( - m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) - ))); +// shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) +// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) +// .alongWith(DriveCommands.alignmentDrive( +// m_driveSubsystem, +// () -> -controller.getLeftY(), +// () -> -controller.getLeftX(), +// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) +// ))); + + shootTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), true)); ampLineupTrigger.whileTrue(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP) .finallyDo(() -> m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP).schedule())) diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index e5685f40..3b586bc5 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -4,17 +4,22 @@ import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.arm.ArmSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; import lib.utils.AimbotUtils; +import lib.utils.FieldConstants; +import lib.utils.FieldRelativeAccel; +import lib.utils.FieldRelativeSpeed; public class AimbotCommand extends Command { @@ -72,8 +77,35 @@ public void execute() { m_smallProperty.updateIfChanged(); m_fastProperty.updateIfChanged(); + // get the robots velocity and acceleration + FieldRelativeSpeed fieldRelativeSpeed = m_driveSubsystem.getFieldRelativeVelocity(); + FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); + + // TODO make an actual equation for shot time based on distance + double shotTime = 0.5; + + Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d(); + Translation2d movingTarget = new Translation2d(); + + // loop over movement calcs to better adjust for acceleration + if (false) { + for (int i = 0; i < 5; i++) { + double virtualGoalX = target.getX() + - shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + double virtualGoalY = target.getY() + - shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + + movingTarget = new Translation2d(virtualGoalX, virtualGoalY); + } + } else { + movingTarget = target; + } + // get our desired rotation and error from it - double desiredRotationDegs = AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()).getDegrees(); + double desiredRotationDegs = + AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), + new Translation3d(movingTarget.getX(), movingTarget.getY(), 0.0)) + .getDegrees(); double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); // if we're far from our setpoint, move faster diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 19bf6a66..0153f428 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -52,10 +52,7 @@ import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; -import lib.utils.AimbotUtils; -import lib.utils.AllianceFlipUtil; -import lib.utils.LocalADStarAK; -import lib.utils.PoseEstimator; +import lib.utils.*; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -89,7 +86,6 @@ public class DriveSubsystem extends SubsystemBase { new Rotation2d(), new Rotation2d() }; - private Rotation2d m_avgRotationRads = new Rotation2d(); private final VisionSubsystem[] m_cameras; @@ -105,6 +101,9 @@ public class DriveSubsystem extends SubsystemBase { new SwerveModuleState() }; + private FieldRelativeSpeed m_fieldRelVel = new FieldRelativeSpeed(); + private FieldRelativeSpeed m_lastFieldRelVel = new FieldRelativeSpeed(); + private FieldRelativeAccel m_fieldRelAccel = new FieldRelativeAccel(); public DriveSubsystem( GyroIO gyroIO, @@ -246,24 +245,6 @@ public void periodic() { m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } - // Update average heading, used for aiming - m_prevRotations = new Rotation2d[] { - m_rawGyroRotation, - m_prevRotations[1], - m_prevRotations[2], - m_prevRotations[3], - m_prevRotations[4] - }; - - double totalRotationRads = 0.0; - - for (Rotation2d mPrevRotation : m_prevRotations) { - totalRotationRads += mPrevRotation.getRadians(); - } - - totalRotationRads /= m_prevRotations.length; - m_avgRotationRads = new Rotation2d(totalRotationRads); - // Apply update m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions); } @@ -291,6 +272,11 @@ public void periodic() { -AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees()); m_field.setRobotPose(getVisionPose()); + + // calculate field relative velocities and acceleration + m_fieldRelVel = new FieldRelativeSpeed(kinematics.toChassisSpeeds(getModuleStates()), getRotation()); + m_fieldRelAccel = new FieldRelativeAccel(m_fieldRelVel, m_lastFieldRelVel, 0.02); + m_lastFieldRelVel = m_fieldRelVel; } /** @@ -386,6 +372,16 @@ public Rotation2d getRotation() { return m_wpiPoseEstimator.getEstimatedPosition().getRotation(); } + /** Returns the robot's field relative velocity, used for shoot on move */ + public FieldRelativeSpeed getFieldRelativeVelocity() { + return m_fieldRelVel; + } + + /** Returns the robot's field relative acceleration, used for shoot on move */ + public FieldRelativeAccel getFieldRelativeAcceleration() { + return m_fieldRelAccel; + } + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { gyroIO.resetGyro(pose.getRotation().getDegrees()); diff --git a/src/main/java/lib/utils/FieldRelativeAccel.java b/src/main/java/lib/utils/FieldRelativeAccel.java new file mode 100644 index 00000000..ff23b26e --- /dev/null +++ b/src/main/java/lib/utils/FieldRelativeAccel.java @@ -0,0 +1,35 @@ +package lib.utils; + +public class FieldRelativeAccel { + public double ax; + public double ay; + public double alpha; + + public FieldRelativeAccel(double ax, double ay, double alpha) { + this.ax = ax; + this.ay = ay; + this.alpha = alpha; + } + + public FieldRelativeAccel(FieldRelativeSpeed newSpeed, FieldRelativeSpeed oldSpeed, double time) { + this.ax = (newSpeed.vx - oldSpeed.vx) / time; + this.ay = (newSpeed.vy - oldSpeed.vy) / time; + this.alpha = (newSpeed.omega - oldSpeed.omega) / time; + + if (Math.abs(this.ax) > 6.0) { + this.ax = 6.0 * Math.signum(this.ax); + } + if (Math.abs(this.ay) > 6.0) { + this.ay = 6.0 * Math.signum(this.ay); + } + if (Math.abs(this.alpha) > 4 * Math.PI) { + this.alpha = 4 * Math.PI * Math.signum(this.alpha); + } + } + + public FieldRelativeAccel() { + this.ax = 0.0; + this.ay = 0.0; + this.alpha = 0.0; + } +} diff --git a/src/main/java/lib/utils/FieldRelativeSpeed.java b/src/main/java/lib/utils/FieldRelativeSpeed.java new file mode 100644 index 00000000..4a2a46da --- /dev/null +++ b/src/main/java/lib/utils/FieldRelativeSpeed.java @@ -0,0 +1,28 @@ +package lib.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class FieldRelativeSpeed { + public double vx; + public double vy; + public double omega; + + public FieldRelativeSpeed(double vx, double vy, double omega) { + this.vx = vx; + this.vy = vy; + this.omega = omega; + } + + public FieldRelativeSpeed(ChassisSpeeds chassisSpeed, Rotation2d gyro) { + this(chassisSpeed.vxMetersPerSecond * gyro.getCos() - chassisSpeed.vyMetersPerSecond * gyro.getSin(), + chassisSpeed.vyMetersPerSecond * gyro.getCos() + chassisSpeed.vxMetersPerSecond * gyro.getSin(), + chassisSpeed.omegaRadiansPerSecond); + } + + public FieldRelativeSpeed() { + this.vx = 0.0; + this.vy = 0.0; + this.omega = 0.0; + } +}