diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..ac5f521 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..b04525a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.640294126157407, + "y": 4.038 + }, + "prevControl": null, + "nextControl": { + "x": 5.640294126157407, + "y": 4.038 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.916, + "y": 4.038 + }, + "prevControl": { + "x": 10.916, + "y": 4.038 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 3a841ee..62b5823 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -10,11 +10,14 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.math.util.Units; import static org.carlmontrobotics.Config.CONFIG; @@ -38,7 +41,8 @@ public static final class Driver { public static final int port = 0; public static final int slowDriveButton = Button.kLeftBumper.value; - public static final int resetFieldOrientationButton = Button.kRightBumper.value; + public static final int shootOnFlyButton = Button.kRightBumper.value; + public static final int resetFieldOrientationButton = 180; //down dpad public static final Axis RIGHT_TRIGGER_BUTTON = Axis.kRightTrigger; public static final Axis LEFT_TRIGGER_BUTTON = Axis.kLeftTrigger; @@ -268,5 +272,26 @@ public static class LimeLightc { public static final int[] sampleLL1_VALID_IDS = {1, 2, 12, 13}; public static final int[] sampleLL2_VALID_IDS = {1, 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; } + + public static class Shooterc { + public static final int masterID = 1; //TODO + public static final boolean masterInverted = false; //TODO + public static final int followerID = 2; //TODO + public static final double kP = 2; //TODO + public static final double kI = 0; //TODO + public static final double kD = 0; //TODO + public static final double gearReduction = 1; + public static final double launchAngleRad = Math.PI/4; //TODO figure out real value + public static final double wheelRadiusMeters = 0.3; //TODO figure out real value + public static final double passiveVelocity = 100; //TODO + public static final double thetaAlignP = 1; //TODO + public static final double toleranceRad = Math.PI/90; //Looks right + + //Poses + public static final Pose2d centerOfRedGoal2d = new Pose2d(); + public static final Pose2d centerOfBlueGoal2d = new Pose2d(); + public static final Pose3d centerOfRedGoal3d = new Pose3d(); + public static final Pose3d centerOfBlueGoal3d = new Pose3d(); + } } //#endregion \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b921a51..55b03d4 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -73,6 +73,7 @@ public class RobotContainer { public final Limelight limelight = new Limelight(); public final Drivetrain drivetrain = new Drivetrain(limelight); + public final Shooter shooter = new Shooter(); private SendableChooser autoChooser = new SendableChooser<>(); @@ -86,16 +87,12 @@ public RobotContainer() { SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putBoolean("AlignOverride", alignOverride); - SmartDashboard.putBoolean("AutoScoring", autoScoring); - SmartDashboard.putBoolean("AlignOverride", true); //#endregion setDefaultCommands(); setBindingsDriver(); setBindingsManipulator(); SmartDashboard.putBoolean("Baby Mode", Config.CONFIG.isBabyMode()); - // SmartDashboard.putData("Rotate Command",new RotateToTag(drivetrain, limelight)); SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); SmartDashboard.putString("Location", DriverStation.getLocation().toString()); SmartDashboard.putBoolean("Connected to FMS?", DriverStation.isFMSAttached()); @@ -105,7 +102,7 @@ public RobotContainer() { //#region ButtonBindings private void setBindingsDriver() { - new JoystickButton(driverController, Driver.resetFieldOrientationButton) + new POVButton(driverController, Driver.resetFieldOrientationButton) .onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); axisTrigger(driverController, Driver.RIGHT_TRIGGER_BUTTON, 0.2) .onTrue(new InstantCommand(()->drivetrain.setFieldOriented(false))) @@ -127,12 +124,13 @@ private void RegisterCustomAutos(){} private void setDefaultCommands() { drivetrain.setDefaultCommand(new TeleopDrive( drivetrain, + shooter, () -> ProcessedAxisValue(driverController, Axis.kLeftY),//.06 drift purple, .10 drift black () -> ProcessedAxisValue(driverController, Axis.kLeftX), () -> ProcessedAxisValue(driverController, Axis.kRightX), () -> driverController.getRawButton(OI.Driver.slowDriveButton), - manipulatorController, - () -> SmartDashboard.getBoolean("Baby Mode", Config.CONFIG.isBabyMode()) + () -> SmartDashboard.getBoolean("Baby Mode", Config.CONFIG.isBabyMode()), + () -> driverController.getRawButton(OI.Driver.shootOnFlyButton) )); } //#endregion diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt b/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt deleted file mode 100644 index 9d2a4c9..0000000 --- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt +++ /dev/null @@ -1,49 +0,0 @@ -// 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 org.carlmontrobotics.commands.DriveCommands; - -import org.carlmontrobotics.subsystems.Drivetrain; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; - -public class RotateToNearest60 extends Command { - - private final RotateToFieldRelativeAngle rotateCommand; - private final Drivetrain drivetrain; - - public RotateToNearest60(Drivetrain drivetrain) { - this.drivetrain = drivetrain; - Rotation2d targetAngle = getNearest60(drivetrain.getHeading()); - this.rotateCommand = new RotateToFieldRelativeAngle(targetAngle, drivetrain); - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - rotateCommand.initialize(); - } - - @Override - public void execute() { - rotateCommand.execute(); - } - - @Override - public boolean isFinished() { - return rotateCommand.isFinished(); - } - - @Override - public void end(boolean interrupted) { - rotateCommand.end(interrupted); - } - - private Rotation2d getNearest60(double currentAngle) { - double modAngle = (Math.round(currentAngle / 60.0) * 60) % 360; - if (modAngle > 180) modAngle -= 360; // Keep angle within -180 to 180 - return Rotation2d.fromDegrees(modAngle); - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java index 0386341..8e544e0 100644 --- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java @@ -1,24 +1,37 @@ package org.carlmontrobotics.commands.DriveCommands; import static org.carlmontrobotics.Constants.Drivetrainc.*; +import static org.carlmontrobotics.Constants.Shooterc.centerOfBlueGoal2d; +import static org.carlmontrobotics.Constants.Shooterc.centerOfRedGoal2d; +import static org.carlmontrobotics.Constants.Shooterc.passiveVelocity; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.carlmontrobotics.Constants; import org.carlmontrobotics.Robot; +import org.carlmontrobotics.commands.ShooterCommands.HeadingAlignController; +import org.carlmontrobotics.commands.ShooterCommands.HexClosest; +import org.carlmontrobotics.commands.ShooterCommands.ShotCalculator; import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Shooter; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class TeleopDrive extends Command { private static double robotPeriod = Robot.kDefaultPeriod; - private final Drivetrain drivetrain; + private final Drivetrain dt; + private final Shooter shooter; private DoubleSupplier fwd; private DoubleSupplier str; private DoubleSupplier rcw; @@ -26,21 +39,23 @@ public class TeleopDrive extends Command { private double currentForwardVel = 0; private double currentStrafeVel = 0; private double prevTimestamp; - GenericHID manipulatorController; - BooleanSupplier babyModeSupplier; + private BooleanSupplier babyModeSupplier; + private BooleanSupplier shootOnFly; + + private boolean isRed = false; /** * Creates a new TeleopDrive. */ - public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, - BooleanSupplier slow, GenericHID manipulatorController, BooleanSupplier babyModeSupplier) { - addRequirements(this.drivetrain = drivetrain); + public TeleopDrive(Drivetrain drivetrain, Shooter shooter, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow, BooleanSupplier babyModeSupplier, BooleanSupplier shootOnFly) { + addRequirements(this.dt = drivetrain, this.shooter = shooter); this.fwd = fwd; this.str = str; this.rcw = rcw; this.slow = slow; - this.manipulatorController = manipulatorController; this.babyModeSupplier = babyModeSupplier; + this.shootOnFly = shootOnFly; } // Called when the command is initially scheduled. @@ -51,6 +66,9 @@ public void initialize() { // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); prevTimestamp = Timer.getFPGATimestamp(); + isRed = DriverStation.getAlliance() + .map(a -> a == DriverStation.Alliance.Red) + .orElse(false); // default when unknown } // Called every time the scheduler runs while the command is scheduled. @@ -58,22 +76,32 @@ public void initialize() { public void execute() { double currentTime = Timer.getFPGATimestamp(); robotPeriod = currentTime - prevTimestamp; - if (!hasDriverInput()) { - drivetrain.drive(0,0,0); - } - else { double[] speeds = getRequestedSpeeds(); - // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); prevTimestamp = currentTime; - // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); - // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); - // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); - // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); - - // SmartDashboard.putNumber("fwd", speeds[0]); - // SmartDashboard.putNumber("strafe", speeds[1]); - // SmartDashboard.putNumber("turn", speeds[2]); - drivetrain.drive(speeds[0], speeds[1], speeds[2]); + if (shootOnFly.getAsBoolean()) { + Pose2d currentPose2d = dt.getDrivetrainPosition(); + double rotation = calculateRotationToAlign(currentPose2d); + boolean alignedForShot = calculateAlignmentValid(currentPose2d); + if (alignedForShot) { + ShotCalculator.ShotResult shotResult = calculateShotVelocity(); + if (shotResult.impossible) { + shooter.setRPMGoal(passiveVelocity); + dt.drive(speeds[0], speeds[1], rotation); + } + else { + double shotVelocity = shotResult.requiredRPM; + shooter.setRPMGoal(shotVelocity); + dt.drive(speeds[0], speeds[1], rotation); + } + } + else { + shooter.setRPMGoal(passiveVelocity); + dt.drive(speeds[0], speeds[1], rotation); + } + } + else { + shooter.stop(); + dt.drive(speeds[0], speeds[1], speeds[2]); } } @@ -85,49 +113,21 @@ public double[] getRequestedSpeeds() { double forward = fwd.getAsDouble(); double strafe = str.getAsDouble(); double rotateClockwise = rcw.getAsDouble(); - // SmartDashboard.putNumber("fwdIN", forward); - // SmartDashboard.putNumber("strafeIN", strafe); - // SmartDashboard.putNumber("turnIN", rotateClockwise); - // System.out.println("fwd str rcw: "+forward+", "+strafe+", "+rotateClockwise); - boolean slow2 = slow.getAsBoolean(); + //boolean slow2 = slow.getAsBoolean(); gets not used ig forward *= maxForward; strafe *= maxStrafe; rotateClockwise *= maxRCW; - - // System.out.println("teleopDrive ExtraSpeedMult%: "+drivetrain.extraSpeedMult); double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); - double rotationMultiplier = drivetrain.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); - // double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); - // double rotationMultiplier = (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); - + double rotationMultiplier = dt.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); if(babyModeSupplier.getAsBoolean()){ driveMultiplier = kBabyDriveSpeed; rotationMultiplier = kBabyDriveRotation; } - // double driveMultiplier = kNormalDriveSpeed; - // double rotationMultiplier = kNormalDriveRotation; - forward *= driveMultiplier; strafe *= driveMultiplier; rotateClockwise *= rotationMultiplier; - - // Limit acceleration of the robot - // double accelerationX = (forward - currentForwardVel) / robotPeriod; - // double accelerationY = (strafe - currentStrafeVel) / robotPeriod; - // double translationalAcceleration = Math.hypot(accelerationX, accelerationY); - // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration); - // if (translationalAcceleration > autoMaxAccelMps2 && false) {//DOES NOT RUN!! - // Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, - // Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); - // Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); - // currentForwardVel += limitedVelocityVector.getX(); - // currentStrafeVel += limitedVelocityVector.getY(); - // } else { currentForwardVel = forward; currentStrafeVel = strafe; - // } - // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel)); - return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; } @@ -137,6 +137,42 @@ public boolean hasDriverInput() { || MathUtil.applyDeadband(rcw.getAsDouble(), Constants.OI.JOY_THRESH)!=0; } + + private double calculateRotationToAlign(Pose2d currentPose2d) { + if (isRed) { + return HeadingAlignController.calculateOmega(currentPose2d, centerOfRedGoal2d); + } + else { + return HeadingAlignController.calculateOmega(currentPose2d, centerOfBlueGoal2d); + } + } + + private boolean calculateAlignmentValid(Pose2d currentPose2d) { + if (isRed) { + return HeadingAlignController.atGoal(currentPose2d, centerOfRedGoal2d); + } + else { + return HeadingAlignController.atGoal(currentPose2d, centerOfBlueGoal2d); + } + } + + private ShotCalculator.ShotResult calculateShotVelocity() { + Pose3d goalPose = isRed ? new Pose3d(11.916, 4.038, 1.8237877672, new Rotation3d()) : new Pose3d(4.618, 4.038, 1.8237877672, new Rotation3d()); + Pose2d current2dPose = dt.getDrivetrainPosition(); + Pose3d current3dPose = new Pose3d(current2dPose); + double[] velocityVectors = dt.getDrivetrainVelocity(); + double shooterVelocity = shooter.getVelocity(); + velocityVectors[0] += shooterVelocity * Math.cos(current2dPose.getRotation().getRadians()); + velocityVectors[1] += shooterVelocity * Math.sin(current2dPose.getRotation().getRadians()); + Translation3d path = goalPose.getTranslation().minus(current3dPose.getTranslation()); + Translation2d closestObstacle = HexClosest.closestVectorToHex(current2dPose, isRed); + double[] obstacleDistances = {closestObstacle.getX(), closestObstacle.getY()}; + double[] obstacleHeights = {Units.inchesToMeters(72), Units.inchesToMeters(72)}; + ShotCalculator.ShotResult speed = ShotCalculator.calculateShot(path.getX(), path.getY(), path.getZ(), velocityVectors[0], velocityVectors[1], obstacleDistances, obstacleHeights); + + return speed; + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java new file mode 100644 index 0000000..e10b20a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java @@ -0,0 +1,53 @@ +package org.carlmontrobotics.commands.ShooterCommands; + +import static org.carlmontrobotics.Constants.Shooterc.thetaAlignP; +import static org.carlmontrobotics.Constants.Shooterc.toleranceRad; + +import edu.wpi.first.math.geometry.Pose2d; + +public class HeadingAlignController { + + /** + * Returns rotational speed (rad/s scaled by kP) needed to face target. + * + * @param currentPose robot pose (with rotation) + * @param targetPose pose representing the point to face (rotation ignored) + */ + public static double calculateOmega(Pose2d currentPose, Pose2d targetPose) { + double dx = targetPose.getX() - currentPose.getX(); + double dy = targetPose.getY() - currentPose.getY(); + + double desiredAngle = Math.atan2(dy, dx); + double currentAngle = currentPose.getRotation().getRadians(); + + double error = wrapAngle(desiredAngle - currentAngle); + + return thetaAlignP * error; + } + + /** + * Returns true if robot is aligned within tolerance. + */ + public static boolean atGoal(Pose2d currentPose, Pose2d targetPose) { + double dx = targetPose.getX() - currentPose.getX(); + double dy = targetPose.getY() - currentPose.getY(); + + double desiredAngle = Math.atan2(dy, dx); + double currentAngle = currentPose.getRotation().getRadians(); + + double error = wrapAngle(desiredAngle - currentAngle); + + return Math.abs(error) <= toleranceRad; + } + + /** + * Wrap angle to [-pi, pi] + */ + private static double wrapAngle(double angle) { + angle %= Math.PI; + if (angle > Math.PI) { + angle -= 2.0 * Math.PI; + } + return angle; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java new file mode 100644 index 0000000..b98e0e3 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java @@ -0,0 +1,75 @@ +package org.carlmontrobotics.commands.ShooterCommands; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; + +public class HexClosest { + private static final Pose2d[] hexVerticesBlue = { + new Pose2d(Units.inchesToMeters(155.491863), Units.inchesToMeters(159.249827), new Rotation2d()), + new Pose2d(Units.inchesToMeters(169.870573), Units.inchesToMeters(180.359655), new Rotation2d()), + new Pose2d(Units.inchesToMeters(193.838106), Units.inchesToMeters(180.228677), new Rotation2d()), + new Pose2d(Units.inchesToMeters(205.837629), Units.inchesToMeters(159.444893), new Rotation2d()), + new Pose2d(Units.inchesToMeters(193.967292), Units.inchesToMeters(138.622912), new Rotation2d()), + new Pose2d(Units.inchesToMeters(169.661894), Units.inchesToMeters(138.558823), new Rotation2d())}; + private static final Pose2d[] hexVerticesRed = { + new Pose2d(Units.inchesToMeters(443.907204), Units.inchesToMeters(159.093750), new Rotation2d()), + new Pose2d(Units.inchesToMeters(455.906727), Units.inchesToMeters(179.979820), new Rotation2d()), + new Pose2d(Units.inchesToMeters(480.123542), Units.inchesToMeters(179.864588), new Rotation2d()), + new Pose2d(Units.inchesToMeters(491.993879), Units.inchesToMeters(159.042607), new Rotation2d()), + new Pose2d(Units.inchesToMeters(480.123542), Units.inchesToMeters(138.322912), new Rotation2d()), + new Pose2d(Units.inchesToMeters(456.026823), Units.inchesToMeters(138.230132), new Rotation2d())}; + /** + * Returns vector (dx, dy) from robot to closest point on hex boundary. + * @param robot provides position of robot and direction + * @param isRed is a boolean determining which hub is the user going for + */ + public static Translation2d closestVectorToHex(Pose2d robot, boolean isRed) { + Pose2d[] hexVertices; + if (isRed) { + hexVertices = hexVerticesRed; + } + else { + hexVertices = hexVerticesBlue; + } + Translation2d p = robot.getTranslation(); + + Translation2d bestPoint = null; + double bestDistSq = Double.POSITIVE_INFINITY; + + for (int i = 0; i < 6; i++) { + Translation2d a = hexVertices[i].getTranslation(); + Translation2d b = hexVertices[(i + 1) % 6].getTranslation(); + + // ---- closest point on segment a→b ---- + Translation2d ab = b.minus(a); + Translation2d ap = p.minus(a); + + double abLenSq = ab.getX()*ab.getX() + ab.getY()*ab.getY(); + Translation2d candidate; + + if (abLenSq == 0) { + candidate = a; + } else { + double t = (ap.getX()*ab.getX() + ap.getY()*ab.getY()) / abLenSq; + t = Math.max(0, Math.min(1, t)); + + candidate = new Translation2d( + a.getX() + ab.getX() * t, + a.getY() + ab.getY() * t + ); + } + // -------------------------------------- + + double dx = candidate.getX() - p.getX(); + double dy = candidate.getY() - p.getY(); + double distSq = dx*dx + dy*dy; + + if (distSq < bestDistSq) { + bestDistSq = distSq; + bestPoint = candidate; + } + } + + return bestPoint.minus(p); // (x, y) components to hex + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java new file mode 100644 index 0000000..a4d9d92 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java @@ -0,0 +1,67 @@ +// 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 org.carlmontrobotics.commands.ShooterCommands; + +import java.nio.channels.Pipe; + +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; + +/* 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 ShootOnFly extends Command { + private Drivetrain dt; + private Shooter shooter; + private boolean isRed; + private Pose2d current2dPose; + private Pose3d current3dPose; + private Pose3d goalPose; + private double[] velocityVectors; + private double shooterVelocity; + private Translation3d path; + + /** Creates a new ShootOnFly. */ + public ShootOnFly(Drivetrain dt, Shooter shooter) { + addRequirements(this.dt = dt, this.shooter = shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + isRed = DriverStation.getAlliance() + .map(a -> a == DriverStation.Alliance.Red) + .orElse(false); // default when unknown + goalPose = isRed ? new Pose3d(11.916, 4.038, 1.8237877672, new Rotation3d()) : new Pose3d(4.618, 4.038, 1.8237877672, new Rotation3d()); + current2dPose = dt.getDrivetrainPosition(); + current3dPose = new Pose3d(current2dPose); + velocityVectors = dt.getDrivetrainVelocity(); + shooterVelocity = shooter.getVelocity(); + velocityVectors[0] += shooterVelocity*Math.cos(current2dPose.getRotation().getRadians()); + velocityVectors[1] += shooterVelocity*Math.sin(current2dPose.getRotation().getRadians()); + path = goalPose.getTranslation().minus(current3dPose.getTranslation()); + + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java new file mode 100644 index 0000000..200a499 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java @@ -0,0 +1,102 @@ +package org.carlmontrobotics.commands.ShooterCommands; + +import static org.carlmontrobotics.Constants.Shooterc.*; + +public class ShotCalculator { + private static double clearanceSafety = 0.1651; // extra margin (m) + + public static class ShotResult { + public double requiredRPM; + public boolean impossible; + public boolean nearImpossible; + public boolean descendingAtGoal; + public boolean clearanceOK; + public double[] clearanceMargins; // meters above each obstacle + } + + public static ShotResult calculateShot( + double dx, double dy, double dz, // displacement to goal (m) + double vxRobot, double vyRobot, // robot velocity field-relative (m/s) + double[] obstacleDistances, // horizontal distance to each obstacle + double[] obstacleHeights // required height at each obstacle + ) { + + ShotResult result = new ShotResult(); + double g = 9.81; + + double x = Math.hypot(dx, dy); + double cos = Math.cos(launchAngleRad); + double tan = Math.tan(launchAngleRad); + + double margin = x * tan - dz; + + // 1. impossible check + if (margin <= 0.0) { + result.impossible = true; + result.requiredRPM = Double.POSITIVE_INFINITY; + return result; + } + + // 2. field-relative projectile speed + double vField = Math.sqrt( + (g * x * x) / + (2.0 * cos * cos * margin) + ); + + // 3. compensate robot motion + double dirX = dx / x; + double dirY = dy / x; + + double vxField = vField * dirX; + double vyField = vField * dirY; + + double vxExit = vxField - vxRobot; + double vyExit = vyField - vyRobot; + + double exitSpeed = Math.hypot(vxExit, vyExit); + + // 4. convert to RPM + double wheelRadPerSec = exitSpeed / wheelRadiusMeters; + result.requiredRPM = wheelRadPerSec * 60.0 / (2.0 * Math.PI); + + // 5. near-impossible check + result.nearImpossible = margin < 0.20; + + // 6. descending check + double vzFinal = + vField * Math.sin(launchAngleRad) + - g * x / (vField * cos); + + result.descendingAtGoal = vzFinal < 0.0; + + // 7. clearance checks + if (obstacleDistances != null && obstacleHeights != null) { + + int n = obstacleDistances.length; + result.clearanceMargins = new double[n]; + result.clearanceOK = true; + + for (int i = 0; i < n; i++) { + + double s = obstacleDistances[i]; + + double z = + s * Math.tan(launchAngleRad) + - (g * s * s) / + (2.0 * vField * vField * cos * cos); + + double marginClear = z - obstacleHeights[i]; + result.clearanceMargins[i] = marginClear; + + if (marginClear < clearanceSafety) { + result.clearanceOK = false; + } + } + } else { + result.clearanceOK = true; + result.clearanceMargins = new double[0]; + } + + return result; + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 404994a..dbe0ca2 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -1,9 +1,40 @@ package org.carlmontrobotics.subsystems; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Volts; +import static org.carlmontrobotics.Config.CONFIG; +import static org.carlmontrobotics.Constants.Drivetrainc.COLLISION_ACCELERATION_THRESHOLD; +import static org.carlmontrobotics.Constants.Drivetrainc.driveBackLeftPort; +import static org.carlmontrobotics.Constants.Drivetrainc.driveBackRightPort; +import static org.carlmontrobotics.Constants.Drivetrainc.driveFrontLeftPort; +import static org.carlmontrobotics.Constants.Drivetrainc.driveFrontRightPort; +import static org.carlmontrobotics.Constants.Drivetrainc.driveGearing; +import static org.carlmontrobotics.Constants.Drivetrainc.isGyroReversed; +import static org.carlmontrobotics.Constants.Drivetrainc.maxSpeed; +import static org.carlmontrobotics.Constants.Drivetrainc.secsPer12Volts; +import static org.carlmontrobotics.Constants.Drivetrainc.trackWidth; +import static org.carlmontrobotics.Constants.Drivetrainc.turnBackLeftPort; +import static org.carlmontrobotics.Constants.Drivetrainc.turnBackRightPort; +import static org.carlmontrobotics.Constants.Drivetrainc.turnFrontLeftPort; +import static org.carlmontrobotics.Constants.Drivetrainc.turnFrontRightPort; +import static org.carlmontrobotics.Constants.Drivetrainc.turnGearing; +import static org.carlmontrobotics.Constants.Drivetrainc.wheelBase; +import static org.carlmontrobotics.Constants.Drivetrainc.wheelDiameterMeters; + import java.util.Arrays; import java.util.Map; import java.util.function.Supplier; +//Constants +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; +import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; //lib199 import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; @@ -11,22 +42,52 @@ import org.carlmontrobotics.lib199.swerve.SwerveModule; import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; -import static org.carlmontrobotics.Config.CONFIG; +import com.ctre.phoenix6.hardware.CANcoder; +//pathplanner +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.hal.SimDouble; - +//math +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; - //wpilib import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -37,85 +98,6 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -//math -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -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.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.controller.PIDController; - -//vendordeps -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.hardware.CANcoder; -import com.studica.frc.AHRS; -import com.studica.frc.AHRS.NavXComType; - -//pathplanner -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; - -//rev -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -//units -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.MutAngle; -import edu.wpi.first.units.measure.MutAngularVelocity; -import edu.wpi.first.units.measure.MutDistance; -import edu.wpi.first.units.measure.MutLinearVelocity; -import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.units.measure.Voltage; - -import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volt; -import static edu.wpi.first.units.Units.Meter; -import static edu.wpi.first.units.Units.Meters; - -//Constants -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.Constants.Drivetrainc; -import org.carlmontrobotics.Constants.Drivetrainc.Autoc; -import org.carlmontrobotics.Robot; -import org.carlmontrobotics.subsystems.Limelight; -import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; -import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; -import static org.carlmontrobotics.Constants.Drivetrainc.*; -import static org.carlmontrobotics.Constants.LimeLightc.*; - public class Drivetrain extends SubsystemBase { private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); @@ -415,123 +397,17 @@ public void setDrivingIdleMode(boolean brake) { @Override public void periodic() { - detectCollision(); //This does nothing PathPlannerLogging.logCurrentPose(getPose()); - - //maybe add the field with the position of the robot with only limelight and the field with the position of the robot with only odometry? - //We can compare the two fields to see if odometry is causing the pose to be inaccurate when it hits the reef. - - // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); - // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); - // double goal = SmartDashboard.getNumber("GoalPos", 0); - // PIDController pid = new PIDController(kP, kI, kD); - // kP = SmartDashboard.getNumber("kP", 0); - // kI = SmartDashboard.getNumber("kI", 0); - // kD = SmartDashboard.getNumber("kD", 0); - //pid.setIZone(20); - //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); - // SparkMaxConfig config = new SparkMaxConfig(); - - //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); - // System.out.println(kP); - // config.closedLoop.pid(kP ,kI,kD); - // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); - // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - // //moduleFL.move(0.0000001, 180); - //moduleFL.move(0.01, 180); - // moduleFR.move(0.000000001, 0); - // moduleBR.move(0.0000001, 0); - // moduleFL.move(0.000001, 0); - // moduleBL.move(0.000001, 0); - // turnPidControllers[0].setReference(goal - - // , ControlType.kPosition, ClosedLoopSlot.kSlot0); - - - // 167 -> -200 - // 138 -> 360 - // for (CANcoder coder : turnEncoders) { - // SignalLogger.writeDouble("Regular position " + coder.toString(), - // coder.getPosition().getValue().baseUnitMagnitude()); - // SignalLogger.writeDouble("Velocity " + coder.toString(), - // coder.getVelocity().getValue().baseUnitMagnitude()); - // SignalLogger.writeDouble("Absolute position " + coder.toString(), - // coder.getAbsolutePosition().getValue().baseUnitMagnitude()); - // } - // String out=""; int i=0; - // for (CANcoder coder : turnEncoders) { - // out+=String.format("[i] Abs Pos: %.3f Goal Pos: %.3f ", coder.getAbsolutePosition().getValue().baseUnitMagnitude(),0); - // i++; - // } - // lobotomized to prevent ucontrollabe swerve behavior - // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); - // moduleFL.periodic(); - // moduleFR.periodic(); - // moduleBL.periodic(); - // moduleBR.periodic(); - double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { // module.turnPeriodic(); // module.turnPeriodic(); - module.move(0.00000000001, goal); module.periodic(); } - - // field.setRobotPose(odometry.getPoseMeters()); - - - - // odometry.update(gyro.getRotation2d(), getModulePositions()); - - // poseEstimator.update(gyro.getRotation2d(), getModulePositions()); - - //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); - - // updateMT2PoseEstimator(); - - // double currSetX = - // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); - // double currSetY = - // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); - // double currSetTheta = SmartDashboard - // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); - - // if (lastSetX != currSetX || lastSetY != currSetY - // || lastSetTheta != currSetTheta) { - // setPose(new Pose2d(currSetX, currSetY, - // Rotation2d.fromDegrees(currSetTheta))); - // } - - // setPose(new Pose2d(getPose().getTranslation().getX(), - // getPose().getTranslation().getY(), - // Rotation2d.fromDegrees(getHeading()))); - - - // SmartDashboard.putNumber("X position with limelight", getPoseWithLimelight().getX()); - // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY()); - SmartDashboard.putNumber("X position with gyro", getPose().getX()); - SmartDashboard.putNumber("Y position with gyro", getPose().getY()); - SmartDashboard.putData(CONFIG); - - //For finding acceleration of drivetrain for collision detector - SmartDashboard.putNumber("Accel X", accelX); - SmartDashboard.putNumber("Accel Y", accelY); - SmartDashboard.putNumber("2D Acceleration ", accelXY); - - // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); - // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); - // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); - // SmartDashboard.putNumber("Robot Heading", getHeading()); - // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); - // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); - // SmartDashboard.putBoolean("Field Oriented", fieldOriented); - // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); - // SmartDashboard.putNumber("Compass Offset", compassOffset); - // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + updateVelocity(); } @Override @@ -1328,4 +1204,39 @@ public void keepRotateMotorsAtDegrees(int angle) { public double getGyroRate() { return gyro.getRate(); } + + public Pose2d getDrivetrainPosition() { + return poseEstimator.getEstimatedPosition(); + } + + public double[] getDrivetrainVelocity() { + double [] info = {drivetrainVX, drivetrainVY, drivetrainVR}; + return info; + } + + Pose2d lastPose; + double lastTime; + /** X direction velocity: m/s*/ + private double drivetrainVX; + /** Y direction velocity: m/s */ + private double drivetrainVY; + /** Rotation velocity: rad/s */ + private double drivetrainVR; + + private void updateVelocity() { + Pose2d currentPose = poseEstimator.getEstimatedPosition(); + double now = Timer.getFPGATimestamp(); + + double dt = now - lastTime; + Translation2d delta = currentPose.getTranslation().minus(lastPose.getTranslation()); + + drivetrainVX = delta.getX() / dt; + drivetrainVY = delta.getY() / dt; + + drivetrainVR = + currentPose.getRotation().minus(lastPose.getRotation()).getRadians() / dt; + + lastPose = currentPose; + lastTime = now; + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Indexer.java b/src/main/java/org/carlmontrobotics/subsystems/Indexer.java new file mode 100644 index 0000000..7d1b4f6 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Indexer.java @@ -0,0 +1,17 @@ +// 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 org.carlmontrobotics.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Indexer extends SubsystemBase { + /** Creates a new Indexer. */ + public Indexer() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Shooter.java b/src/main/java/org/carlmontrobotics/subsystems/Shooter.java new file mode 100644 index 0000000..52b28f6 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Shooter.java @@ -0,0 +1,93 @@ +// 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 org.carlmontrobotics.subsystems; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static org.carlmontrobotics.Constants.Shooterc.*; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.MotorControllerType; + +public class Shooter extends SubsystemBase { + /** Creates a new Shooter. */ + private SparkFlex masterShooter; + private SparkFlex followShooter; + private RelativeEncoder masterEncoder; + private RelativeEncoder followEncoder; + private SparkBaseConfig masterConfig; + private SparkBaseConfig followConfig; + private SparkClosedLoopController pidController; + private double shooterGoal; + private boolean manualMode = false; + + public Shooter() { + configureMotors(); + masterShooter = MotorControllerFactory.createSparkFlex(masterID, MotorConfig.NEO_VORTEX, masterConfig); + followShooter = MotorControllerFactory.createSparkFlex(followerID, MotorConfig.NEO_VORTEX, followConfig); + } + + private void configureMotors() { + masterConfig = MotorControllerFactory.createConfig(MotorControllerType.SPARK_FLEX); + masterConfig.idleMode(IdleMode.kCoast) + .encoder + .positionConversionFactor(gearReduction) //rotations + .velocityConversionFactor(gearReduction); //rotations per minute + + followConfig.apply(masterConfig); + followConfig.follow(masterID, true); + + masterConfig.inverted(masterInverted) + .closedLoop.pid(kP, kI, kD); + } + + public void setVoltage(double voltage) { + pidController.setSetpoint(voltage, ControlType.kVoltage); + } + + public void setVoltagePercentage(double percentage) { + pidController.setSetpoint(percentage, ControlType.kDutyCycle); + } + + public void setRPMGoal(double goal) { + shooterGoal = goal; + pidController.setSetpoint(goal, ControlType.kVelocity); + } + + public void stop() { + pidController.setSetpoint(0, ControlType.kDutyCycle); + } + + public void turnOnManualMode() { + if (!manualMode) { + manualMode = true; + pidController.setSetpoint(0, ControlType.kDutyCycle); + } + } + + public double getRPMGoal() { + return shooterGoal; + } + + /**Gets RPM */ + public double getVelocity() { + return masterEncoder.getVelocity(); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}