diff --git a/2023-Robot/.Glass/glass.json b/2023-Robot/.Glass/glass.json index c8922c96..5b51c74e 100644 --- a/2023-Robot/.Glass/glass.json +++ b/2023-Robot/.Glass/glass.json @@ -31,7 +31,7 @@ "autoFit": true } ], - "height": 338, + "height": 0, "series": [ { "color": [ @@ -49,7 +49,7 @@ "Plot <1>": { "plots": [ { - "height": 338, + "height": 0, "series": [ { "color": [ diff --git a/2023-Robot/build.gradle b/2023-Robot/build.gradle index a2ad8db2..49a88d6e 100644 --- a/2023-Robot/build.gradle +++ b/2023-Robot/build.gradle @@ -3,8 +3,8 @@ plugins { id "edu.wpi.first.GradleRIO" version "2023.4.3" } -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 +sourceCompatibility = JavaVersion.VERSION_17 +targetCompatibility = JavaVersion.VERSION_17 def ROBOT_MAIN_CLASS = "frc.team670.robot.Main" diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index a09714c4..bbd88266 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit a09714c4230c2f4a9046214a77cde628a32d6864 +Subproject commit bbd8826628a92d7bbba55e12f69bef337901376f diff --git a/2023-Robot/src/main/java/frc/team670/robot/Robot.java b/2023-Robot/src/main/java/frc/team670/robot/Robot.java index d7a75e4f..5c5706b2 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/Robot.java +++ b/2023-Robot/src/main/java/frc/team670/robot/Robot.java @@ -19,6 +19,5 @@ public class Robot extends RobotBase { public Robot() { super(new RobotContainer()); - } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java index 24b52a30..a704f9b6 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -7,38 +7,25 @@ package frc.team670.robot; -import java.lang.reflect.Field; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.VideoMode.PixelFormat; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.RobotContainerBase; import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; -import frc.team670.mustanglib.utils.LEDColor; -import frc.team670.mustanglib.utils.Logger; import frc.team670.mustanglib.utils.MustangController; -import frc.team670.robot.commands.drivebase.NonPidAutoLevel; -import frc.team670.robot.commands.drivebase.SwerveDriveParkCommand; -import frc.team670.robot.commands.pathplanner.AutonCalibration; -import frc.team670.robot.commands.pathplanner.CenterEngage; +import frc.team670.robot.commands.arm.ResetArmFromAbsolute; import frc.team670.robot.commands.pathplanner.CenterEngageSequential; import frc.team670.robot.commands.pathplanner.CenterIntake; import frc.team670.robot.commands.pathplanner.ConeCube; import frc.team670.robot.commands.pathplanner.ConeCubeCube; -import frc.team670.robot.commands.pathplanner.ConeCubeEngage; import frc.team670.robot.commands.pathplanner.CubeEngage; import frc.team670.robot.commands.pathplanner.ScoreEngage; import frc.team670.robot.commands.pathplanner.ScoreMid; -import frc.team670.robot.constants.FieldConstants; import frc.team670.robot.constants.OI; -import frc.team670.robot.constants.RobotMap; +import frc.team670.robot.constants.RobotConstants; import frc.team670.robot.subsystems.Claw; import frc.team670.robot.subsystems.DriveBase; import frc.team670.robot.subsystems.LED; @@ -52,41 +39,42 @@ */ public class RobotContainer extends RobotContainerBase { - - private final PowerDistribution pd = new PowerDistribution(1, ModuleType.kCTRE); - - private final Vision vision = new Vision(pd); - private final DriveBase driveBase = new DriveBase(getDriverController()); - private final Arm arm = new Arm(); - private final LED led = new LED(RobotMap.LED_PORT, 0, 61); - private final Claw claw = new Claw(led); + private final Vision mVision = Vision.getInstance(); + private final DriveBase mDriveBase = DriveBase.getInstance(); + private final LED mLed = LED.getInstance(); + private final Arm mArm = Arm.getInstance(); + private final Claw mClaw = Claw.getInstance(); private MustangCommand cableScore, cableEngage, stationScore, stationEngage, centerEngage, centerIntake, scoreMid; - private static OI oi = new OI(); private Notifier updateArbitraryFeedForward; - private final String matchStarted = "match-started"; - private final String autonChooser = "auton-chooser"; + private final String kMatchStartedString = "match-started"; + private final String kAutonChooserString = "auton-chooser"; public RobotContainer() { super(); - addSubsystem(driveBase, vision, arm, arm.getShoulder(), arm.getElbow(), arm.getWrist(), - claw, led); - oi.configureButtonBindings(driveBase, vision, arm, claw, led); + addSubsystem(mDriveBase); + OI.configureButtonBindings(); + + // SmartDashboard.putNumber("fr", RobotConstants.robotSpecificConstants + // .get("kFrontLeftModuleSteerOffsetRadians")); + // SmartDashboard.putNumber("fl", RobotConstants.robotSpecificConstants.get( + // "kFrontLeftModuleSteerOffsetRadians")); + for (MustangSubsystemBase subsystem : getSubsystems()) { subsystem.setDebugSubsystem(true); } - cableScore = new ConeCube(driveBase, claw, arm, "CableScoreShort"); - stationScore = new ConeCubeCube(driveBase, claw, arm, "Station3Piece"); - cableEngage = new CubeEngage(driveBase, claw, arm, "CableEngage"); - stationEngage = new ScoreEngage(driveBase, claw, arm, "StationScoreEngage3"); - centerEngage = new CenterEngageSequential(driveBase, claw, arm); - centerIntake = new CenterIntake(driveBase, claw, arm, "CenterIntake"); - scoreMid = new ScoreMid(driveBase, claw, arm); + cableScore = new ConeCube(mDriveBase, mClaw, mArm, "CableScoreShort"); + stationScore = new ConeCubeCube(mDriveBase, mClaw, mArm, "Station3Piece"); + cableEngage = new CubeEngage(mDriveBase, mClaw, mArm, "CableEngage"); + stationEngage = new ScoreEngage(mDriveBase, mClaw, mArm, "StationScoreEngage3"); + centerEngage = new CenterEngageSequential(mDriveBase, mClaw, mArm); + centerIntake = new CenterIntake(mDriveBase, mClaw, mArm, "CenterIntake"); + scoreMid = new ScoreMid(mDriveBase, mClaw, mArm); } @@ -94,13 +82,12 @@ public RobotContainer() { public void robotInit() { CameraServer.startAutomaticCapture().setVideoMode(PixelFormat.kYUYV, 160, 120, 30); - driveBase.initVision(vision); - SmartDashboard.putNumber(autonChooser, 0); - updateArbitraryFeedForward = new Notifier(new Runnable() { - public void run() { - arm.updateArbitraryFeedForward(); - } - }); + mDriveBase.initVision(mVision); + SmartDashboard.putNumber(kAutonChooserString, 0); + updateArbitraryFeedForward = new Notifier( + () -> { + mArm.updateArbitraryFeedForward(); + }); updateArbitraryFeedForward.startPeriodic(0.01); } @@ -112,74 +99,48 @@ public void run() { */ @Override public MustangCommand getAutonomousCommand() { - // return new NonPidAutoLevel(driveBase, true); - // return new ConeCube(driveBase, claw, arm, "StationScoreShort"); + SmartDashboard.putBoolean(kMatchStartedString, true); - SmartDashboard.putBoolean(matchStarted, true); - - // SmartDashboard.putNumber(autonChooser, 0); - int selectedPath = (int) SmartDashboard.getNumber(autonChooser, 0); + int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0); MustangCommand autonCommand; switch (selectedPath) { case 0: autonCommand = cableScore; - led.solidhsv(LEDColor.LIGHT_BLUE); break; case 1: autonCommand = stationScore; - led.solidhsv(LEDColor.SEXY_YELLOW); break; case 2: autonCommand = cableEngage; - led.solidhsv(LEDColor.SEXY_PURPLE); break; case 3: autonCommand = stationEngage; - led.solidhsv(LEDColor.GREEN); break; case 4: autonCommand = centerEngage; - led.animatedRainbow(false, 10, 10); break; case 5: autonCommand = centerIntake; - led.solidhsv(LEDColor.PINK); break; case 6: autonCommand = scoreMid; - led.animatedMustangRainbow(10, 10); break; default: autonCommand = centerEngage; - led.animatedRainbow(false, 10, 10); - } + mLed.updateAutonPathColor(selectedPath); return autonCommand; - - // LEAVE COMMENTED - // greturn new ConeCube(driveBase, claw, arm, "CableScore"); - //return new AutonCalibration(driveBase, "Straight180"); // TODO: use curve - // path after - // straight - // path - // return new ConeCubeCube(driveBase, claw, arm, "Station3Piece"); - // return new ConeCubeEngage(driveBase, claw, arm, "StationScoreEngage2"); - // return new NonPidAutoLevel(driveBase, true); - //return new CenterEngage(driveBase, claw, arm, "CenterEngage"); - - // return new ConeCube(driveBase, claw, arm, "CableScore"); - // return new ConeCube(driveBase, claw, arm, "RightConeCube"); - // return new NonPidAutoLevel(driveBase, true); } @Override public void autonomousInit() { - arm.setStateToStarting(); + mArm.setStateToStarting(); } @Override public void teleopInit() { - arm.clearSetpoint(); + mArm.clearSetpoint(); + new ResetArmFromAbsolute(mArm); } @Override @@ -188,45 +149,17 @@ public void testInit() { @Override public void disabled() { - SmartDashboard.putBoolean(matchStarted, false); + SmartDashboard.putBoolean(kMatchStartedString, false); } @Override public void disabledPeriodic() { - // int selectedPath = (int) - // (SmartDashboard.getEntry(autonChooser).getInteger(-1)); + mArm.getShoulder().sendAngleToDashboard(); + mArm.getElbow().sendAngleToDashboard(); + mArm.getWrist().sendAngleToDashboard(); - arm.getShoulder().sendAngleToDashboard(); - arm.getElbow().sendAngleToDashboard(); - arm.getWrist().sendAngleToDashboard(); - - int selectedPath = (int) SmartDashboard.getNumber(autonChooser, 0); - switch (selectedPath) { - case 0: - led.blinkhsv(LEDColor.LIGHT_BLUE); - break; - case 1: - led.blinkhsv(LEDColor.SEXY_YELLOW); - break; - case 2: - led.blinkhsv(LEDColor.SEXY_PURPLE); - break; - case 3: - led.blinkhsv(LEDColor.GREEN); - break; - case 4: - led.animatedRainbow(false, 10, 10); - break; - case 5: - led.blinkhsv(LEDColor.PINK); - break; - case 6: - led.animatedMustangRainbow(10, 10); - break; - default: - led.animatedRainbow(false, 10, 10); - - } + int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0); + mLed.updateAutonPathColor(selectedPath); } @Override @@ -250,7 +183,6 @@ public void teleopPeriodic() { parkBeforeDisable(); } - public MustangController getOperatorController() { return OI.getOperatorController(); } @@ -266,7 +198,7 @@ public MustangController getBackupController() { private void parkBeforeDisable() { double cTime = DriverStation.getMatchTime(); if (cTime <= 0.1 && cTime != -1) { - driveBase.park(); + mDriveBase.park(); } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java index 6af98789..4fa27116 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java @@ -2,14 +2,11 @@ import java.util.HashMap; import java.util.Map; -import java.util.function.Supplier; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPoint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.commands.MustangScheduler; @@ -17,7 +14,6 @@ import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand; import frc.team670.robot.commands.vision.IsLockedOn; -import frc.team670.robot.constants.FieldConstants; import frc.team670.robot.constants.RobotConstants; import frc.team670.robot.subsystems.DriveBase; @@ -58,7 +54,8 @@ public Map getHealthRequirements() { @Override public void initialize() { this.startPose = driveBase.getPose(); - PathPlannerTrajectory traj = PathPlanner.generatePath(RobotConstants.kAutoPathConstraints, + PathPlannerTrajectory traj = PathPlanner.generatePath( + RobotConstants.DriveBase.kAutoPathConstraints, calcStartPoint(endPose), calcEndPoint(startPose)); driveBase.getPoseEstimator().addTrajectory(traj); diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPosePID.java b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPosePID.java index e3ab94f6..3b3be0a2 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPosePID.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPosePID.java @@ -49,8 +49,8 @@ public MoveToPosePID(SwerveDrive swerve, Pose2d pose, boolean isRelative) { PIDController xController = new PIDController(0.5, 0, 0); PIDController yController = new PIDController(0.5, 0, 0); ProfiledPIDController thetacontroller = new ProfiledPIDController(4, 0, 1, // not tuned yet - new Constraints(RobotConstants.kMaxAngularSpeedRadiansPerSecond, - RobotConstants.kMaxAngularSpeedRadiansPerSecondSquared)); + new Constraints(RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecond, + RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecondSquared)); holonomicDriveController = new HolonomicDriveController(xController, yController, thetacontroller); holonomicDriveController.setTolerance(new Pose2d(0.1, 0.1, Rotation2d.fromDegrees(0.5))); @@ -73,8 +73,8 @@ public MoveToPosePID(SwerveDrive swerve, double x, double y, boolean isRelative) PIDController xController = new PIDController(0.5, 0, 0); PIDController yController = new PIDController(0.5, 0, 0); ProfiledPIDController thetacontroller = new ProfiledPIDController(4, 0, 1, // not tuned yet - new Constraints(RobotConstants.kMaxAngularSpeedRadiansPerSecond, - RobotConstants.kMaxAngularSpeedRadiansPerSecondSquared)); + new Constraints(RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecond, + RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecondSquared)); holonomicDriveController = new HolonomicDriveController(xController, yController, thetacontroller); holonomicDriveController.setTolerance(new Pose2d(0.1, 0.1, Rotation2d.fromDegrees(0.5))); diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/PathFindMoveToPose.java b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/PathFindMoveToPose.java index fdd609f6..52c0f68c 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/PathFindMoveToPose.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/PathFindMoveToPose.java @@ -11,13 +11,13 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team670.mustanglib.commands.MustangCommand; +import frc.team670.mustanglib.pathfinder.ObstacleAvoidanceAStarMap; +import frc.team670.mustanglib.pathfinder.PoseNode; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand; import frc.team670.robot.constants.FieldConstants; import frc.team670.robot.constants.RobotConstants; -import frc.team670.robot.pathfinder.ObstacleAvoidanceAStarMap; -import frc.team670.robot.pathfinder.PoseNode; import frc.team670.robot.subsystems.DriveBase; public class PathFindMoveToPose extends CommandBase implements MustangCommand { @@ -47,8 +47,9 @@ public void initialize() { // points in the path. PathPoint[] fullPathPoints = getPathPointsFromNodes(fullPath); - PathPlannerTrajectory trajectory = PathPlanner - .generatePath(RobotConstants.kAutoPathConstraints, Arrays.asList(fullPathPoints)); + PathPlannerTrajectory trajectory = PathPlanner.generatePath( + RobotConstants.DriveBase.kAutoPathConstraints, + Arrays.asList(fullPathPoints)); driveBase.getPoseEstimator().addTrajectory(trajectory); pathDrivingCommand = driveBase.getFollowTrajectoryCommand(trajectory); pathDrivingCommand.schedule(); diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/TurnToAngle.java b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/TurnToAngle.java index a4db458e..6fae2f81 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/TurnToAngle.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/TurnToAngle.java @@ -44,8 +44,8 @@ public TurnToAngle(SwerveDrive swerve, double angle) { PIDController xcontroller = new PIDController(0, 0, 0); PIDController ycontroller = new PIDController(0, 0, 0); ProfiledPIDController thetacontroller = new ProfiledPIDController(4, 0, 1, // not tuned yet - new Constraints(RobotConstants.kMaxAngularSpeedRadiansPerSecond, - RobotConstants.kMaxAngularSpeedRadiansPerSecondSquared)); + new Constraints(RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecond, + RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecondSquared)); holonomicDriveController = new HolonomicDriveController(xcontroller, ycontroller, thetacontroller); @@ -65,8 +65,8 @@ public TurnToAngle(SwerveDrive swerve, double angle, boolean isRelative, PIDController xcontroller = new PIDController(0, 0, 0); PIDController ycontroller = new PIDController(0, 0, 0); ProfiledPIDController thetacontroller = new ProfiledPIDController(4, 0, 1, // not tuned yet - new Constraints(RobotConstants.kMaxAngularSpeedRadiansPerSecond, - RobotConstants.kMaxAngularSpeedRadiansPerSecondSquared)); + new Constraints(RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecond, + RobotConstants.DriveBase.kMaxAngularSpeedRadiansPerSecondSquared)); holonomicDriveController = new HolonomicDriveController(xcontroller, ycontroller, thetacontroller); diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorPurple.java b/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorPurple.java index 1cb00484..dd8713b2 100755 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorPurple.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorPurple.java @@ -23,7 +23,7 @@ public SetColorPurple(LED led) { public void initialize() { // Logger.consoleLog("LED COLOR BEING SET TO PURPLE"); - led.setColorPurple(); + led.setCubeColor(); } @Override diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorYellow.java b/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorYellow.java index bdba08af..5c2d02e6 100755 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorYellow.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/leds/SetColorYellow.java @@ -21,7 +21,7 @@ public SetColorYellow(LED led) { } public void initialize() { - led.setColorYellow(); + led.setConeColor(); } @Override diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/AutonCalibration.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/AutonCalibration.java index e5e8280c..1d67ef4b 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/AutonCalibration.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/AutonCalibration.java @@ -5,23 +5,12 @@ import java.util.Map; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.auto.SwerveAutoBuilder; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; -import frc.team670.robot.commands.arm.MoveToTarget; -import frc.team670.robot.commands.claw.ClawEject; -import frc.team670.robot.commands.drivebase.NonPidAutoLevel; -import frc.team670.robot.constants.RobotConstants; -import frc.team670.robot.subsystems.Claw; import frc.team670.robot.subsystems.DriveBase; -import frc.team670.robot.subsystems.arm.Arm; -import frc.team670.robot.subsystems.arm.ArmState; public class AutonCalibration extends SequentialCommandGroup implements MustangCommand { @@ -36,15 +25,8 @@ public AutonCalibration(DriveBase driveBase, String pathName) { this.pathName = pathName; List trajectoryGroup = PathPlanner.loadPathGroup(pathName, 2.5, 2); - HashMap eventMap = new HashMap<>(); + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(new HashMap<>()).fullAuto(trajectoryGroup); - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); addCommands(fullAuto); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngage.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngage.java index 23e5d82a..fdcdc34e 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngage.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngage.java @@ -44,14 +44,8 @@ public CenterEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { eventMap.put("clawEject", new ClawEject(claw)); eventMap.put("stow", new MoveToTarget(arm, ArmState.STOWED)); eventMap.put("autoLevel", new NonPidAutoLevel(driveBase, true)); - - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); addCommands(fullAuto); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngageSequential.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngageSequential.java index c3340f27..eff10227 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngageSequential.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterEngageSequential.java @@ -3,33 +3,26 @@ import java.util.HashMap; import java.util.List; import java.util.Map; - -import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.auto.SwerveAutoBuilder; -import com.pathplanner.lib.commands.FollowPathWithEvents; - import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; -import frc.team670.mustanglib.swervelib.pathplanner.MustangFollowPathWithEvents; import frc.team670.robot.commands.arm.MoveToTarget; import frc.team670.robot.commands.claw.ClawInstantEject; import frc.team670.robot.commands.drivebase.NonPidAutoLevel; import frc.team670.robot.commands.drivebase.TurnToAngle; import frc.team670.robot.constants.OI; -import frc.team670.robot.constants.RobotConstants; import frc.team670.robot.subsystems.Claw; import frc.team670.robot.subsystems.DriveBase; import frc.team670.robot.subsystems.arm.Arm; import frc.team670.robot.subsystems.arm.ArmState; public class CenterEngageSequential extends SequentialCommandGroup implements MustangCommand { - + public Map getHealthRequirements() { return new HashMap(); } @@ -37,18 +30,12 @@ public Map getHealthRequirements() { public CenterEngageSequential(DriveBase driveBase, Claw claw, Arm arm) { List trajectoryGroup = PathPlanner.loadPathGroup("BackUp", 4, 4.5); - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, null, true, new Subsystem[] {driveBase}); - - addCommands(new SequentialCommandGroup( - new MoveToTarget(arm, ArmState.SCORE_MID), - new ClawInstantEject(claw), - new MoveToTarget(arm, ArmState.STOWED), - autoBuilder.fullAuto(trajectoryGroup), - new TurnToAngle(driveBase, 180, true, OI.getDriverController()), - new NonPidAutoLevel(driveBase, true))); + + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(null).fullAuto(trajectoryGroup); + addCommands(new SequentialCommandGroup(new MoveToTarget(arm, ArmState.SCORE_MID), + new ClawInstantEject(claw), new MoveToTarget(arm, ArmState.STOWED), fullAuto, + new TurnToAngle(driveBase, 180, true, OI.getDriverController()), + new NonPidAutoLevel(driveBase, true))); } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterIntake.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterIntake.java index 60c5ec53..7fd3e303 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterIntake.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CenterIntake.java @@ -26,7 +26,7 @@ import frc.team670.robot.subsystems.arm.ArmState; public class CenterIntake extends SequentialCommandGroup implements MustangCommand { - + String pathName; public Map getHealthRequirements() { @@ -47,14 +47,9 @@ public CenterIntake(DriveBase driveBase, Claw claw, Arm arm, String pathName) { eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.HYBRID)); eventMap.put("clawIntake", new ClawInstantIntake(claw)); eventMap.put("autoLevel", new NonPidAutoLevel(driveBase, false)); - - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); + addCommands(fullAuto); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java index 513b2a4f..13946863 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java @@ -40,20 +40,15 @@ public ConeCube(DriveBase driveBase, Claw claw, Arm arm, String pathName) { HashMap eventMap = new HashMap<>(); // eventMap stuff - //eventMap.put("clawIntake1", new ClawInstantIntake(claw)); + // eventMap.put("clawIntake1", new ClawInstantIntake(claw)); eventMap.put("moveToMid", new MoveToTarget(arm, ArmState.SCORE_MID)); eventMap.put("clawEject", new ClawInstantEject(claw)); eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.HYBRID)); - eventMap.put("clawIntake", new ClawInstantIntake(claw)); //May want to use IntakeAndStow after testing. + eventMap.put("clawIntake", new ClawInstantIntake(claw)); // May want to use IntakeAndStow + // after testing. eventMap.put("moveToStowed", new MoveToTarget(arm, ArmState.STOWED)); - - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); addCommands(fullAuto); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeCube.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeCube.java index 1e39ea8f..96cd05a4 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeCube.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeCube.java @@ -45,19 +45,10 @@ public ConeCubeCube(DriveBase driveBase, Claw claw, Arm arm, String pathName) { eventMap.put("moveToHigh", new MoveToTarget(arm, ArmState.SCORE_HIGH)); eventMap.put("clawEject", new ClawInstantEject(claw)); eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.HYBRID)); - eventMap.put("clawIntake", new ClawInstantIntake(claw)); // May want to use IntakeAndStow - // after testing. + eventMap.put("clawIntake", new ClawInstantIntake(claw)); eventMap.put("moveToStowed", new MoveToTarget(arm, ArmState.STOWED)); - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, - RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); - + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); addCommands(fullAuto); } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeEngage.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeEngage.java index af554269..78a9979f 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeEngage.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCubeEngage.java @@ -41,23 +41,19 @@ public ConeCubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) HashMap eventMap = new HashMap<>(); // eventMap stuff - //eventMap.put("clawIntake1", new ClawInstantIntake(claw)); + // eventMap.put("clawIntake1", new ClawInstantIntake(claw)); // eventMap.put("moveToMid", new MoveToTarget(arm, ArmState.SCORE_MID)); // eventMap.put("clawEject", new ClawInstantEject(claw)); // eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.HYBRID)); - // eventMap.put("clawIntake", new ClawInstantIntake(claw)); //May want to use IntakeAndStow after testing. + // eventMap.put("clawIntake", new ClawInstantIntake(claw)); //May want to use IntakeAndStow + // after testing. // eventMap.put("moveToStowed", new MoveToTarget(arm, ArmState.STOWED)); // eventMap.put("moveToHigh", new MoveToTarget(arm, ArmState.SCORE_HIGH)); eventMap.put("autoLevel", new NonPidAutoLevel(driveBase, true)); - - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); addCommands(fullAuto); } + + protected ConeCubeEngage() {} } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java index eed2a34e..9fbebcee 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java @@ -39,10 +39,11 @@ public Map getHealthRequirements() { public CubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { this.pathName = pathName; - List trajectoryGroup = PathPlanner.loadPathGroup(pathName, 2.75, 1.5); + List trajectoryGroup = + PathPlanner.loadPathGroup(pathName, 2.75, 1.5); - PIDConstants PID_translation = RobotConstants.AUTON_TRANSLATION_CONTROLLER; - PIDConstants PID_theta = RobotConstants.AUTON_THETA_CONTROLLER; + PIDConstants PID_translation = RobotConstants.DriveBase.kAutonTranslationPID; + PIDConstants PID_theta = RobotConstants.DriveBase.kAutonThetaPID; Map eventMap = new HashMap<>(); @@ -57,14 +58,7 @@ public CubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { // on, markers are the // same - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, PID_translation, PID_theta, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); - + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); addCommands(fullAuto); } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ScoreEngage.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ScoreEngage.java index e350df63..014621c4 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ScoreEngage.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ScoreEngage.java @@ -6,8 +6,6 @@ import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.auto.SwerveAutoBuilder; - -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -50,17 +48,10 @@ public ScoreEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { eventMap.put("clawEject", new ClawInstantEject(claw)); eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.HYBRID)); eventMap.put("clawIntake", new ClawInstantIntake(claw)); // May want to use IntakeAndStow - // after testing. - - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getPose, - driveBase::resetOdometry, driveBaseKinematics, - RobotConstants.AUTON_TRANSLATION_CONTROLLER, RobotConstants.AUTON_THETA_CONTROLLER, - driveBase::setModuleStates, eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); + + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(eventMap).fullAuto(trajectoryGroup); - addCommands(fullAuto, new TurnToAngle(driveBase, 180, true, OI.getDriverController()), new NonPidAutoLevel(driveBase, true)); + addCommands(fullAuto, new TurnToAngle(driveBase, 180, true, OI.getDriverController()), + new NonPidAutoLevel(driveBase, true)); } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/vision/IsLockedOn.java b/2023-Robot/src/main/java/frc/team670/robot/commands/vision/IsLockedOn.java index 7626dd52..8478da25 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/vision/IsLockedOn.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/vision/IsLockedOn.java @@ -1,21 +1,14 @@ package frc.team670.robot.commands.vision; import java.util.Map; - import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import javax.print.attribute.HashAttributeSet; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; -import frc.team670.mustanglib.subsystems.VisionSubsystemBase; import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; -import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; -import frc.team670.mustanglib.utils.Logger; import frc.team670.robot.constants.RobotConstants; import frc.team670.robot.subsystems.DriveBase; @@ -29,13 +22,7 @@ public class IsLockedOn extends CommandBase implements MustangCommand { private STATUS turnStatus, strafeStatus, moveStatus; private enum STATUS { - MOVE_UP, - MOVE_DOWN, - MOVE_LEFT, - MOVE_RIGHT, - TURN_CLOCK, - TURN_COUNTERCLOCK, - OKAY + MOVE_UP, MOVE_DOWN, MOVE_LEFT, MOVE_RIGHT, TURN_CLOCK, TURN_COUNTERCLOCK, OKAY } public IsLockedOn(DriveBase driveBase, Pose2d goalPose) { @@ -51,7 +38,8 @@ public Map getHealthRequirements() { @Override public void execute() { updatePoseAlignment(); - SmartDashboard.putStringArray("align", new String[] {moveStatus.name(), strafeStatus.name(), turnStatus.name()}); + SmartDashboard.putStringArray("align", + new String[] {moveStatus.name(), strafeStatus.name(), turnStatus.name()}); } @Override @@ -69,26 +57,26 @@ private void updatePoseAlignment() { double driverDX = allianceAdjustment(goalPose.getX() - currentPose.getX()); double driverDY = allianceAdjustment(goalPose.getY() - currentPose.getY()); double dRot = goalPose.getRotation().getDegrees() - currentPose.getRotation().getDegrees(); - - if (driverDX > RobotConstants.LOCKED_ON_ERROR_X) { + + if (driverDX > RobotConstants.Vision.kLockedOnErrorX) { moveStatus = STATUS.MOVE_UP; - } else if (driverDX < RobotConstants.LOCKED_ON_ERROR_X) { + } else if (driverDX < RobotConstants.Vision.kLockedOnErrorX) { moveStatus = STATUS.MOVE_DOWN; } else { moveStatus = STATUS.OKAY; } - - if (driverDY > RobotConstants.LOCKED_ON_ERROR_Y) { + + if (driverDY > RobotConstants.Vision.xLockedOnErrorY) { strafeStatus = STATUS.MOVE_RIGHT; - } else if (driverDY < RobotConstants.LOCKED_ON_ERROR_Y) { + } else if (driverDY < RobotConstants.Vision.xLockedOnErrorY) { strafeStatus = STATUS.MOVE_LEFT; } else { strafeStatus = STATUS.OKAY; } - - if (dRot > RobotConstants.LOCKED_ON_ERROR_DEGREES) { + + if (dRot > RobotConstants.Vision.kLockedOnErrorDegrees) { turnStatus = STATUS.TURN_CLOCK; - } else if (dRot < RobotConstants.LOCKED_ON_ERROR_DEGREES) { + } else if (dRot < RobotConstants.Vision.kLockedOnErrorDegrees) { turnStatus = STATUS.TURN_COUNTERCLOCK; } else { turnStatus = STATUS.OKAY; diff --git a/2023-Robot/src/main/java/frc/team670/robot/constants/FieldConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/FieldConstants.java index 879f46c4..a88d1e45 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/FieldConstants.java +++ b/2023-Robot/src/main/java/frc/team670/robot/constants/FieldConstants.java @@ -15,398 +15,388 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.team670.robot.pathfinder.Obstacle; -import frc.team670.robot.pathfinder.PoseNode; +import frc.team670.mustanglib.pathfinder.Obstacle; +import frc.team670.mustanglib.pathfinder.PoseNode; /** * ORIGIN AT BOTTOM LEFT * https://firstfrc.blob.core.windows.net/frc2023/Manual/Sections/2023FRCGameManual-05.pdf */ public class FieldConstants { - public static final double fieldLength = Units.inchesToMeters(651.25); - public static final double fieldWidth = Units.inchesToMeters(315.5); - public static final double tapeWidth = Units.inchesToMeters(2.0); - - // Dimensions for community and charging station, including the tape. - public static final class Community { - // Region dimensions - public static final double innerX = 0.0; - public static final double midX = Units.inchesToMeters(132.375); // Tape to the left - // of charging - // station - public static final double outerX = Units.inchesToMeters(193.25); // Tape to the - // right of - // charging - // station - public static final double leftY = Units.feetToMeters(18.0); - public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth; - public static final double rightY = 0.0; - public static final Translation2d[] regionCorners = new Translation2d[] { - new Translation2d(innerX, rightY), new Translation2d(innerX, leftY), - new Translation2d(midX, leftY), new Translation2d(midX, midY), - new Translation2d(outerX, midY), - new Translation2d(outerX, rightY), }; - - // Charging station dimensions - public static final double chargingStationLength = Units.inchesToMeters(76.125); - public static final double chargingStationWidth = Units.inchesToMeters(97.25); - public static final double chargingStationOuterX = outerX - tapeWidth; - public static final double chargingStationInnerX = chargingStationOuterX - chargingStationLength; - public static final double chargingStationLeftY = midY - tapeWidth; - public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth; - public static final Translation2d[] chargingStationCorners = new Translation2d[] { - new Translation2d(chargingStationInnerX, chargingStationRightY), - new Translation2d(chargingStationInnerX, chargingStationLeftY), - new Translation2d(chargingStationOuterX, chargingStationRightY), - new Translation2d(chargingStationOuterX, chargingStationLeftY) }; - - // Cable bump - public static final double cableBumpInnerX = innerX + Grids.outerX + Units.inchesToMeters(95.25); - public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7); - public static final Translation2d[] cableBumpCorners = new Translation2d[] { - new Translation2d(cableBumpInnerX, 0.0), - new Translation2d(cableBumpInnerX, chargingStationRightY), - new Translation2d(cableBumpOuterX, 0.0), - new Translation2d(cableBumpOuterX, chargingStationRightY) }; + public static final double fieldLength = Units.inchesToMeters(651.25); + public static final double fieldWidth = Units.inchesToMeters(315.5); + public static final double tapeWidth = Units.inchesToMeters(2.0); + + // Dimensions for community and charging station, including the tape. + public static final class Community { + // Region dimensions + public static final double innerX = 0.0; + public static final double midX = Units.inchesToMeters(132.375); // Tape to the left + // of charging + // station + public static final double outerX = Units.inchesToMeters(193.25); // Tape to the + // right of + // charging + // station + public static final double leftY = Units.feetToMeters(18.0); + public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth; + public static final double rightY = 0.0; + public static final Translation2d[] regionCorners = new Translation2d[] { + new Translation2d(innerX, rightY), new Translation2d(innerX, leftY), + new Translation2d(midX, leftY), new Translation2d(midX, midY), + new Translation2d(outerX, midY), + new Translation2d(outerX, rightY), }; + + // Charging station dimensions + public static final double chargingStationLength = Units.inchesToMeters(76.125); + public static final double chargingStationWidth = Units.inchesToMeters(97.25); + public static final double chargingStationOuterX = outerX - tapeWidth; + public static final double chargingStationInnerX = chargingStationOuterX - chargingStationLength; + public static final double chargingStationLeftY = midY - tapeWidth; + public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth; + public static final Translation2d[] chargingStationCorners = new Translation2d[] { + new Translation2d(chargingStationInnerX, chargingStationRightY), + new Translation2d(chargingStationInnerX, chargingStationLeftY), + new Translation2d(chargingStationOuterX, chargingStationRightY), + new Translation2d(chargingStationOuterX, chargingStationLeftY) }; + + // Cable bump + public static final double cableBumpInnerX = innerX + Grids.outerX + Units.inchesToMeters(95.25); + public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7); + public static final Translation2d[] cableBumpCorners = new Translation2d[] { + new Translation2d(cableBumpInnerX, 0.0), + new Translation2d(cableBumpInnerX, chargingStationRightY), + new Translation2d(cableBumpOuterX, 0.0), + new Translation2d(cableBumpOuterX, chargingStationRightY) }; + } + + // Dimensions for grids and nodes + public static final class Grids { + // X layout + public static final double outerX = Units.inchesToMeters(54.25); + public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered + // when + // under + // cube + // nodes + public static final double midX = outerX - Units.inchesToMeters(22.75); + public static final double highX = outerX - Units.inchesToMeters(39.75); + + // Y layout + public static final int nodeRowCount = 9; + public static final double nodeFirstY = Units.inchesToMeters(20.19); + public static final double nodeSeparationY = Units.inchesToMeters(22.0); + + // Z layout + public static final double cubeEdgeHigh = Units.inchesToMeters(3.0); + public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh; + public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh; + public static final double highConeZ = Units.inchesToMeters(46.0); + public static final double midConeZ = Units.inchesToMeters(34.0); + + // Translations (all nodes in the same column/row have the same X/Y coordinate) + public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount]; + public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount]; + public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount]; + + static { + for (int i = 0; i < nodeRowCount; i++) { + boolean isCube = i == 1 || i == 4 || i == 7; + lowTranslations[i] = new Translation2d(lowX, + nodeFirstY + nodeSeparationY * i); + midTranslations[i] = new Translation2d(midX, + nodeFirstY + nodeSeparationY * i); + mid3dTranslations[i] = new Translation3d(midX, + nodeFirstY + nodeSeparationY * i, + isCube ? midCubeZ : midConeZ); + high3dTranslations[i] = new Translation3d(highX, + nodeFirstY + nodeSeparationY * i, + isCube ? highCubeZ : highConeZ); + highTranslations[i] = new Translation2d(highX, + nodeFirstY + nodeSeparationY * i); + } } - // Dimensions for grids and nodes - public static final class Grids { - // X layout - public static final double outerX = Units.inchesToMeters(54.25); - public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered - // when - // under - // cube - // nodes - public static final double midX = outerX - Units.inchesToMeters(22.75); - public static final double highX = outerX - Units.inchesToMeters(39.75); - - // Y layout - public static final int nodeRowCount = 9; - public static final double nodeFirstY = Units.inchesToMeters(20.19); - public static final double nodeSeparationY = Units.inchesToMeters(22.0); - - // Z layout - public static final double cubeEdgeHigh = Units.inchesToMeters(3.0); - public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh; - public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh; - public static final double highConeZ = Units.inchesToMeters(46.0); - public static final double midConeZ = Units.inchesToMeters(34.0); - - // Translations (all nodes in the same column/row have the same X/Y coordinate) - public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount]; - public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount]; - public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount]; - public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount]; - public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount]; - - static { - for (int i = 0; i < nodeRowCount; i++) { - boolean isCube = i == 1 || i == 4 || i == 7; - lowTranslations[i] = new Translation2d(lowX, - nodeFirstY + nodeSeparationY * i); - midTranslations[i] = new Translation2d(midX, - nodeFirstY + nodeSeparationY * i); - mid3dTranslations[i] = new Translation3d(midX, - nodeFirstY + nodeSeparationY * i, - isCube ? midCubeZ : midConeZ); - high3dTranslations[i] = new Translation3d(highX, - nodeFirstY + nodeSeparationY * i, - isCube ? highCubeZ : highConeZ); - highTranslations[i] = new Translation2d(highX, - nodeFirstY + nodeSeparationY * i); - } - } - - // Complex low layout (shifted to account for cube vs cone rows and wide edge - // nodes) - public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0; // Centered - // X - // under - // cone - // nodes - public static final double complexLowXCubes = lowX; // Centered X under cube nodes - public static final double complexLowOuterYOffset = nodeFirstY - - Units.inchesToMeters(3.0) - (Units.inchesToMeters(25.75) / 2.0); - - public static final Translation2d[] complexLowTranslations = new Translation2d[] { - new Translation2d(complexLowXCones, - nodeFirstY - complexLowOuterYOffset), - new Translation2d(complexLowXCubes, - nodeFirstY + nodeSeparationY * 1), - new Translation2d(complexLowXCones, - nodeFirstY + nodeSeparationY * 2), - new Translation2d(complexLowXCones, - nodeFirstY + nodeSeparationY * 3), - new Translation2d(complexLowXCubes, - nodeFirstY + nodeSeparationY * 4), - new Translation2d(complexLowXCones, - nodeFirstY + nodeSeparationY * 5), - new Translation2d(complexLowXCones, - nodeFirstY + nodeSeparationY * 6), - new Translation2d(complexLowXCubes, - nodeFirstY + nodeSeparationY * 7), - new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 8 - + complexLowOuterYOffset) }; - - public static final Pose2d[] scoringPoses = new Pose2d[complexLowTranslations.length]; - static { - for (int i = 0; i < scoringPoses.length; i++) { - scoringPoses[i] = new Pose2d( - complexLowTranslations[i].plus(new Translation2d( - RobotConstants.DRIVEBASE_CLEARANCE, - 0)), - new Rotation2d()); - } - } + // Complex low layout (shifted to account for cube vs cone rows and wide edge + // nodes) + public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0; // Centered + // X + // under + // cone + // nodes + public static final double complexLowXCubes = lowX; // Centered X under cube nodes + public static final double complexLowOuterYOffset = nodeFirstY + - Units.inchesToMeters(3.0) - (Units.inchesToMeters(25.75) / 2.0); + + public static final Translation2d[] complexLowTranslations = new Translation2d[] { + new Translation2d(complexLowXCones, + nodeFirstY - complexLowOuterYOffset), + new Translation2d(complexLowXCubes, + nodeFirstY + nodeSeparationY * 1), + new Translation2d(complexLowXCones, + nodeFirstY + nodeSeparationY * 2), + new Translation2d(complexLowXCones, + nodeFirstY + nodeSeparationY * 3), + new Translation2d(complexLowXCubes, + nodeFirstY + nodeSeparationY * 4), + new Translation2d(complexLowXCones, + nodeFirstY + nodeSeparationY * 5), + new Translation2d(complexLowXCones, + nodeFirstY + nodeSeparationY * 6), + new Translation2d(complexLowXCubes, + nodeFirstY + nodeSeparationY * 7), + new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 8 + + complexLowOuterYOffset) }; + + public static final Pose2d[] scoringPoses = new Pose2d[complexLowTranslations.length]; + static { + for (int i = 0; i < scoringPoses.length; i++) { + scoringPoses[i] = new Pose2d( + complexLowTranslations[i].plus(new Translation2d( + RobotConstants.DriveBase.kClearance, + 0)), + new Rotation2d()); + } } - - // Dimensions for loading zone and substations, including the tape - public static final class LoadingZone { - // Region dimensions - public static final double width = Units.inchesToMeters(99.0); - public static final double innerX = FieldConstants.fieldLength; - public static final double midX = fieldLength - Units.inchesToMeters(132.25); - public static final double outerX = fieldLength - Units.inchesToMeters(264.25); - public static final double leftY = FieldConstants.fieldWidth; - public static final double midY = leftY - Units.inchesToMeters(50.5); - public static final double rightY = leftY - width; - public static final Translation2d[] regionCorners = new Translation2d[] { - new Translation2d(midX, rightY), // Start at lower left next to - // border with opponent - // community - new Translation2d(midX, midY), new Translation2d(outerX, midY), - new Translation2d(outerX, leftY), new Translation2d(innerX, leftY), - new Translation2d(innerX, rightY), }; - - // Double substation dimensions - public static final double doubleSubstationLength = Units.inchesToMeters(14.0); - public static final double doubleSubstationX = innerX - doubleSubstationLength; - public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375); - - // Single substation dimensions - public static final double singleSubstationWidth = Units.inchesToMeters(22.75); - public static final double singleSubstationLeftX = FieldConstants.fieldLength - - doubleSubstationLength - Units.inchesToMeters(88.77); - public static final double singleSubstationCenterX = singleSubstationLeftX - + (singleSubstationWidth / 2.0); - public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth; - public static final Translation2d singleSubstationTranslation = new Translation2d( - singleSubstationCenterX, - leftY); - - public static final double singleSubstationHeight = Units.inchesToMeters(18.0); - public static final double singleSubstationLowZ = Units.inchesToMeters(27.125); - public static final double singleSubstationCenterZ = singleSubstationLowZ - + (singleSubstationHeight / 2.0); - public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight; - public static final Pose2d[] IntakePoses = { - // single substation - new Pose2d(singleSubstationTranslation - .minus(new Translation2d(-RobotConstants.DRIVEBASE_WIDTH/2, RobotConstants.DRIVEBASE_CLEARANCE)), - new Rotation2d(-Math.PI / 2)), - - // double substation - new Pose2d(fieldLength - 0.31 - RobotConstants.DRIVEBASE_CLEARANCE, 6.01 + 1.31, - new Rotation2d(Math.PI)), - new Pose2d(fieldLength - 0.31 - RobotConstants.DRIVEBASE_CLEARANCE, 6.01, - new Rotation2d(Math.PI)) - }; - } - - // Locations of staged game pieces - public static final class StagingLocations { - public static final double centerOffsetX = Units.inchesToMeters(47.36); - public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36); - public static final double firstY = Units.inchesToMeters(36.19); - public static final double separationY = Units.inchesToMeters(48.0); - public static final Translation2d[] translations = new Translation2d[4]; - - static { - for (int i = 0; i < translations.length; i++) { - translations[i] = new Translation2d(positionX, - firstY + (i * separationY)); - } - } + } + + // Dimensions for loading zone and substations, including the tape + public static final class LoadingZone { + // Region dimensions + public static final double width = Units.inchesToMeters(99.0); + public static final double innerX = FieldConstants.fieldLength; + public static final double midX = fieldLength - Units.inchesToMeters(132.25); + public static final double outerX = fieldLength - Units.inchesToMeters(264.25); + public static final double leftY = FieldConstants.fieldWidth; + public static final double midY = leftY - Units.inchesToMeters(50.5); + public static final double rightY = leftY - width; + public static final Translation2d[] regionCorners = new Translation2d[] { + new Translation2d(midX, rightY), // Start at lower left next to + // border with opponent + // community + new Translation2d(midX, midY), new Translation2d(outerX, midY), + new Translation2d(outerX, leftY), new Translation2d(innerX, leftY), + new Translation2d(innerX, rightY), }; + + // Double substation dimensions + public static final double doubleSubstationLength = Units.inchesToMeters(14.0); + public static final double doubleSubstationX = innerX - doubleSubstationLength; + public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375); + + // Single substation dimensions + public static final double singleSubstationWidth = Units.inchesToMeters(22.75); + public static final double singleSubstationLeftX = FieldConstants.fieldLength + - doubleSubstationLength - Units.inchesToMeters(88.77); + public static final double singleSubstationCenterX = singleSubstationLeftX + (singleSubstationWidth / 2.0); + public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth; + public static final Translation2d singleSubstationTranslation = new Translation2d(singleSubstationCenterX, + leftY); + + public static final double singleSubstationHeight = Units.inchesToMeters(18.0); + public static final double singleSubstationLowZ = Units.inchesToMeters(27.125); + public static final double singleSubstationCenterZ = singleSubstationLowZ + (singleSubstationHeight / 2.0); + public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight; + public static final Pose2d[] IntakePoses = { + // single substation + new Pose2d(singleSubstationTranslation.minus(new Translation2d( + -RobotConstants.DriveBase.kWidth / 2, + RobotConstants.DriveBase.kClearance)), + new Rotation2d(-Math.PI / 2)), + + // double substation + new Pose2d(fieldLength - 0.31 - RobotConstants.DriveBase.kClearance, + 6.01 + 1.31, new Rotation2d(Math.PI)), + new Pose2d(fieldLength - 0.31 - RobotConstants.DriveBase.kClearance, + 6.01, new Rotation2d(Math.PI)) }; + } + + // Locations of staged game pieces + public static final class StagingLocations { + public static final double centerOffsetX = Units.inchesToMeters(47.36); + public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36); + public static final double firstY = Units.inchesToMeters(36.19); + public static final double separationY = Units.inchesToMeters(48.0); + public static final Translation2d[] translations = new Translation2d[4]; + + static { + for (int i = 0; i < translations.length; i++) { + translations[i] = new Translation2d(positionX, + firstY + (i * separationY)); + } } - - public static final Map blueAprilTags = Map.of( - 1, - new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)), - 2, - new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)), - 3, new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)), - 4, - new Pose3d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74), - Units.inchesToMeters(27.38), - new Rotation3d(0.0, 0.0, Math.PI))); - - public static final Map redAprilTags = Map.of( - 5, - new Pose3d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), - Units.inchesToMeters(27.38), new Rotation3d()), - 6, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), - Units.inchesToMeters(18.22), new Rotation3d()), - 7, - new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), - Units.inchesToMeters(18.22), new Rotation3d()), - 8, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), - Units.inchesToMeters(18.22), new Rotation3d())); - - // AprilTag locations (do not flip for red alliance) - // FIELD ORIENTED, bottom left (blue) is 0, 0 - public static final Map aprilTags = Map.of(1, - new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)), - 2, - new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)), - 3, new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI)), - 4, - new Pose3d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74), - Units.inchesToMeters(27.38), - new Rotation3d(0.0, 0.0, Math.PI)), - 5, - new Pose3d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), - Units.inchesToMeters(27.38), new Rotation3d()), - 6, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), - Units.inchesToMeters(18.22), new Rotation3d()), - 7, - new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), - Units.inchesToMeters(18.22), new Rotation3d()), - 8, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), - Units.inchesToMeters(18.22), new Rotation3d())); - - public static final Translation2d[] obstacleCorners = { - // right inner - new Translation2d( - FieldConstants.Community.chargingStationCorners[0].getX(), - FieldConstants.Community.chargingStationCorners[0].getY()), - new Translation2d( - FieldConstants.Community.chargingStationCorners[1].getX(), - FieldConstants.Community.chargingStationCorners[1].getY()), - new Translation2d( - FieldConstants.Community.chargingStationCorners[3].getX(), - FieldConstants.Community.chargingStationCorners[3].getY()), - new Translation2d( - FieldConstants.Community.chargingStationCorners[2].getX(), - FieldConstants.Community.chargingStationCorners[2] - .getY()) }; - - public static AprilTagFieldLayout getFieldLayout(Map tags) { - List t = new ArrayList<>(); - FieldConstants.aprilTags.forEach((i, p) -> { - t.add(new AprilTag(i, p)); - }); - return new AprilTagFieldLayout(t, FieldConstants.fieldLength, FieldConstants.fieldWidth); + } + + public static final Map blueAprilTags = Map.of(1, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)), + 2, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)), + 3, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)), + 4, + new Pose3d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74), + Units.inchesToMeters(27.38), + new Rotation3d(0.0, 0.0, Math.PI))); + + public static final Map redAprilTags = Map.of(5, + new Pose3d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), + Units.inchesToMeters(27.38), new Rotation3d()), + 6, + new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), + Units.inchesToMeters(18.22), new Rotation3d()), + 7, + new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), + Units.inchesToMeters(18.22), new Rotation3d()), + 8, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), + Units.inchesToMeters(18.22), new Rotation3d())); + + // AprilTag locations (do not flip for red alliance) + // FIELD ORIENTED, bottom left (blue) is 0, 0 + public static final Map aprilTags = Map.of(1, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(42.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)), + 2, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(108.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)), + 3, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI)), + 4, + new Pose3d(Units.inchesToMeters(636.96), Units.inchesToMeters(265.74), + Units.inchesToMeters(27.38), + new Rotation3d(0.0, 0.0, Math.PI)), + 5, + new Pose3d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), + Units.inchesToMeters(27.38), new Rotation3d()), + 6, + new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), + Units.inchesToMeters(18.22), new Rotation3d()), + 7, + new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), + Units.inchesToMeters(18.22), new Rotation3d()), + 8, new Pose3d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), + Units.inchesToMeters(18.22), new Rotation3d())); + + public static final Translation2d[] obstacleCorners = { + // right inner + new Translation2d(FieldConstants.Community.chargingStationCorners[0].getX(), + FieldConstants.Community.chargingStationCorners[0].getY()), + new Translation2d(FieldConstants.Community.chargingStationCorners[1].getX(), + FieldConstants.Community.chargingStationCorners[1].getY()), + new Translation2d(FieldConstants.Community.chargingStationCorners[3].getX(), + FieldConstants.Community.chargingStationCorners[3].getY()), + new Translation2d(FieldConstants.Community.chargingStationCorners[2].getX(), + FieldConstants.Community.chargingStationCorners[2].getY()) }; + + public static AprilTagFieldLayout getFieldLayout(Map tags) { + List t = new ArrayList<>(); + FieldConstants.aprilTags.forEach((i, p) -> { + t.add(new AprilTag(i, p)); + }); + return new AprilTagFieldLayout(t, FieldConstants.fieldLength, + FieldConstants.fieldWidth); + } + + public static final List obstacles = List.of( + // add charging station of one side + new Obstacle(new Translation2d[] { obstacleCorners[0], obstacleCorners[1], + obstacleCorners[2], obstacleCorners[3] }), + new Obstacle(new Translation2d[] { allianceFlip(obstacleCorners[0]), + allianceFlip(obstacleCorners[1]), + allianceFlip(obstacleCorners[2]), + allianceFlip(obstacleCorners[3]) })); + + public static List obstacleContingencyNodes = List.of( + new PoseNode(allianceFlip(Community.chargingStationCorners[0].plus( + new Translation2d(-RobotConstants.DriveBase.kClearance, + -RobotConstants.DriveBase.kClearance))), + new Rotation2d()), + new PoseNode(allianceFlip(Community.chargingStationCorners[1].plus( + new Translation2d(-RobotConstants.DriveBase.kClearance, + RobotConstants.DriveBase.kClearance))), + new Rotation2d()), + new PoseNode(allianceFlip(Community.chargingStationCorners[3] + .plus(new Translation2d(RobotConstants.DriveBase.kClearance, + RobotConstants.DriveBase.kClearance))), + new Rotation2d()), + new PoseNode(allianceFlip(Community.chargingStationCorners[2] + .plus(new Translation2d(RobotConstants.DriveBase.kClearance, + -RobotConstants.DriveBase.kClearance))), + new Rotation2d()), + new PoseNode(Community.chargingStationCorners[0].plus( + new Translation2d(-RobotConstants.DriveBase.kClearance, + -RobotConstants.DriveBase.kClearance)), + new Rotation2d()), + new PoseNode(Community.chargingStationCorners[1].plus( + new Translation2d(-RobotConstants.DriveBase.kClearance, + RobotConstants.DriveBase.kClearance)), + new Rotation2d()), + new PoseNode(Community.chargingStationCorners[3] + .plus(new Translation2d(RobotConstants.DriveBase.kClearance, + RobotConstants.DriveBase.kClearance)), + new Rotation2d()), + new PoseNode(Community.chargingStationCorners[2] + .plus(new Translation2d(RobotConstants.DriveBase.kClearance, + -RobotConstants.DriveBase.kClearance)), + new Rotation2d())); + + /** + * Flips a translation to the correct side of the field based on the current + * alliance color. + * By default, all translations and poses in {@link FieldConstants} are stored + * with the + * origin at the rightmost point on the BLUE ALLIANCE wall. + */ + public static Translation2d allianceFlip(Translation2d translation) { + if (DriverStation.getAlliance() == Alliance.Red) { + return new Translation2d(fieldLength - translation.getX(), + translation.getY()); + } else { + return translation; } - - public static final List obstacles = List.of( - // add charging station of one side - new Obstacle(new Translation2d[] { obstacleCorners[0], obstacleCorners[1], - obstacleCorners[2], obstacleCorners[3] }), - new Obstacle(new Translation2d[] { allianceFlip(obstacleCorners[0]), - allianceFlip(obstacleCorners[1]), - allianceFlip(obstacleCorners[2]), allianceFlip(obstacleCorners[3]) })); - - public static List obstacleContingencyNodes = List.of( - new PoseNode( - allianceFlip(Community.chargingStationCorners[0].plus(new Translation2d( - -RobotConstants.DRIVEBASE_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE))), - new Rotation2d()), - new PoseNode( - allianceFlip(Community.chargingStationCorners[1].plus(new Translation2d( - -RobotConstants.DRIVEBASE_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE))), - new Rotation2d()), - new PoseNode( - allianceFlip(Community.chargingStationCorners[3].plus( - new Translation2d(RobotConstants.DRIVEBASE_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE))), - new Rotation2d()), - new PoseNode( - allianceFlip(Community.chargingStationCorners[2].plus(new Translation2d( - RobotConstants.DRIVEBASE_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE))), - new Rotation2d()), - new PoseNode( - Community.chargingStationCorners[0].plus(new Translation2d( - -RobotConstants.DRIVEBASE_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE)), - new Rotation2d()), - new PoseNode( - Community.chargingStationCorners[1].plus(new Translation2d( - -RobotConstants.DRIVEBASE_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE)), - new Rotation2d()), - new PoseNode( - Community.chargingStationCorners[3].plus( - new Translation2d(RobotConstants.DRIVEBASE_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE)), - new Rotation2d()), - new PoseNode( - Community.chargingStationCorners[2].plus(new Translation2d( - RobotConstants.DRIVEBASE_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE)), - new Rotation2d())); - - /** - * Flips a translation to the correct side of the field based on the current - * alliance color. - * By default, all translations and poses in {@link FieldConstants} are stored - * with the - * origin at the rightmost point on the BLUE ALLIANCE wall. - */ - public static Translation2d allianceFlip(Translation2d translation) { - if (DriverStation.getAlliance() == Alliance.Red) { - return new Translation2d(fieldLength - translation.getX(), - translation.getY()); - } else { - return translation; - } + } + + /** + * Flips a pose FIELD ORIENTED + */ + public static Pose2d allianceFlip(Pose2d pose) { + if (DriverStation.getAlliance() == Alliance.Red) { + return new Pose2d(fieldLength - pose.getX(), pose.getY(), + pose.getRotation().times(-1)); + } else { + return pose; } - - /** - * Flips a pose FIELD ORIENTED - */ - public static Pose2d allianceFlip(Pose2d pose) { - if (DriverStation.getAlliance() == Alliance.Red) { - return new Pose2d(fieldLength - pose.getX(), pose.getY(), pose.getRotation().times(-1)); - } else { - return pose; - } + } + + public static Pose2d allianceOrientedAllianceFlip(Pose2d pose) { + if (DriverStation.getAlliance() == Alliance.Red) { + return new Pose2d(fieldLength - pose.getX(), fieldWidth - pose.getY(), + pose.getRotation().times(-1)); + } else { + return pose; } + } - public static Pose2d allianceOrientedAllianceFlip(Pose2d pose) { - if (DriverStation.getAlliance() == Alliance.Red) { - return new Pose2d(fieldLength - pose.getX(), fieldWidth - pose.getY(), - pose.getRotation().times(-1)); - } else { - return pose; - } - } - public static Translation2d allianceOrientedAllianceFlip(Translation2d t) { - if (DriverStation.getAlliance() == Alliance.Red) { - return new Translation2d(fieldLength - t.getX(), fieldWidth - t.getY()); - } else { - return t; - } + public static Translation2d allianceOrientedAllianceFlip(Translation2d t) { + if (DriverStation.getAlliance() == Alliance.Red) { + return new Translation2d(fieldLength - t.getX(), fieldWidth - t.getY()); + } else { + return t; } + } - public static Rotation2d getRobotFacingRotation() { - return DriverStation.getAlliance() == Alliance.Red ? new Rotation2d() - : new Rotation2d(Math.PI); - } + public static Rotation2d getRobotFacingRotation() { + return DriverStation.getAlliance() == Alliance.Red ? new Rotation2d() + : new Rotation2d(Math.PI); + } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java b/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java index 9ff40010..bbdce974 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java +++ b/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java @@ -3,14 +3,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; + import frc.team670.mustanglib.commands.drive.teleop.SetSwerveForwardDirection; import frc.team670.mustanglib.commands.drive.teleop.XboxSwerveDrive; -import frc.team670.mustanglib.constants.OIBase; -import frc.team670.mustanglib.subsystems.MustangSubsystemBase; import frc.team670.mustanglib.utils.MustangController; import frc.team670.mustanglib.utils.MustangController.XboxButtons; import frc.team670.robot.commands.arm.ManualMoveElbow; -import frc.team670.robot.commands.arm.ManualMoveShoulder; import frc.team670.robot.commands.arm.MoveToTarget; import frc.team670.robot.commands.arm.ResetArmFromAbsolute; import frc.team670.robot.commands.claw.ClawIdle; @@ -26,119 +24,104 @@ import frc.team670.robot.subsystems.arm.Arm; import frc.team670.robot.subsystems.arm.ArmState; -public class OI extends OIBase { - - // Controllers - private static MustangController driverController = new MustangController(0); - private static MustangController operatorController = new MustangController(1); - - // Driver buttons - // private static JoystickButton zeroArm = new JoystickButton(driverController, - // XboxButtons.START); - private static JoystickButton zeroArm = new JoystickButton(operatorController, XboxButtons.START); - private static JoystickButton zeroGyroDriver = new JoystickButton(driverController, XboxButtons.START); - // private static JoystickButton moveToTarget = new - // JoystickButton(driverController, - // XboxButtons.RIGHT_BUMPER); - private static POVButton singleSubAlign = new POVButton(driverController, 0); - // private static JoystickButton creep = new JoystickButton(driverController, - // XboxButtons.RIGHT_TRIGGER); - // private static POVButton creep = new POVButton(driverController, 0); - private static POVButton alignToClosest = new POVButton(driverController, 180); - private static POVButton alignToLeft = new POVButton(driverController, 270); - private static POVButton alignToRight = new POVButton(driverController, 90); - - // private static POVButton alignToClosest = new POVButton(driverController, 0); - - // private static JoystickButton singleSubstation = new - // JoystickButton(driverController, 0) - - // Operator buttons - private static POVButton hybrid = new POVButton(operatorController, 180); - private static POVButton scoreMidR = new POVButton(operatorController, 90); - private static POVButton singleStation = new POVButton(operatorController, 270); - private static POVButton scoreHigh = new POVButton(operatorController, 0); - private static JoystickButton intakeShelf = new JoystickButton(operatorController, XboxButtons.X); - private static JoystickButton uprightGround = new JoystickButton(operatorController, XboxButtons.BACK); - - private static JoystickButton stow = new JoystickButton(operatorController, XboxButtons.B); - private static JoystickButton manualElbowControlPositive = new JoystickButton(operatorController, - XboxButtons.RIGHT_JOYSTICK_BUTTON); - private static JoystickButton manualElbowControlNegative = new JoystickButton(operatorController, - XboxButtons.LEFT_JOYSTICK_BUTTON); - - private static JoystickButton clawSuck = new JoystickButton(operatorController, XboxButtons.RIGHT_BUMPER); - private static JoystickButton clawEject = new JoystickButton(driverController, XboxButtons.LEFT_BUMPER); - private static JoystickButton clawIdle = new JoystickButton(operatorController, XboxButtons.LEFT_BUMPER); - - // Align to cardinal directions - private static JoystickButton rotateTo0 = new JoystickButton(driverController, XboxButtons.Y); - private static JoystickButton rotateTo90 = new JoystickButton(driverController, XboxButtons.X); - private static JoystickButton rotateTo180 = new JoystickButton(driverController, XboxButtons.A); - private static JoystickButton rotateTo270 = new JoystickButton(driverController, XboxButtons.B); - - // LED commands - private static JoystickButton ledYellow = new JoystickButton(operatorController, XboxButtons.Y); - private static JoystickButton ledPurple = new JoystickButton(operatorController, XboxButtons.A); - - public static MustangController getDriverController() { - return driverController; - } - - public static MustangController getOperatorController() { - return operatorController; - } - - @Override - public void configureButtonBindings(MustangSubsystemBase... subsystemBases) { - DriveBase driveBase = (DriveBase) subsystemBases[0]; - // Vision vision = (Vision) subsystemBases[1]; - Arm arm = (Arm) subsystemBases[2]; - Claw claw = (Claw) subsystemBases[3]; - LED led = (LED) subsystemBases[4]; - driveBase.initDefaultCommand(); - - zeroGyroDriver.onTrue(new SetSwerveForwardDirection(driveBase)); - zeroArm.onTrue(new ResetArmFromAbsolute(arm)); - // moveToTarget.whileTrue(new MoveToPose(driveBase, - // (FieldConstants.LoadingZone.IntakePoses[0]))); // moves to substation - // singleSubAlign.whileTrue(new MoveToPose(driveBase, - // (FieldConstants.allianceOrientedAllianceFlip(FieldConstants.LoadingZone.IntakePoses[0])))); - // // moves to substation - singleSubAlign.whileTrue(new AutoAlignToSubstation(driveBase, false)); // moves to - // substation - - alignToClosest.whileTrue(new AutoAlign(driveBase, AutoAlign.Direction.CLOSEST)); - alignToLeft.whileTrue(new AutoAlign(driveBase, AutoAlign.Direction.LEFT)); - alignToRight.whileTrue(new AutoAlign(driveBase, AutoAlign.Direction.RIGHT)); - - // creep.whileTrue(new Creep(driveBase)); - - // arm movement commands - hybrid.onTrue(new MoveToTarget(arm, ArmState.HYBRID)); - scoreMidR.onTrue(new MoveToTarget(arm, ArmState.SCORE_MID)); - singleStation.onTrue(new MoveToTarget(arm, claw, ArmState.SINGLE_STATION)); - scoreHigh.onTrue(new MoveToTarget(arm, ArmState.SCORE_HIGH)); - intakeShelf.onTrue(new MoveToTarget(arm, claw, ArmState.INTAKE_SHELF)); - uprightGround.onTrue(new MoveToTarget(arm, ArmState.UPRIGHT_GROUND)); - stow.onTrue(new MoveToTarget(arm, ArmState.STOWED)); - manualElbowControlNegative.onTrue(new ManualMoveElbow(arm, false)); - manualElbowControlPositive.onTrue(new ManualMoveElbow(arm, true)); - - // Claw control commands - clawSuck.onTrue(new ClawInstantIntake(claw)); - clawEject.onTrue(new EjectAndStow(claw, arm)); - clawIdle.onTrue(new ClawIdle(claw)); - - // Rotate to cardinal direction while driving - XboxSwerveDrive driveCommand = (XboxSwerveDrive) driveBase.getDefaultCommand(); - rotateTo0.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(0))); - rotateTo90.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(Math.PI / 2))); - rotateTo180.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(Math.PI))); - rotateTo270.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(3 * Math.PI / 2))); - - ledPurple.onTrue(new SetColorPurple(led)); - ledYellow.onTrue(new SetColorYellow(led)); - - } +public final class OI { + + // Controllers + private static MustangController driverController = new MustangController(0); + private static MustangController operatorController = new MustangController(1); + + // Driver buttons + private static JoystickButton zeroArm = + new JoystickButton(operatorController, XboxButtons.START); + private static JoystickButton zeroGyroDriver = + new JoystickButton(driverController, XboxButtons.START); + private static POVButton singleSubAlign = new POVButton(driverController, 0); + private static POVButton alignToClosest = new POVButton(driverController, 180); + private static POVButton alignToLeft = new POVButton(driverController, 270); + private static POVButton alignToRight = new POVButton(driverController, 90); + + // Operator buttons + private static POVButton hybrid = new POVButton(operatorController, 180); + private static POVButton scoreMidR = new POVButton(operatorController, 90); + private static POVButton singleStation = new POVButton(operatorController, 270); + private static POVButton scoreHigh = new POVButton(operatorController, 0); + private static JoystickButton intakeShelf = + new JoystickButton(operatorController, XboxButtons.X); + private static JoystickButton uprightGround = + new JoystickButton(operatorController, XboxButtons.BACK); + + private static JoystickButton stow = new JoystickButton(operatorController, XboxButtons.B); + private static JoystickButton manualElbowControlPositive = + new JoystickButton(operatorController, XboxButtons.RIGHT_JOYSTICK_BUTTON); + private static JoystickButton manualElbowControlNegative = + new JoystickButton(operatorController, XboxButtons.LEFT_JOYSTICK_BUTTON); + + private static JoystickButton clawSuck = + new JoystickButton(operatorController, XboxButtons.RIGHT_BUMPER); + private static JoystickButton clawEject = + new JoystickButton(driverController, XboxButtons.LEFT_BUMPER); + private static JoystickButton clawIdle = + new JoystickButton(operatorController, XboxButtons.LEFT_BUMPER); + + // Align to cardinal directions + private static JoystickButton rotateTo0 = new JoystickButton(driverController, XboxButtons.Y); + private static JoystickButton rotateTo90 = new JoystickButton(driverController, XboxButtons.X); + private static JoystickButton rotateTo180 = new JoystickButton(driverController, XboxButtons.A); + private static JoystickButton rotateTo270 = new JoystickButton(driverController, XboxButtons.B); + + // LED commands + private static JoystickButton ledYellow = new JoystickButton(operatorController, XboxButtons.Y); + private static JoystickButton ledPurple = new JoystickButton(operatorController, XboxButtons.A); + + public static MustangController getDriverController() { + return driverController; + } + + public static MustangController getOperatorController() { + return operatorController; + } + + public static void configureButtonBindings() { + DriveBase driveBase = DriveBase.getInstance(); + Arm arm = Arm.getInstance(); + Claw claw = Claw.getInstance(); + LED led = LED.getInstance(); + + driveBase.initDefaultCommand(new XboxSwerveDrive(driveBase, driverController)); + + zeroGyroDriver.onTrue(new SetSwerveForwardDirection(driveBase)); + zeroArm.onTrue(new ResetArmFromAbsolute(arm)); + singleSubAlign.whileTrue(new AutoAlignToSubstation(driveBase, false)); // moves to + // substation + + alignToClosest.whileTrue(new AutoAlign(driveBase, AutoAlign.Direction.CLOSEST)); + alignToLeft.whileTrue(new AutoAlign(driveBase, AutoAlign.Direction.LEFT)); + alignToRight.whileTrue(new AutoAlign(driveBase, AutoAlign.Direction.RIGHT)); + + // arm movement commands + hybrid.onTrue(new MoveToTarget(arm, ArmState.HYBRID)); + scoreMidR.onTrue(new MoveToTarget(arm, ArmState.SCORE_MID)); + singleStation.onTrue(new MoveToTarget(arm, claw, ArmState.SINGLE_STATION)); + scoreHigh.onTrue(new MoveToTarget(arm, ArmState.SCORE_HIGH)); + intakeShelf.onTrue(new MoveToTarget(arm, claw, ArmState.INTAKE_SHELF)); + uprightGround.onTrue(new MoveToTarget(arm, ArmState.UPRIGHT_GROUND)); + stow.onTrue(new MoveToTarget(arm, ArmState.STOWED)); + manualElbowControlNegative.onTrue(new ManualMoveElbow(arm, false)); + manualElbowControlPositive.onTrue(new ManualMoveElbow(arm, true)); + + // Claw control commands + clawSuck.onTrue(new ClawInstantIntake(claw)); + clawEject.onTrue(new EjectAndStow(claw, arm)); + clawIdle.onTrue(new ClawIdle(claw)); + + // Rotate to cardinal direction while driving + XboxSwerveDrive driveCommand = (XboxSwerveDrive) driveBase.getDefaultCommand(); + rotateTo0.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(0))); + rotateTo90.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(Math.PI / 2))); + rotateTo180.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(Math.PI))); + rotateTo270.onTrue(driveCommand.new SetDesiredHeading(new Rotation2d(3 * Math.PI / 2))); + + ledPurple.onTrue(new SetColorPurple(led)); + ledYellow.onTrue(new SetColorYellow(led)); + } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java index 73a25ee9..122aea27 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java +++ b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java @@ -5,25 +5,34 @@ package frc.team670.robot.constants; import static java.util.Map.entry; + import java.net.NetworkInterface; import java.net.SocketException; import java.util.Enumeration; +import java.util.List; import java.util.Map; +import java.util.Set; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.auto.PIDConstants; +import com.revrobotics.CANSparkMax.IdleMode; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SerialPort; -import frc.team670.mustanglib.constants.RobotConstantsBase; -import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper; +import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; +import frc.team670.mustanglib.subsystems.VisionSubsystemBase; +import frc.team670.mustanglib.subsystems.VisionSubsystemBase.TagCountDeviation; +import frc.team670.mustanglib.subsystems.VisionSubsystemBase.UnitDeviationParams; +import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; +import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper.GearRatio; import frc.team670.mustanglib.swervelib.ModuleConfiguration; import frc.team670.mustanglib.swervelib.SdsModuleConfigurations; -import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper.GearRatio; +import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; import frc.team670.robot.subsystems.arm.ArmSegment; /** @@ -37,276 +46,382 @@ * wherever the * constants are needed, to reduce verbosity. */ -public final class RobotConstants extends RobotConstantsBase { - - public static final String MAC_ADDRESS = getMACAddress(); - - /** - * Set your team number using the WPILib extension's "Set Team Number" action. - * 0) FACTORY RESET - * ALL MOTOR CONTROLLERS 1) Set all of the *_ANGLE_OFFSET constants to - * -Math.toRadians(0.0). 2) - * Deploy the code to your robot. Power cycle. NOTE: The robot isn't drivable quite yet, we - * still have to - * setup the module offsets 3) Turn the robot on its side and align all the - * wheels so they are - * facing in the forwards direction. NOTE: The wheels will be pointed forwards - * (not backwards) - * when modules are turned so the large bevel gears are towards the LEFT side of - * the robot. When - * aligning the wheels they must be as straight as possible. It is recommended - * to use a long - * strait edge such as a piece of 2x1 in order to make the wheels straight. 4) - * Record the angles - * of each module using the angle put onto Shuffleboard. The values are named - * Front Left Module - * Angle, Front Right Module Angle, etc. 5) Set the values of the *_ANGLE_OFFSET - * to - * -Math.toRadians() NOTE: All angles must be in - * degrees. 6) Re-deploy and power cycle - * and try to drive the robot forwards. All the wheels should stay parallel to - * each other. If - * not go back to step 3. 7) Make sure all the wheels are spinning in the - * correct direction. If - * not, add 180 degrees to the offset of each wheel that is spinning in the - * incorrect direction. - * i.e -Math.toRadians( + 180.0). - */ - public static Map> hardwareSpecificConstants = Map.ofEntries( - entry("00:80:2F:34:0B:07", Map.ofEntries( // Mac address from 670_SunTzu - entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(90.895)), - entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(299.807)), - entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(137.499)), - entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(318.604)), - entry("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL", 0.484233),//0.47777 - entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.004892), - entry("WRIST_ABSOLUTE_ENCODER_AT_VERTICAL", 0.337308), - entry("SHOULDER_GEAR_RATIO", 96.0), - entry("ELBOW_GEAR_RATIO", 70.833333333333), - entry("SWERVE_MODULE_CONFIGURATION", 2.0), - entry("WRIST_GEAR_RATIO", 125.0))), - entry("00:80:2F:33:D0:46", Map.ofEntries( // The mac address is from 670_2023 - entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(82.694)), - entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(233.29)), - entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(225.77)), - entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(112.53)), - entry("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL", 0.895), - // entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.922), - entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.588), - entry("WRIST_ABSOLUTE_ENCODER_AT_VERTICAL", 0.918), - entry("SHOULDER_GEAR_RATIO", 75.0), entry("ELBOW_GEAR_RATIO", 90.0), - entry("SWERVE_MODULE_CONFIGURATION", 1.0), - entry("WRIST_GEAR_RATIO", 125.0))), - entry("00:80:2F:22:B4:F6", Map.ofEntries()) // The mac address is from 670_WCD (test - // bench) +public final class RobotConstants { + /** + * Set your team number using the WPILib extension's "Set Team Number" action. + * 0) FACTORY RESET + * ALL MOTOR CONTROLLERS 1) Set all of the *_ANGLE_OFFSET constants to + * -Math.toRadians(0.0). 2) + * Deploy the code to your robot. Power cycle. NOTE: The robot isn't drivable + * quite yet, we + * still have to setup the module offsets 3) Turn the robot on its side and + * align all the wheels + * so they are facing in the forwards direction. NOTE: The wheels will be + * pointed forwards (not + * backwards) when modules are turned so the large bevel gears are towards the + * LEFT side of the + * robot. When aligning the wheels they must be as straight as possible. It is + * recommended to + * use a long straight edge such as a piece of 2x1 in order to make the wheels + * straight. 4) + * Record the angles of each module using the angle put onto Shuffleboard. The + * values are named + * Front Left Module Angle, Front Right Module Angle, etc. 5) Set the values of + * the + * *_ANGLE_OFFSET to -Math.toRadians() NOTE: All angles + * must be in + * degrees. 6) Re-deploy and power cycle and try to drive the robot forwards. + * All the wheels + * should stay parallel to each other. If not go back to step 3. 7) Make sure + * all the wheels are + * spinning in the correct direction. If not, add 180 degrees to the offset of + * each wheel that + * is spinning in the incorrect direction. i.e -Math.toRadians( + 180.0). + */ + + public static final String kSunTzuAddress = "00:80:2F:34:0B:07"; + public static final String kSkipperAddress = "00:80:2F:33:D0:46"; + public static final String kRobotAddress = getMACAddress(); + + public static Map robotSpecificConstants = Map.ofEntries( + entry(kSunTzuAddress, Map.ofEntries( + entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(90.895)), + entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(299.807)), + entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(137.499)), + entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(318.604)), + entry("kShoulderAbsoluteEncoderVerticalOffset", 0.484233), // 0.47777 + entry("kElbowAbsoluteEncoderVerticalOffset", 0.004892), + entry("kWristAbsoluteEncoderVerticalOffset", 0.174231), // 0.177702 + entry("kShoulderGearRatio", 96.0), entry("kElbowGearRatio", 70.833333333333), + entry("kSwerveModuleConfig", 2.0), entry("kWristGearRatio", 125.0))), + entry(kSkipperAddress,Map.ofEntries( + entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(84.023437)), + entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(232.734379)), + entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(22.939455)), + entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(134.281651)), + entry("kShoulderAbsoluteEncoderVerticalOffset", 0.895), + entry("kElbowAbsoluteEncoderVerticalOffset", 0.588), + entry("kWristAbsoluteEncoderVerticalOffset", 0.918), + entry("kShoulderGearRatio", 75.0), entry("kElbowGearRatio", 90.0), + entry("kSwerveModuleConfig", 1.0), entry("kWristGearRatio", 125.0)))) + .get(kRobotAddress); + + public static final class DriveBase { + public static final double kWidth = Units.inchesToMeters(36); + public static double kClearance = Math.hypot(kWidth, kWidth) / 2 + 0.05; + public static final double kTrackWidthMeters = 0.6096; + public static final double kWheelBaseMeters = 0.6096; + + public static final ModuleConfiguration kModuleConfig = robotSpecificConstants.get("kSwerveModuleConfig") == 1.0 + ? SdsModuleConfigurations.MK4I_L1 + : SdsModuleConfigurations.MK4I_L2; + + public static final GearRatio kSwerveModuleGearRatio = robotSpecificConstants.get("kSwerveModuleConfig") == 1.0 + ? GearRatio.L1 + : GearRatio.L2; + + public static final int kFrontLeftModuleSteerMotorID = 20; + public static final int kFrontLeftModuleDriveMotorID = 21; + public static final int kFrontLeftModuleSteerEncoderID = 30; + public static final double kFrontLeftModuleSteerOffsetRadians = robotSpecificConstants + .get("kFrontLeftModuleSteerOffsetRadians"); + + public static final int kFrontRightModuleSteerMotorID = 22; + public static final int kFrontRightModuleDriveMotorID = 23; + public static final int kFrontRightModuleSteerEncoderID = 32; + public static final double kFrontRightModuleSteerOffsetRadians = robotSpecificConstants + .get("kFrontRightModuleSteerOffsetRadians"); + + public static final int kBackRightModuleSteerMotorID = 24; + public static final int kBackRightModuleDriveMotorID = 25; + public static final int kBackRightModuleSteerEncoderID = 34; + public static final double kBackRightModuleSteerOffsetRadians = robotSpecificConstants + .get("kBackRightModuleSteerOffsetRadians"); + + public static final int kBackLeftModuleSteerMotorID = 26; + public static final int kBackLeftModuleDriveMotorID = 27; + public static final int kBackLeftModuleSteerEncoderID = 36; + public static final double kBackLeftModuleSteerOffsetRadians = robotSpecificConstants + .get("kBackLeftModuleSteerOffsetRadians"); + + + + public final static SerialPort.Port kNAVXPort = SerialPort.Port.kMXP; + public static final double kPitchOffset = 2; - ); - - /** - * The left-to-right distance between the drivetrain wheels - * - * Should be measured from center to center. - */ - - public static final double DRIVEBASE_WIDTH = Units.inchesToMeters(36); - public static double DRIVEBASE_CLEARANCE = Math.hypot(DRIVEBASE_WIDTH, DRIVEBASE_WIDTH) / 2 + 0.05; - - public static final double DRIVETRAIN_TRACKWIDTH_METERS = 0.6096; - - /** - * The front-to-back distance between the drivetrain wheels. - * - * Should be measured from center to center. - */ - public static final double DRIVETRAIN_WHEELBASE_METERS = 0.6096; - - public static final double LIMIT = 1.0; - - // Swerve Drivebase constants - public static final ModuleConfiguration SWERVE_MODULE_CONFIGURATION = hardwareSpecificConstants - .get(MAC_ADDRESS) - .get("SWERVE_MODULE_CONFIGURATION") == 1.0 ? SdsModuleConfigurations.MK4I_L1 - : SdsModuleConfigurations.MK4I_L2; - - public static final GearRatio SWERVE_GEAR_RATIO = hardwareSpecificConstants - .get(MAC_ADDRESS) - .get("SWERVE_MODULE_CONFIGURATION") == 1.0 ? GearRatio.L1 - : GearRatio.L2; - - public static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 25; - public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 24; - public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 34; - public static final double BACK_RIGHT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("BACK_RIGHT_MODULE_STEER_OFFSET"); - - public static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 27; - public static final int BACK_LEFT_MODULE_STEER_MOTOR = 26; - public static final int BACK_LEFT_MODULE_STEER_ENCODER = 36; - public static final double BACK_LEFT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("BACK_LEFT_MODULE_STEER_OFFSET"); - - public static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 23; - public static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 22; - public static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 32; - public static final double FRONT_RIGHT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("FRONT_RIGHT_MODULE_STEER_OFFSET"); - - public static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 21; - public static final int FRONT_LEFT_MODULE_STEER_MOTOR = 20; - public static final int FRONT_LEFT_MODULE_STEER_ENCODER = 30; - public static final double FRONT_LEFT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("FRONT_LEFT_MODULE_STEER_OFFSET"); - - public final static SerialPort.Port NAVX_PORT = SerialPort.Port.kMXP; public static final double kMaxSpeedMetersPerSecond = 2; public static final double kMaxAccelerationMetersPerSecondSquared = 1; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI * 4; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI * 8; - public static final double MAX_VELOCITY_METERS_PER_SECOND = 5676.0 / 60.0 - * SdsModuleConfigurations.MK4I_L1.getDriveReduction() - * SdsModuleConfigurations.MK4I_L1.getWheelDiameter() * Math.PI; - // TODO: TUNE PID CONTROLLERS AND + public static final double kMaxVoltage = 12.0; + public static final double kMaxDriveCurrent = 45.0; + public static final double kMaxSteerCurrent = 20.0; + + // The formula for calculating the theoretical maximum velocity is: + // / 60 * * * + // pi + // An example of this constant for a Mk4 L2 module with NEOs to drive is: + // 5880.0 / 60.0 / SdsModuleConfigurations.MK4_L2.getDriveReduction() * + // SdsModuleConfigurations.MK4_L2.getWheelDiameter() * Math.PI + public static final double kMaxVelocityMetersPerSecond = 5676.0 / 60.0 + * kModuleConfig.getDriveReduction() * kModuleConfig.getWheelDiameter() * Math.PI; + + public static final double kMaxAngularVelocityRadiansPerSecond = kMaxVelocityMetersPerSecond + / Math.hypot(kTrackWidthMeters / 2.0, kWheelBaseMeters / 2.0); + + public static final SwerveDrive.Config kConfig = new SwerveDrive.Config(kTrackWidthMeters, + kWheelBaseMeters, kMaxVelocityMetersPerSecond, kMaxVoltage, kMaxDriveCurrent, + kMaxSteerCurrent, kNAVXPort, kSwerveModuleGearRatio, kFrontLeftModuleDriveMotorID, + kFrontLeftModuleSteerMotorID, kFrontLeftModuleSteerEncoderID, + kFrontLeftModuleSteerOffsetRadians, kFrontRightModuleDriveMotorID, + kFrontRightModuleSteerMotorID, kFrontRightModuleSteerEncoderID, + kFrontRightModuleSteerOffsetRadians, kBackLeftModuleDriveMotorID, + kBackLeftModuleSteerMotorID, kBackLeftModuleSteerEncoderID, + kBackLeftModuleSteerOffsetRadians, kBackRightModuleDriveMotorID, + kBackRightModuleSteerMotorID, kBackRightModuleSteerEncoderID, + kBackRightModuleSteerOffsetRadians); + + public static final PIDConstants kAutonTranslationPID = new PIDConstants(4, 0, 0); + public static final PIDConstants kAutonThetaPID = new PIDConstants(0.5, 0, 0); + + // PID controllers public static final PIDController xController = new PIDController(3, 0, 0); public static final PIDController yController = new PIDController(3, 0, 0); public static final PIDController thetaController = new PIDController(0.2, 0, 0); + public static final PathConstraints kAutoPathConstraints = new PathConstraints( + kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); + } + + // vision + public static final class Vision { + public static final String[] kVisionCameraIDs = { "Arducam_B", "Arducam_C" }; + public static final Transform3d[] kCameraOffsets = { + // Cam B - RIGHT + new Transform3d( + new Translation3d(Units.inchesToMeters(0.56), Units.inchesToMeters(-5.25), + Units.inchesToMeters(19 + 5)), + new Rotation3d(0, 0, Units.degreesToRadians(-45))), + // Cam C - LEFT + new Transform3d( + new Translation3d(Units.inchesToMeters(0.56), Units.inchesToMeters(5.25), + Units.inchesToMeters(19 + 5)), + new Rotation3d(0, 0, Units.degreesToRadians(45))) }; + + public static final double kLockedOnErrorX = 0.3; + public static final double xLockedOnErrorY = 0.3; + public static final double kLockedOnErrorDegrees = 10; + + public static final double kPoseAmbiguityCutOff = 0.05; + public static final List> kPossibleFrameFIDCombos = List.of(Set.of(1, 2, 3, 4), + Set.of(5, 6, 7, 8)); + + public static final int kMaxFrameFIDs = 4; + public static final Map kVisionStdFromTagsSeen = Map.ofEntries( + Map.entry(1, new TagCountDeviation( + new UnitDeviationParams(.25, .4, .9), + new UnitDeviationParams(.35, .5, 1.2), + new UnitDeviationParams(.5, .7, 1.5))), + Map.entry(2, new TagCountDeviation( + new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5))), + Map.entry(3, new TagCountDeviation( + new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5))) + ); + + public static final AprilTagFieldLayout kFieldLayout = FieldConstants.getFieldLayout(FieldConstants.aprilTags); + public static final VisionSubsystemBase.Config kConfig = new VisionSubsystemBase.Config(kFieldLayout, kVisionCameraIDs, kCameraOffsets, kPoseAmbiguityCutOff, kMaxFrameFIDs, kPossibleFrameFIDCombos, kVisionStdFromTagsSeen); + } + + public static final class Arm { + + public static final class Shoulder { + public static final int kLeaderMotorID = 2; + public static final int kFollowerMotorID = 3; + public static final int kAbsoluteEncoderID = 0; + public static final double kAbsoluteEncoderVerticalOffset = RobotConstants.robotSpecificConstants + .get("kShoulderAbsoluteEncoderVerticalOffset"); + public static final int kGearRatio = (RobotConstants.robotSpecificConstants.get("kShoulderGearRatio")) + .intValue(); + public static final int kSoftLimitMin = 60; + public static final int kSoftLimitMax = 300; + public static final double kMaxOverrideDegrees = 15; + public static final double kLengthInches = 26; + public static final double kMassLb = 8.7; + public static final double kMassDistribution = 0.356; + public static final double kAllowedErrorDegrees= 0.2; + public static final double kAllowedErrorRotations = kGearRatio*kAllowedErrorDegrees/360; + + public static final ArmSegment kSegment = new ArmSegment(kLengthInches, kMassLb, kMassDistribution, + frc.team670.robot.subsystems.arm.Shoulder.SHOULDER_ARBITRARY_FF); + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + public static final double kP = 0.0005; + public static final double kI = 0; + public static final double kD = 0.00005; + public static final double kFF = 0.00017618; + public static final double kIz = 0; + + public static final MotorConfig.Motor_Type kMotorType = MotorConfig.Motor_Type.NEO; + public static final double kMaxOutput = 1; + public static final double kMinOutput = -1; + public static final double kMaxAcceleration = 2500; + //public static final float[] kSoftLimits = new float[] { (float) Units.degreesToRotations(kSoftLimitMax), (float) Units.degreesToRotations(kSoftLimitMin) }; + public static final float[] kSoftLimits = null; + public static final int kContinuousCurrent = 20; + public static final int kPeakCurrent = 60; + + public static final double kMaxRotatorRPM = 1500; + public static final double kMinRotatorRPM = 0; + + public static final SparkMaxRotatingSubsystem.Config kConfig = new SparkMaxRotatingSubsystem.Config( + kLeaderMotorID, 0, kMotorType, kIdleMode, + kGearRatio, kP, kI, kD, kFF, kIz, kMaxOutput, kMinOutput, + kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorRotations, + kSoftLimits, kContinuousCurrent, kPeakCurrent); + + } + + public static final class Elbow { + public static final double kAbsoluteEncoderVerticalOffset = RobotConstants.robotSpecificConstants + .get("kElbowAbsoluteEncoderVerticalOffset"); + public static final double kGearRatio = RobotConstants.robotSpecificConstants.get("kElbowGearRatio"); + public static final int kSoftLimitMin = 20; + public static final int kSoftLimitMax = 340; + public static final int kMaxOverrideDegreees = 15; + public static final double kAllowedErrorDegrees = 0.75; + public static final double kAllowedErrorRotations = kGearRatio*kAllowedErrorDegrees/360; + public static final double kLengthInches = 35; + public static final double kMassLb = 5.5; + public static final double kMassDistribution = 0.686; + public static final ArmSegment kSegment = new ArmSegment(kLengthInches, kMassLb, kMassDistribution, 0.5); + + public static final int kMotorID = 4; + public static final IdleMode kIdleMode = IdleMode.kBrake; + public static final int kAbsoluteEncoderID = 1; + + public static final double kP = 0.0007; + public static final double kI = 0; + public static final double kD = 0.00015; + public static final double kFF = 0.000176; + public static final double kIz = 0; + + public static final MotorConfig.Motor_Type kMotorType = MotorConfig.Motor_Type.NEO; + public static final double kMaxOutput = 1; + public static final double kMinOutput = -1; + public static final double kMaxAcceleration = 4000; + //public static final float[] kSoftLimits = new float[] { (float) Units.degreesToRotations(kSoftLimitMax),(float) Units.degreesToRotations(kSoftLimitMin) }; + public static final float[] kSoftLimits = null; + public static final int kContinuousCurrent = 20; + public static final int kPeakCurrent = 60; + + public static final double kMaxRotatorRPM = 3000; + public static final double kMinRotatorRPM = 0; + + public static final SparkMaxRotatingSubsystem.Config kConfig = new SparkMaxRotatingSubsystem.Config( + kMotorID, 0, kMotorType, kIdleMode, + kGearRatio, kP, kI, kD, kFF, kIz, kMaxOutput, kMinOutput, + kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorDegrees, + kSoftLimits, kContinuousCurrent, kPeakCurrent); + } + + public static final class Wrist { + + // Wrist + public static final double kAbsoluteEncoderVerticalOffset = RobotConstants.robotSpecificConstants + .get("kWristAbsoluteEncoderVerticalOffset"); + public static final double kGearRatio = (RobotConstants.robotSpecificConstants.get("kWristGearRatio")) + .intValue(); + public static final double kMaxOverrideDegrees = 90; + public static final double kAllowedErrorDegrees = 1.5; + public static final double kAllowedErrorRotations = kGearRatio* kAllowedErrorDegrees/360; + public static final ArmSegment kWristSegment = new ArmSegment(Claw.kLengthInches, Claw.kMassLB, + Claw.kMassDistribution, 0); + + public static final int kMotorID = 5; + public static final IdleMode kIdleMode = IdleMode.kBrake; + public static final int kAbsoluteEncoderID = 2; + + public static final double kP = 0.00011; + public static final double kI = 0; + public static final double kD = 0.00015; + public static final double kFF = 0.000176; + public static final double kIz = 0; + + public static final MotorConfig.Motor_Type kMotorType = MotorConfig.Motor_Type.NEO_550; + public static final double kMaxOutput = 1; + public static final double kMinOutput = -1; + public static final double kMaxAcceleration = 15000; + public static final float[] kSoftLimits = null; + public static final int kContinuousCurrent = 20; + public static final int kPeakCurrent = 20; + + public static final double kMaxRotatorRPM = 6000; + public static final double kMinRotatorRPM = 0; + + public static final SparkMaxRotatingSubsystem.Config kConfig = new SparkMaxRotatingSubsystem.Config( + kMotorID, 0, kMotorType, kIdleMode, + kGearRatio, kP, kI, kD, kFF, kIz, kMaxOutput, kMinOutput, + kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorRotations, + kSoftLimits, kContinuousCurrent, kPeakCurrent); - // Auton PID controllers - public static final PIDConstants AUTON_TRANSLATION_CONTROLLER = new PIDConstants(4, 0, 0); - public static final PIDConstants AUTON_THETA_CONTROLLER = new PIDConstants(0.5, 0, 0); - - // vision - - public static final String[] VISION_CAMERA_NAMES = { "Arducam_B", "Arducam_C" }; - // public static final String[] VISION_CAMERA_NAMES = - // {"Arducam_OV9281_USB_Camera", "Arducam_C"}; - public static final Transform3d[] CAMERA_OFFSETS = { - // Cam B - RIGHT - new Transform3d(new Translation3d(Units.inchesToMeters(0.56), - Units.inchesToMeters(-5.25), Units.inchesToMeters(19 + 5)), - new Rotation3d(0, 0, Units.degreesToRadians(-45))), - // Cam C - LEFT - new Transform3d(new Translation3d(Units.inchesToMeters(0.56), - Units.inchesToMeters(5.25), Units.inchesToMeters(19 + 5)), - new Rotation3d(0, 0, Units.degreesToRadians(45)))}; - - public static final double LOCKED_ON_ERROR_X = 0.3; // TODO: test what angles are - // appropriate - // for grabbing - public static final double LOCKED_ON_ERROR_Y = 0.3; - public static final double LOCKED_ON_ERROR_DEGREES = 10; - public static final double kTrackwidthMeters = 0.702; - - // Shoulder - public static final double SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL"); - public static final int SHOULDER_GEAR_RATIO = (hardwareSpecificConstants.get(MAC_ADDRESS) - .get("SHOULDER_GEAR_RATIO")).intValue(); - public static final int SHOULDER_SOFT_LIMIT_MIN = 60; - public static final int SHOULDER_SOFT_LIMIT_MAX = 300; - public static final double SHOULDER_ARBITRARY_FF = 0.5; - public static final double SHOULDER_MAX_OVERRIDE_DEGREES = 15; - - public static final double SHOULDER_LENGTH_INCHES = 26; - public static final double SHOULDER_TO_ELBOW_MASS_LB = 8.7; - public static final double SHOULDER_MASS_DISTRIBUTION = 0.356; - public static final double SHOULDER_ALLOWED_ERR_DEG = 0.2; - - // Elbow - public static final double ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL"); - public static final double ELBOW_GEAR_RATIO = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("ELBOW_GEAR_RATIO"); - - public static final int ELBOW_SOFT_LIMIT_MIN = 20; - public static final int ELBOW_SOFT_LIMIT_MAX = 340; - public static final int ELBOW_MAX_OVERRIDE_DEGREES = 15; - public static final double ELBOW_ALLOWED_ERR_DEG = 0.75; - // public static final double ELBOW_ARBITRARY_FF = 0.0; - public static final double ELBOW_ARBITRARY_FF = 0.5; - public static final double ELBOW_LENGTH_INCHES = 35; - public static final double ELBOW_TO_CLAW_MASS_LB = 5.5; // TODO: set - public static final double ELBOW_MASS_DISTRIBUTION = 0.686; // TODO: set - - // Wrist - public static final double WRIST_ABSOLUTE_ENCODER_AT_VERTICAL = hardwareSpecificConstants.get(MAC_ADDRESS) - .get("WRIST_ABSOLUTE_ENCODER_AT_VERTICAL"); - public static final double WRIST_GEAR_RATIO = (hardwareSpecificConstants.get(MAC_ADDRESS) - .get("WRIST_GEAR_RATIO")).intValue();; - public static final double WRIST_ARBITRARY_FF = 0; - public static final double WRIST_MAX_OVERRIDE_DEGREES = 90; - public static final double WRIST_ALLOWED_ERR_DEG = 1.5; - - public static final double CLAW_LENGTH_INCHES = 4; - public static final double CLAW_MASS_LB = 6.5; - public static final double CLAW_MASS_DISTRIBUTION = 1; - - public static final double CONE_MASS_LB = 1.4; - - // Gravity-cancelling constants - public static final ArmSegment SHOULDER_SEGMENT = new ArmSegment(SHOULDER_LENGTH_INCHES, - SHOULDER_TO_ELBOW_MASS_LB, SHOULDER_MASS_DISTRIBUTION, SHOULDER_ARBITRARY_FF); - public static final ArmSegment ELBOW_SEGMENT = new ArmSegment(ELBOW_LENGTH_INCHES, - ELBOW_TO_CLAW_MASS_LB, ELBOW_MASS_DISTRIBUTION, ELBOW_ARBITRARY_FF); - public static final ArmSegment WRIST_SEGMENT = new ArmSegment(CLAW_LENGTH_INCHES, CLAW_MASS_LB, - CLAW_MASS_DISTRIBUTION, WRIST_ARBITRARY_FF); - - // claw constants - public static final double CLAW_ROLLING_SPEED = 1.0; - public static final double CLAW_EJECTING_SPEED = -0.60; - public static final double CLAW_CURRENT_MAX = 23.0; - public static final double CLAW_IDLE_SPEED = 0.05; - public static final int CLAW_EJECT_ITERATIONS = 30; - public static final int CLAW_CURRENT_SPIKE_ITERATIONS = 25; - - public static final int kTimeoutMs = 0; - public static final double leftKsVolts = 0.4; - public static final double leftKvVoltSecondsPerMeter = 2.1; - public static final double leftKaVoltSecondsSquaredPerMeter = 0.15; - public static final double rightKsVolts = leftKsVolts; - public static final double rightKvVoltSecondsPerMeter = leftKvVoltSecondsPerMeter; - public static final double rightKaVoltSecondsSquaredPerMeter = leftKaVoltSecondsSquaredPerMeter; - - public static final double PITCH_OFFSET = 2; - - // Auto contraints for vision - public static final PathConstraints kAutoPathConstraints = new PathConstraints(kMaxSpeedMetersPerSecond, - kMaxAccelerationMetersPerSecondSquared); - - /** - * This is code from Poofs 2022 - * - * @return the MAC address of the robot - */ - public static String getMACAddress() { - try { - Enumeration nwInterface = NetworkInterface.getNetworkInterfaces(); - StringBuilder ret = new StringBuilder(); - while (nwInterface.hasMoreElements()) { - NetworkInterface nis = nwInterface.nextElement(); - System.out.println("NIS: " + nis.getDisplayName()); - if (nis != null && "eth0".equals(nis.getDisplayName())) { - byte[] mac = nis.getHardwareAddress(); - if (mac != null) { - for (int i = 0; i < mac.length; i++) { - ret.append(String.format("%02X%s", mac[i], - (i < mac.length - 1) ? ":" : "")); - } - String addr = ret.toString(); - System.out.println("NIS " + nis.getDisplayName() + " addr: " + addr); - return addr; - } else { - System.out.println("Address doesn't exist or is not accessible"); - } - } else { - System.out.println("Skipping adaptor: " + nis.getDisplayName()); - } + } + + public static final class Claw { + + public static final int kMotorID = 6; + public static final double kLengthInches = 4; + public static final double kMassLB = 6.5; + public static final double kMassDistribution = 1; + public static final double kRollingSpeed = 1.0; + public static final double kEjectingSpeed = -0.60; + public static final double kCurrentMax = 23.0; + public static final double kIdleSpeed = 0.05; + public static final int kEjectIterations = 30; + public static final int kCurrentSpikeIterations = 25; + } + } + + public static final class Led { + public static final int kPort = 0; + public static final int kStartIndex = 0; + public static final int kEndindex = 61; + } + + /** + * This is code from Poofs 2022 + * + * @return the MAC address of the robot + */ + public static String getMACAddress() { + try { + Enumeration nwInterface = NetworkInterface.getNetworkInterfaces(); + StringBuilder ret = new StringBuilder(); + while (nwInterface.hasMoreElements()) { + NetworkInterface nis = nwInterface.nextElement(); + System.out.println("NIS: " + nis.getDisplayName()); + if (nis != null && "eth0".equals(nis.getDisplayName())) { + byte[] mac = nis.getHardwareAddress(); + if (mac != null) { + for (int i = 0; i < mac.length; i++) { + ret.append(String.format("%02X%s", mac[i], + (i < mac.length - 1) ? ":" : "")); } - } catch (SocketException | NullPointerException e) { - e.printStackTrace(); + String addr = ret.toString(); + System.out.println("NIS " + nis.getDisplayName() + " addr: " + addr); + return addr; + } else { + System.out.println("Address doesn't exist or is not accessible"); + } + } else { + System.out.println("Skipping adaptor: " + nis.getDisplayName()); } - System.out.println("\n\nMAC ADDRESS NOTHING"); - return ""; + } + } catch (SocketException | NullPointerException e) { + e.printStackTrace(); } + System.out.println("\n\nMAC ADDRESS NOTHING"); + return ""; + } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java deleted file mode 100644 index 42e9d094..00000000 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.team670.robot.constants; - -public class RobotMap { - - public static final int PDP_ID = 0; - - // Drive Base - public static final int SPARK_LEFT_MOTOR_1 = 20; // These are properly set. - public static final int SPARK_LEFT_MOTOR_2 = 21; - public static final int SPARK_RIGHT_MOTOR_1 = 22; - public static final int SPARK_RIGHT_MOTOR_2 = 23; - - // Arm - public static final int SHOULDER_LEADER_MOTOR = 2; - public static final int SHOULDER_FOLLOWER_MOTOR = 3; - public static final int SHOULDER_ABSOLUTE_ENCODER = 0; - public static final int ELBOW_MOTOR = 4; - public static final int ELBOW_ABSOLUTE_ENCODER = 1; - public static final int WRIST_MOTOR = 5; - public static final int WRIST_ABSOLUTE_ENCODER = 2; - public static final int CLAW_MOTOR = 6; - - // Joysticks - public static final int DRIVER_CONTROLLER_PORT = 0; - public static final int OPERATOR_CONTROLLER_PORT = 1; - - public static final int LED_PORT = 0; - - - - - -} diff --git a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/Obstacle.java b/2023-Robot/src/main/java/frc/team670/robot/pathfinder/Obstacle.java deleted file mode 100644 index fda0f895..00000000 --- a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/Obstacle.java +++ /dev/null @@ -1,1079 +0,0 @@ -package frc.team670.robot.pathfinder; - -import java.util.ArrayList; -import java.util.List; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import java.awt.Point; -import java.awt.Polygon; -import java.awt.Rectangle; -import java.awt.Shape; -import java.awt.geom.AffineTransform; -import java.awt.geom.GeneralPath; -import java.awt.geom.Line2D; -import java.awt.geom.PathIterator; -import java.awt.geom.Point2D; -import java.awt.geom.Rectangle2D; -import java.io.Serializable; - -/** - * Polygon represnting an obstacle. Credits to Hemlock. - * @author ethan c Dx - */ -public class Obstacle { - PolygonDouble polygon; - - public Obstacle(double[] xPoints, double[] yPoints) { - this.polygon = new PolygonDouble(xPoints, yPoints); - } - - public Obstacle(Translation2d[] corners) { - double[] xPoints = new double[corners.length]; - double[] yPoints = new double[corners.length]; - - for (int i = 0; i < corners.length; i++) { - xPoints[i] = corners[i].getX(); - yPoints[i] = corners[i].getY(); - } - this.polygon = new PolygonDouble(xPoints, yPoints); - } - - public void addNodes(ObstacleAvoidanceAStarMap nodes) { - for (int i = 0; i < polygon.npoints; i++) { - nodes.addNode(new PoseNode(polygon.xpoints[i], polygon.ypoints[i])); - } - } - - public double[] getXPoints() { - return this.polygon.xpoints; - } - - public double[] getYPoints() { - return this.polygon.ypoints; - } - - public Translation2d[] getCorners() { - Translation2d[] corners = new Translation2d[polygon.npoints]; - for (int i = 0; i < polygon.npoints; i++) - corners[i] = new Translation2d(this.polygon.xpoints[i], this.polygon.ypoints[i]); - return corners; - } - - /** - * Creates a polygon that's offset by the distance passed in. - * - * Makes a copy of each point of the polygon, offset by a vector at 90deg from the angle of the - * edges, then connects them together. - * - * Has functionality in place to prevent issues with concave shapes having overlapping lines and - * other weirdness. - * - * @param distance Distance to expand the shape outwards by. - * @return New obstacle, which has the distance passed in added to all sides. - */ - public Obstacle offset(double distance) { - // Get a list of all edges with the offsets added onto them - List offsetEdges = new ArrayList<>(); - for (int i = 0; i < polygon.npoints; i++) { - offsetEdges.add(new ObstacleEdge(polygon.xpoints[i], polygon.ypoints[i], - polygon.xpoints[(i + 1) % polygon.npoints], - polygon.ypoints[(i + 1) % polygon.npoints]).offset(distance)); - } - List xPoints = new ArrayList<>(); - List yPoints = new ArrayList<>(); - - // Loop through all edges, checking if there's an intersection between any of them. - // If an intersection does occur, cut the point off at that intersection, - // Creating a new point at the intersection, which should be the correct distance away. - for (int i = 0; i < offsetEdges.size(); i++) { - ObstacleEdge edge = offsetEdges.get(i); - // Check against every other edge, including last -> first - for (int j = i + 1; j <= offsetEdges.size(); j++) { - // Wrap edges back to beginning so the last edge can be checked against the first - // one - ObstacleEdge otherEdge = offsetEdges.get(j % offsetEdges.size()); - Translation2d intersectionPoint = edge.findIntersectionPoint(otherEdge); - if (intersectionPoint == null && !edge.hasBeenPlotted()) { - // If lines don't intersect, and the edge hasn't been plotted out already - if (!edge.hasBeenCleaned()) { - // If edge was cleaned from a previous loop, don't add the first point - xPoints.add(offsetEdges.get(i).point1.getX()); - yPoints.add(offsetEdges.get(i).point1.getY()); - } - xPoints.add(offsetEdges.get(i).point2.getX()); - yPoints.add(offsetEdges.get(i).point2.getY()); - edge.setHasBeenPlotted(true); - } else { - // If lines do intersect - if (!edge.hasBeenPlotted()) { - // Don't duplicate points - if (!edge.hasBeenCleaned()) { - // If edge was cleaned from a previous loop, don't add the first point - xPoints.add(offsetEdges.get(i).point1.getX()); - yPoints.add(offsetEdges.get(i).point1.getY()); - } - xPoints.add(intersectionPoint.getX()); - yPoints.add(intersectionPoint.getY()); - otherEdge.setHasBeenCleaned(true); - edge.setHasBeenPlotted(true); - } - } - } - } - - double[] xArr = new double[xPoints.size()]; - double[] yArr = new double[yPoints.size()]; - - for (int i = 0; i < xArr.length; i++) - xArr[i] = xPoints.get(i); - for (int i = 0; i < yArr.length; i++) - yArr[i] = yPoints.get(i); - - - return new Obstacle(xArr, yArr); - } - - public String toString() { - String output = "Polygon(\n"; - for (int i = 0; i < polygon.npoints; i++) { - output += Double.toString(Math.round(polygon.xpoints[i] * 100) / 100.0) + ", "; - output += Double.toString(Math.round(polygon.ypoints[i] * 100) / 100.0) + "\n"; - } - return output + ")"; - } - - private class ObstacleEdge { - private Translation2d point1; - private Translation2d point2; - private boolean hasBeenCleaned = false; - private boolean hasBeenPlotted = false; - - public ObstacleEdge(double x1, double y1, double x2, double y2) { - point1 = new Translation2d(x1, y1); - point2 = new Translation2d(x2, y2); - } - - public ObstacleEdge(Translation2d point1, Translation2d point2) { - this.point1 = point1; - this.point2 = point2; - } - - public ObstacleEdge offset(double distance) { - // Calculate angle of edge - Rotation2d angle = point2.minus(point1).getAngle(); - // Calculate perpendicular angle - Rotation2d transformAngle = angle.plus(Rotation2d.fromDegrees(90)); - // Create offset vector using distance and angles - Translation2d offset = new Translation2d(distance, 0).rotateBy(transformAngle); - // Add offset to points - return new ObstacleEdge(point1.plus(offset), point2.plus(offset)); - } - - /** - * This is heavily based on an algorithm in the "Tricks of the Windows Game Programming - * Gurus" book by Andre LeMothe - * - * @param other - * @return - */ - public Translation2d findIntersectionPoint(ObstacleEdge other) { - // Calculate x distance of line 1 - double s1_x = this.point2.getX() - this.point1.getX(); - // Calculate x distance of line 2 - double s2_x = other.point2.getX() - other.point1.getX(); - // Calculate y distance of line 1 - double s1_y = this.point2.getY() - this.point1.getY(); - // Calculate y distance of line 2 - double s2_y = other.point2.getY() - other.point1.getY(); - - double s, t; - // Denominator portion of below equations, split into variable because it's the same - // between the two - double d = -s2_x * s1_y + s1_x * s2_y; - - // Magical math that I need to look into how it works more - s = (-s1_y * (this.point1.getX() - other.point1.getX()) - + s1_x * (this.point1.getY() - other.point1.getY())) / d; - t = (s2_x * (this.point1.getY() - other.point1.getY()) - - s2_y * (this.point1.getX() - other.point1.getX())) / d; - - double i_x, i_y; - if (s >= 0 && s <= 1 && t >= 0 && t <= 1) { - // Intersection found - i_x = this.point1.getX() + (t * s1_x); - i_y = this.point1.getY() + (t * s1_y); - return new Translation2d(i_x, i_y); - } - return null; - - } - - public String toString() { - return point1.toString() + "," + point2.toString(); - } - - public boolean hasBeenCleaned() { - return hasBeenCleaned; - } - - public void setHasBeenCleaned(boolean hasBeenCleaned) { - this.hasBeenCleaned = hasBeenCleaned; - } - - public boolean hasBeenPlotted() { - return hasBeenPlotted; - } - - public void setHasBeenPlotted(boolean hasBeenPlotted) { - this.hasBeenPlotted = hasBeenPlotted; - } - } - - /* - * - * Licensed to the Apache Software Foundation (ASF) under one or more contributor license - * agreements. See the NOTICE file distributed with this work for additional information - * regarding copyright ownership. The ASF licenses this file to You under the Apache License, - * Version 2.0 (the "License"); you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software distributed under the - * License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, - * either express or implied. See the License for the specific language governing permissions - * and limitations under the License. - * - */ - - - - /** - * This class is a Polygon with double coordinates. - * - * @version $Id: Polygon2D.java 594018 2007-11-12 04:17:41Z cam $ - */ - public class PolygonDouble implements Shape, Cloneable, Serializable { - - /** - * The total number of points. The value of npoints represents the number of - * valid points in this Polygon. - */ - public int npoints; - - /** - * The array of x coordinates. The value of {@link #npoints npoints} is equal to the - * number of points in this Polygon2D. - */ - public double[] xpoints; - - /** - * The array of x coordinates. The value of {@link #npoints npoints} is equal to the - * number of points in this Polygon2D. - */ - public double[] ypoints; - - /** - * Bounds of the Polygon2D. - * - * @see #getBounds() - */ - protected Rectangle2D bounds; - - private GeneralPath path; - private GeneralPath closedPath; - - /** - * Creates an empty Polygon2D. - */ - public PolygonDouble() { - xpoints = new double[4]; - ypoints = new double[4]; - } - - /** - * Constructs and initializes a Polygon2D from the specified Rectangle2D. - * - * @param rec the Rectangle2D - * @throws NullPointerException rec is null. - */ - public PolygonDouble(Rectangle2D rec) { - if (rec == null) { - throw new IndexOutOfBoundsException("null Rectangle"); - } - npoints = 4; - xpoints = new double[4]; - ypoints = new double[4]; - xpoints[0] = rec.getMinX(); - ypoints[0] = rec.getMinY(); - xpoints[1] = rec.getMaxX(); - ypoints[1] = rec.getMinY(); - xpoints[2] = rec.getMaxX(); - ypoints[2] = rec.getMaxY(); - xpoints[3] = rec.getMinX(); - ypoints[3] = rec.getMaxY(); - calculatePath(); - } - - /** - * Constructs and initializes a Polygon2D from the specified Polygon. - * - * @param pol the Polygon - * @throws NullPointerException pol is null. - */ - public PolygonDouble(Polygon pol) { - if (pol == null) { - throw new IndexOutOfBoundsException("null Polygon"); - } - this.npoints = pol.npoints; - this.xpoints = new double[pol.npoints]; - this.ypoints = new double[pol.npoints]; - for (int i = 0; i < pol.npoints; i++) { - xpoints[i] = pol.xpoints[i]; - ypoints[i] = pol.ypoints[i]; - } - calculatePath(); - } - - /** - * Constructs and initializes a Polygon2D from the specified parameters. - * - * @param xpoints an array of x coordinates - * @param ypoints an array of y coordinates - * @param npoints the total number of points in the Polygon2D - * @throws NegativeArraySizeException if the value of npoints is negative. - * @throws IndexOutOfBoundsException if npoints is greater than the length of - * xpoints or the length of ypoints. - * @throws NullPointerException if xpoints or ypoints is - * null. - */ - public PolygonDouble(double[] xpoints, double[] ypoints) { - if (xpoints.length != ypoints.length) { - throw new IndexOutOfBoundsException( - "npoints > xpoints.length || npoints > ypoints.length"); - } - this.npoints = xpoints.length; - this.xpoints = new double[npoints]; - this.ypoints = new double[npoints]; - System.arraycopy(xpoints, 0, this.xpoints, 0, npoints); - System.arraycopy(ypoints, 0, this.ypoints, 0, npoints); - calculatePath(); - } - - /** - * Constructs and initializes a Polygon2D from the specified parameters. - * - * @param xpoints an array of x coordinates - * @param ypoints an array of y coordinates - * @param npoints the total number of points in the Polygon2D - * @throws NegativeArraySizeException if the value of npoints is negative. - * @throws IndexOutOfBoundsException if npoints is greater than the length of - * xpoints or the length of ypoints. - * @throws NullPointerException if xpoints or ypoints is - * null. - */ - public PolygonDouble(int[] xpoints, int[] ypoints, int npoints) { - if (npoints > xpoints.length || npoints > ypoints.length) { - throw new IndexOutOfBoundsException( - "npoints > xpoints.length || npoints > ypoints.length"); - } - this.npoints = npoints; - this.xpoints = new double[npoints]; - this.ypoints = new double[npoints]; - for (int i = 0; i < npoints; i++) { - this.xpoints[i] = xpoints[i]; - this.ypoints[i] = ypoints[i]; - } - calculatePath(); - } - - /** - * Resets this Polygon object to an empty polygon. - */ - public void reset() { - npoints = 0; - bounds = null; - path = new GeneralPath(); - closedPath = null; - } - - public Object clone() { - PolygonDouble pol = new PolygonDouble(); - for (int i = 0; i < npoints; i++) { - pol.addPoint(xpoints[i], ypoints[i]); - } - return pol; - } - - private void calculatePath() { - path = new GeneralPath(); - path.moveTo(xpoints[0], ypoints[0]); - for (int i = 1; i < npoints; i++) { - path.lineTo(xpoints[i], ypoints[i]); - } - bounds = path.getBounds2D(); - closedPath = null; - } - - private void updatePath(double x, double y) { - closedPath = null; - if (path == null) { - path = new GeneralPath(GeneralPath.WIND_EVEN_ODD); - path.moveTo(x, y); - bounds = new Rectangle2D.Double(x, y, 0, 0); - } else { - path.lineTo(x, y); - double _xmax = bounds.getMaxX(); - double _ymax = bounds.getMaxY(); - double _xmin = bounds.getMinX(); - double _ymin = bounds.getMinY(); - if (x < _xmin) - _xmin = x; - else if (x > _xmax) - _xmax = x; - if (y < _ymin) - _ymin = y; - else if (y > _ymax) - _ymax = y; - bounds = new Rectangle2D.Double(_xmin, _ymin, _xmax - _xmin, _ymax - _ymin); - } - } - - /* - * get the associated {@link Polyline2D}. - */ - public Polyline2D getPolyline2D() { - - Polyline2D pol = new Polyline2D(xpoints, ypoints, npoints); - - pol.addPoint(xpoints[0], ypoints[0]); - - return pol; - } - - public Polygon getPolygon() { - int[] _xpoints = new int[npoints]; - int[] _ypoints = new int[npoints]; - for (int i = 0; i < npoints; i++) { - _xpoints[i] = (int) xpoints[i]; // todo maybe rounding is better ? - _ypoints[i] = (int) ypoints[i]; - } - - return new Polygon(_xpoints, _ypoints, npoints); - } - - public void addPoint(Point2D p) { - addPoint(p.getX(), p.getY()); - } - - /** - * Appends the specified coordinates to this Polygon2D. - * - * @param x the specified x coordinate - * @param y the specified y coordinate - */ - public void addPoint(double x, double y) { - if (npoints == xpoints.length) { - double[] tmp; - - tmp = new double[npoints * 2]; - System.arraycopy(xpoints, 0, tmp, 0, npoints); - xpoints = tmp; - - tmp = new double[npoints * 2]; - System.arraycopy(ypoints, 0, tmp, 0, npoints); - ypoints = tmp; - } - xpoints[npoints] = x; - ypoints[npoints] = y; - npoints++; - updatePath(x, y); - } - - /** - * Determines whether the specified {@link Point} is inside this Polygon. - * - * @param p the specified Point to be tested - * @return true if the Polygon contains the Point; - * false otherwise. - * @see #contains(double, double) - */ - public boolean contains(Point p) { - return contains(p.x, p.y); - } - - /** - * Determines whether the specified coordinates are inside this Polygon. - *

- * - * @param x the specified x coordinate to be tested - * @param y the specified y coordinate to be tested - * @return true if this Polygon contains the specified - * coordinates, (x, y); false otherwise. - */ - public boolean contains(int x, int y) { - return contains(x, (double) y); - } - - /** - * Returns the high precision bounding box of the {@link Shape}. - * - * @return a {@link Rectangle2D} that precisely bounds the Shape. - */ - public Rectangle2D getBounds2D() { - return bounds; - } - - public Rectangle getBounds() { - if (bounds == null) - return null; - else - return bounds.getBounds(); - } - - /** - * Determines if the specified coordinates are inside this Polygon. For the - * definition of insideness, see the class comments of {@link Shape}. - * - * @param x the specified x coordinate - * @param y the specified y coordinate - * @return true if the Polygon contains the specified coordinates; - * false otherwise. - */ - public boolean contains(double x, double y) { - if (npoints <= 2 || !bounds.contains(x, y)) { - return false; - } - updateComputingPath(); - - return closedPath.contains(x, y); - } - - private void updateComputingPath() { - if (npoints >= 1) { - if (closedPath == null) { - closedPath = (GeneralPath) path.clone(); - closedPath.closePath(); - } - } - } - - /** - * Tests if a specified {@link Point2D} is inside the boundary of this Polygon. - * - * @param p a specified Point2D - * @return true if this Polygon contains the specified - * Point2D; false otherwise. - * @see #contains(double, double) - */ - public boolean contains(Point2D p) { - return contains(p.getX(), p.getY()); - } - - /** - * Tests if the interior of this Polygon intersects the interior of a specified - * set of rectangular coordinates. - * - * @param x the x coordinate of the specified rectangular shape's top-left corner - * @param y the y coordinate of the specified rectangular shape's top-left corner - * @param w the width of the specified rectangular shape - * @param h the height of the specified rectangular shape - * @return true if the interior of this Polygon and the interior - * of the specified set of rectangular coordinates intersect each other; - * false otherwise. - */ - public boolean intersects(double x, double y, double w, double h) { - if (npoints <= 0 || !bounds.intersects(x, y, w, h)) { - return false; - } - updateComputingPath(); - return closedPath.intersects(x, y, w, h); - } - - /** - * Tests if the interior of this Polygon intersects the interior of a specified - * Rectangle2D. - * - * @param r a specified Rectangle2D - * @return true if this Polygon and the interior of the specified - * Rectangle2D intersect each other; false otherwise. - */ - public boolean intersects(Rectangle2D r) { - return intersects(r.getX(), r.getY(), r.getWidth(), r.getHeight()); - } - - /** - * Tests if the interior of this Polygon entirely contains the specified set of - * rectangular coordinates. - * - * @param x the x coordinate of the top-left corner of the specified set of rectangular - * coordinates - * @param y the y coordinate of the top-left corner of the specified set of rectangular - * coordinates - * @param w the width of the set of rectangular coordinates - * @param h the height of the set of rectangular coordinates - * @return true if this Polygon entirely contains the specified - * set of rectangular coordinates; false otherwise. - */ - public boolean contains(double x, double y, double w, double h) { - if (npoints <= 0 || !bounds.intersects(x, y, w, h)) { - return false; - } - - updateComputingPath(); - return closedPath.contains(x, y, w, h); - } - - /** - * Tests if the interior of this Polygon entirely contains the specified - * Rectangle2D. - * - * @param r the specified Rectangle2D - * @return true if this Polygon entirely contains the specified - * Rectangle2D; false otherwise. - * @see #contains(double, double, double, double) - */ - public boolean contains(Rectangle2D r) { - return contains(r.getX(), r.getY(), r.getWidth(), r.getHeight()); - } - - /** - * Returns an iterator object that iterates along the boundary of this Polygon - * and provides access to the geometry of the outline of this Polygon. An - * optional {@link AffineTransform} can be specified so that the coordinates returned in the - * iteration are transformed accordingly. - * - * @param at an optional AffineTransform to be applied to the coordinates as - * they are returned in the iteration, or null if untransformed - * coordinates are desired - * @return a {@link PathIterator} object that provides access to the geometry of this - * Polygon. - */ - public PathIterator getPathIterator(AffineTransform at) { - updateComputingPath(); - if (closedPath == null) - return null; - else - return closedPath.getPathIterator(at); - } - - /** - * Returns an iterator object that iterates along the boundary of the Polygon2D - * and provides access to the geometry of the outline of the Shape. Only - * SEG_MOVETO, SEG_LINETO, and SEG_CLOSE point types are returned by the iterator. Since - * polygons are already flat, the flatness parameter is ignored. - * - * @param at an optional AffineTransform to be applied to the coordinates as - * they are returned in the iteration, or null if untransformed - * coordinates are desired - * @param flatness the maximum amount that the control points for a given curve can vary - * from colinear before a subdivided curve is replaced by a straight line connecting - * the endpoints. Since polygons are already flat the flatness parameter - * is ignored. - * @return a PathIterator object that provides access to the Shape - * object's geometry. - */ - public PathIterator getPathIterator(AffineTransform at, double flatness) { - return getPathIterator(at); - } - } - - /* - * - * Licensed to the Apache Software Foundation (ASF) under one or more contributor license - * agreements. See the NOTICE file distributed with this work for additional information - * regarding copyright ownership. The ASF licenses this file to You under the Apache License, - * Version 2.0 (the "License"); you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software distributed under the - * License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, - * either express or implied. See the License for the specific language governing permissions - * and limitations under the License. - * - */ - - - /** - * This class has the same behavior than {@link Polygon2D}, except that the figure is not - * closed. - * - * @version $Id: Polyline2D.java 594018 2007-11-12 04:17:41Z cam $ - */ - class Polyline2D implements Shape, Cloneable, Serializable { - - private static final double ASSUME_ZERO = 0.001f; - - /** - * The total number of points. The value of npoints represents the number of - * points in this Polyline2D. - */ - public int npoints; - - /** - * The array of x coordinates. The value of {@link #npoints npoints} is equal to the - * number of points in this Polyline2D. - */ - public double[] xpoints; - - /** - * The array of x coordinates. The value of {@link #npoints npoints} is equal to the - * number of points in this Polyline2D. - */ - public double[] ypoints; - - /** - * Bounds of the Polyline2D. - * - * @see #getBounds() - */ - protected Rectangle2D bounds; - - private GeneralPath path; - private GeneralPath closedPath; - - /** - * Creates an empty Polyline2D. - */ - public Polyline2D() { - xpoints = new double[4]; - ypoints = new double[4]; - } - - /** - * Constructs and initializes a Polyline2D from the specified parameters. - * - * @param xpoints an array of x coordinates - * @param ypoints an array of y coordinates - * @param npoints the total number of points in the Polyline2D - * @throws NegativeArraySizeException if the value of npoints is negative. - * @throws IndexOutOfBoundsException if npoints is greater than the length of - * xpoints or the length of ypoints. - * @throws NullPointerException if xpoints or ypoints is - * null. - */ - public Polyline2D(double[] xpoints, double[] ypoints, int npoints) { - if (npoints > xpoints.length || npoints > ypoints.length) { - throw new IndexOutOfBoundsException( - "npoints > xpoints.length || npoints > ypoints.length"); - } - this.npoints = npoints; - this.xpoints = new double[npoints + 1]; // make space for one more to close the polyline - this.ypoints = new double[npoints + 1]; // make space for one more to close the polyline - System.arraycopy(xpoints, 0, this.xpoints, 0, npoints); - System.arraycopy(ypoints, 0, this.ypoints, 0, npoints); - calculatePath(); - } - - /** - * Constructs and initializes a Polyline2D from the specified parameters. - * - * @param xpoints an array of x coordinates - * @param ypoints an array of y coordinates - * @param npoints the total number of points in the Polyline2D - * @throws NegativeArraySizeException if the value of npoints is negative. - * @throws IndexOutOfBoundsException if npoints is greater than the length of - * xpoints or the length of ypoints. - * @throws NullPointerException if xpoints or ypoints is - * null. - */ - public Polyline2D(int[] xpoints, int[] ypoints, int npoints) { - if (npoints > xpoints.length || npoints > ypoints.length) { - throw new IndexOutOfBoundsException( - "npoints > xpoints.length || npoints > ypoints.length"); - } - this.npoints = npoints; - this.xpoints = new double[npoints]; - this.ypoints = new double[npoints]; - for (int i = 0; i < npoints; i++) { - this.xpoints[i] = xpoints[i]; - this.ypoints[i] = ypoints[i]; - } - calculatePath(); - } - - public Polyline2D(Line2D line) { - npoints = 2; - xpoints = new double[2]; - ypoints = new double[2]; - xpoints[0] = line.getX1(); - xpoints[1] = line.getX2(); - ypoints[0] = line.getY1(); - ypoints[1] = line.getY2(); - calculatePath(); - } - - /** - * Resets this Polyline2D object to an empty polygon. The coordinate arrays and - * the data in them are left untouched but the number of points is reset to zero to mark the - * old vertex data as invalid and to start accumulating new vertex data at the beginning. - * All internally-cached data relating to the old vertices are discarded. Note that since - * the coordinate arrays from before the reset are reused, creating a new empty - * Polyline2D might be more memory efficient than resetting the current one if - * the number of vertices in the new polyline data is significantly smaller than the number - * of vertices in the data from before the reset. - */ - public void reset() { - npoints = 0; - bounds = null; - path = new GeneralPath(); - closedPath = null; - } - - public Object clone() { - Polyline2D pol = new Polyline2D(); - for (int i = 0; i < npoints; i++) { - pol.addPoint(xpoints[i], ypoints[i]); - } - return pol; - } - - private void calculatePath() { - path = new GeneralPath(); - path.moveTo(xpoints[0], ypoints[0]); - for (int i = 1; i < npoints; i++) { - path.lineTo(xpoints[i], ypoints[i]); - } - bounds = path.getBounds2D(); - closedPath = null; - } - - private void updatePath(double x, double y) { - closedPath = null; - if (path == null) { - path = new GeneralPath(GeneralPath.WIND_EVEN_ODD); - path.moveTo(x, y); - bounds = new Rectangle2D.Double(x, y, 0, 0); - } else { - path.lineTo(x, y); - double _xmax = bounds.getMaxX(); - double _ymax = bounds.getMaxY(); - double _xmin = bounds.getMinX(); - double _ymin = bounds.getMinY(); - if (x < _xmin) - _xmin = x; - else if (x > _xmax) - _xmax = x; - if (y < _ymin) - _ymin = y; - else if (y > _ymax) - _ymax = y; - bounds = new Rectangle2D.Double(_xmin, _ymin, _xmax - _xmin, _ymax - _ymin); - } - } - - public void addPoint(Point2D p) { - addPoint(p.getX(), p.getY()); - } - - /** - * Appends the specified coordinates to this Polyline2D. - *

- * If an operation that calculates the bounding box of this Polyline2D has - * already been performed, such as getBounds or contains, then - * this method updates the bounding box. - * - * @param x the specified x coordinate - * @param y the specified y coordinate - * @see java.awt.Polygon#getBounds - * @see java.awt.Polygon#contains(double, double) - */ - public void addPoint(double x, double y) { - if (npoints == xpoints.length) { - double[] tmp; - - tmp = new double[npoints * 2]; - System.arraycopy(xpoints, 0, tmp, 0, npoints); - xpoints = tmp; - - tmp = new double[npoints * 2]; - System.arraycopy(ypoints, 0, tmp, 0, npoints); - ypoints = tmp; - } - xpoints[npoints] = x; - ypoints[npoints] = y; - npoints++; - updatePath(x, y); - } - - /** - * Gets the bounding box of this Polyline2D. The bounding box is the smallest - * {@link Rectangle} whose sides are parallel to the x and y axes of the coordinate space, - * and can completely contain the Polyline2D. - * - * @return a Rectangle that defines the bounds of this Polyline2D. - */ - public Rectangle getBounds() { - if (bounds == null) - return null; - else - return bounds.getBounds(); - } - - private void updateComputingPath() { - if (npoints >= 1) { - if (closedPath == null) { - closedPath = (GeneralPath) path.clone(); - closedPath.closePath(); - } - } - } - - /** - * Determines whether the specified {@link Point} is inside this Polyline2D. - * This method is required to implement the Shape interface, but in the case of Line2D - * objects it always returns false since a line contains no area. - */ - public boolean contains(Point p) { - return false; - } - - /** - * Determines if the specified coordinates are inside this Polyline2D. This - * method is required to implement the Shape interface, but in the case of Line2D objects it - * always returns false since a line contains no area. - */ - public boolean contains(double x, double y) { - return false; - } - - /** - * Determines whether the specified coordinates are inside this Polyline2D. - * This method is required to implement the Shape interface, but in the case of Line2D - * objects it always returns false since a line contains no area. - */ - public boolean contains(int x, int y) { - return false; - } - - /** - * Returns the high precision bounding box of the {@link Shape}. - * - * @return a {@link Rectangle2D} that precisely bounds the Shape. - */ - public Rectangle2D getBounds2D() { - return bounds; - } - - /** - * Tests if a specified {@link Point2D} is inside the boundary of this - * Polyline2D. This method is required to implement the Shape interface, but in - * the case of Line2D objects it always returns false since a line contains no area. - */ - public boolean contains(Point2D p) { - return false; - } - - /** - * Tests if the interior of this Polygon intersects the interior of a specified - * set of rectangular coordinates. - * - * @param x the x coordinate of the specified rectangular shape's top-left corner - * @param y the y coordinate of the specified rectangular shape's top-left corner - * @param w the width of the specified rectangular shape - * @param h the height of the specified rectangular shape - * @return true if the interior of this Polygon and the interior - * of the specified set of rectangular coordinates intersect each other; - * false otherwise. - */ - public boolean intersects(double x, double y, double w, double h) { - if (npoints <= 0 || !bounds.intersects(x, y, w, h)) { - return false; - } - updateComputingPath(); - return closedPath.intersects(x, y, w, h); - } - - /** - * Tests if the interior of this Polygon intersects the interior of a specified - * Rectangle2D. - * - * @param r a specified Rectangle2D - * @return true if this Polygon and the interior of the specified - * Rectangle2D intersect each other; false otherwise. - */ - public boolean intersects(Rectangle2D r) { - return intersects(r.getX(), r.getY(), r.getWidth(), r.getHeight()); - } - - /** - * Tests if the interior of this Polyline2D entirely contains the specified set - * of rectangular coordinates. This method is required to implement the Shape interface, but - * in the case of Line2D objects it always returns false since a line contains no area. - */ - public boolean contains(double x, double y, double w, double h) { - return false; - } - - /** - * Tests if the interior of this Polyline2D entirely contains the specified - * Rectangle2D. This method is required to implement the Shape interface, but - * in the case of Line2D objects it always returns false since a line contains no area. - */ - public boolean contains(Rectangle2D r) { - return false; - } - - /** - * Returns an iterator object that iterates along the boundary of this Polygon - * and provides access to the geometry of the outline of this Polygon. An - * optional {@link AffineTransform} can be specified so that the coordinates returned in the - * iteration are transformed accordingly. - * - * @param at an optional AffineTransform to be applied to the coordinates as - * they are returned in the iteration, or null if untransformed - * coordinates are desired - * @return a {@link PathIterator} object that provides access to the geometry of this - * Polygon. - */ - public PathIterator getPathIterator(AffineTransform at) { - if (path == null) - return null; - else - return path.getPathIterator(at); - } - - /* - * get the associated {@link Polygon2D}. This method take care that may be the last point - * can be equal to the first. In that case it must not be included in the Polygon, as - * polygons declare their first point only once. - */ - public PolygonDouble getPolygon2D() { - PolygonDouble pol = new PolygonDouble(); - for (int i = 0; i < npoints - 1; i++) { - pol.addPoint(xpoints[i], ypoints[i]); - } - Point2D.Double p0 = new Point2D.Double(xpoints[0], ypoints[0]); - Point2D.Double p1 = new Point2D.Double(xpoints[npoints - 1], ypoints[npoints - 1]); - - if (p0.distance(p1) > ASSUME_ZERO) - pol.addPoint(xpoints[npoints - 1], ypoints[npoints - 1]); - - return pol; - } - - /** - * Returns an iterator object that iterates along the boundary of the Shape and - * provides access to the geometry of the outline of the Shape. Only SEG_MOVETO - * and SEG_LINETO, point types are returned by the iterator. Since polylines are already - * flat, the flatness parameter is ignored. - * - * @param at an optional AffineTransform to be applied to the coordinates as - * they are returned in the iteration, or null if untransformed - * coordinates are desired - * @param flatness the maximum amount that the control points for a given curve can vary - * from colinear before a subdivided curve is replaced by a straight line connecting - * the endpoints. Since polygons are already flat the flatness parameter - * is ignored. - * @return a PathIterator object that provides access to the Shape - * object's geometry. - */ - public PathIterator getPathIterator(AffineTransform at, double flatness) { - return path.getPathIterator(at); - } - } - -} diff --git a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/ObstacleAvoidanceAStarMap.java b/2023-Robot/src/main/java/frc/team670/robot/pathfinder/ObstacleAvoidanceAStarMap.java deleted file mode 100644 index d910d88c..00000000 --- a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/ObstacleAvoidanceAStarMap.java +++ /dev/null @@ -1,125 +0,0 @@ -package frc.team670.robot.pathfinder; - -import java.awt.geom.Line2D; -import java.util.ArrayList; -import java.util.List; -import com.fasterxml.jackson.databind.ext.OptionalHandlerFactory; -import frc.team670.mustanglib.utils.math.sort.AStarSearch; -import frc.team670.robot.pathfinder.Obstacle.PolygonDouble; - -/** - * @author ethan c :D - */ -public class ObstacleAvoidanceAStarMap { - - private final AStarSearch searchAlg = new AStarSearch<>(); - private List contingencyNodes = new ArrayList<>(); - private final List edges = new ArrayList<>(); - private final List obstacles = new ArrayList<>(); - private PoseNode startNode, endNode; - - public ObstacleAvoidanceAStarMap(PoseNode start, PoseNode destination, List obstacles) { - this.startNode = start; - this.endNode = destination; - addObstacles(obstacles); - } - - public ObstacleAvoidanceAStarMap(PoseNode start, PoseNode destination, List obstacles, List obstacleContingencyNodes) { - this.startNode = start; - this.endNode = destination; - this.contingencyNodes = obstacleContingencyNodes; - addObstacles(obstacles); - } - - // Add a node to the navigation mesh - public void addNode(PoseNode node) { - this.contingencyNodes.add(node); - } - - public void addNodes(List nodes) { - this.contingencyNodes.addAll(nodes); - } - - public int getNodeSize() { - return contingencyNodes.size(); - } - - public PoseNode getNode(int index) { - return contingencyNodes.get(index); - } - - public List findPath() { - List fullPath = new ArrayList<>(); - if (intersectsObstacles(new PoseEdge(startNode, endNode))) { - System.out.println("intersects obstacle!"); - loadMap(); - fullPath = searchAlg.search(startNode, endNode); - } else { - System.out.println("no intersection"); - fullPath.add(startNode); - fullPath.add(endNode); - } - return fullPath; - } - - public void addObstacles(List obstacles) { - this.obstacles.addAll(obstacles); - } - - // Add edges to nodes it doesn't intersect obstacles - private void loadMap() { - PoseEdge startToContingency; - for (PoseNode node : contingencyNodes) { - startToContingency = new PoseEdge(startNode, node); - if (!intersectsObstacles(startToContingency)) { - this.edges.add(startToContingency); - startToContingency.start.addNeighbor(startToContingency.end); - startToContingency.end.addNeighbor(startToContingency.start); - } - } - PoseEdge contingencyToEnd; - for (PoseNode node : contingencyNodes) { - contingencyToEnd = new PoseEdge(node, endNode); - if (!intersectsObstacles(contingencyToEnd)) { - this.edges.add(contingencyToEnd); - contingencyToEnd.start.addNeighbor(contingencyToEnd.end); - contingencyToEnd.end.addNeighbor(contingencyToEnd.start); - } - } - PoseEdge contTocont; - for (PoseNode node : contingencyNodes) { - for (PoseNode other : contingencyNodes) { - if (node == other) continue; - - contTocont = new PoseEdge(node, other); - if(!intersectsObstacles(contTocont)) { - this.edges.add(contTocont); - contTocont.start.addNeighbor(contTocont.end); - contTocont.end.addNeighbor(contTocont.start); - } - } - } - } - - private boolean intersectsObstacles(PoseEdge edge) { - for (Obstacle obstacle : obstacles) { - PolygonDouble polygon = obstacle.polygon; - for (int i = 0; i < polygon.npoints; i++) { - int j = (i + 1) % polygon.npoints; - double x1 = polygon.xpoints[i]; - double y1 = polygon.ypoints[i]; - double x2 = polygon.xpoints[j]; - double y2 = polygon.ypoints[j]; - if (Line2D.linesIntersect(x1, y1, x2, y2, edge.start.getX(), edge.start.getY(), - edge.end.getX(), edge.end.getY())) { - return true; // if line from start to final interesects with any obstacle lines - } - } - } - return false; - } - - public List getEdges() { - return edges; - } -} diff --git a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseEdge.java b/2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseEdge.java deleted file mode 100644 index 809f958b..00000000 --- a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseEdge.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.team670.robot.pathfinder; - -import frc.team670.mustanglib.utils.math.sort.Edge; - -/** - * Edge of two pose nodes. Inspired by Hemlock 5712 - * @author ethan c :P - */ -public class PoseEdge implements Edge { - public PoseNode start, end; - private double cost; - - public PoseEdge(PoseNode start, PoseNode end, double cost) { - this.start = start; - this.end = end; - this.cost = cost; - } - - public PoseEdge(PoseNode start, PoseNode end) { - this.start = start; - this.end = end; - this.cost = start.getHeuristicDistance(end); - } - - @Override - public double getCost() { - return cost; - } - - @Override - public PoseNode getSource() { - return start; - } - - @Override - public PoseNode getDest() { - return end; - } -} diff --git a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseNode.java b/2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseNode.java deleted file mode 100644 index 12dc9797..00000000 --- a/2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseNode.java +++ /dev/null @@ -1,65 +0,0 @@ -package frc.team670.robot.pathfinder; - -import java.util.ArrayList; -import java.util.List; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.team670.mustanglib.utils.math.sort.Node; - -/** - * Node representing Pose. Inspired by Hemlock 5712 - * @author ethan c ;) - */ -public class PoseNode implements Node { - public Pose2d pose; - private List neighbors; - - public PoseNode(double x, double y) { - this.pose = new Pose2d(x, y, Rotation2d.fromDegrees(0)); - this.neighbors = new ArrayList<>(); - } - - public PoseNode(double x, double y, Rotation2d holonomicRotation) { - this.pose = new Pose2d(x, y, Rotation2d.fromDegrees(0)); - this.neighbors = new ArrayList<>(); - } - - public PoseNode(Pose2d currentPose) { - this.pose = currentPose; - this.neighbors = new ArrayList<>(); - } - - public PoseNode(Translation2d coordinates, Rotation2d holonomicRotation) { - this.pose = new Pose2d(coordinates, holonomicRotation); - this.neighbors = new ArrayList<>(); - } - - - public double getX() { - return pose.getX(); - } - - public double getY() { - return pose.getY(); - } - - public Rotation2d getHolRot() { - return pose.getRotation(); - } - - @Override - public void addNeighbor(PoseNode neighbor) { - this.neighbors.add(neighbor); - } - - @Override - public double getHeuristicDistance(PoseNode target) { - return this.pose.getTranslation().getDistance(target.pose.getTranslation()); - } - - @Override - public List getNeighbors() { - return neighbors; - } -} diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java index 384aa581..2eb18b56 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java @@ -1,14 +1,11 @@ package frc.team670.robot.subsystems; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; +import frc.team670.mustanglib.subsystems.LEDSubsystem.LEDColor; import frc.team670.mustanglib.utils.motorcontroller.MotorConfig.Motor_Type; -import frc.team670.mustanglib.utils.LEDColor; import frc.team670.mustanglib.utils.motorcontroller.SparkMAXFactory; import frc.team670.mustanglib.utils.motorcontroller.SparkMAXLite; -import frc.team670.robot.constants.OI; import frc.team670.robot.constants.RobotConstants; -import frc.team670.robot.constants.RobotMap; - import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.wpilibj.DriverStation; @@ -29,18 +26,27 @@ public enum Status { private int currentSpikeCounter = 0; private int ejectCounter = 0; private boolean isFull = false; - private double ejectingSpeed = RobotConstants.CLAW_EJECTING_SPEED; + private double ejectingSpeed = + RobotConstants.Arm.Claw.kEjectingSpeed; private LED led; - public Claw(LED led) { - motor = SparkMAXFactory.buildSparkMAX(RobotMap.CLAW_MOTOR, SparkMAXFactory.defaultConfig, Motor_Type.NEO); + private static Claw mInstance; + + public static synchronized Claw getInstance() { + mInstance = mInstance == null ? new Claw() : mInstance; + return mInstance; + } + + public Claw() { + motor = SparkMAXFactory.buildSparkMAX( + RobotConstants.Arm.Claw.kMotorID, + SparkMAXFactory.defaultConfig, Motor_Type.NEO); status = Status.IDLE; - this.led = led; + this.led = LED.getInstance(); motor.setInverted(true); motor.setIdleMode(IdleMode.kBrake); - } public LED getLed() { @@ -56,14 +62,13 @@ public boolean isFull() { } public void startEjecting() { - this.startEjecting(RobotConstants.CLAW_EJECTING_SPEED); + this.startEjecting(RobotConstants.Arm.Claw.kEjectingSpeed); } /** * Ejects the held item at the given speed * - * @param ejectingSpeed Should be <0. The more negative, the faster the claw - * will run backwards. + * @param ejectingSpeed Should be <0. The more negative, the faster the claw will run backwards. */ public void startEjecting(double ejectingSpeed) { this.ejectingSpeed = ejectingSpeed; @@ -81,8 +86,7 @@ public void startIntaking() { } /** - * Sets the claw to an IDLE state - * Please note that "IDLE" does not mean "stopped"! + * Sets the claw to an IDLE state Please note that "IDLE" does not mean "stopped"! */ public void setIdle() { setStatus(Status.IDLE); @@ -113,13 +117,14 @@ public void mustangPeriodic() { switch (status) { case IDLE: - motor.set(RobotConstants.CLAW_IDLE_SPEED); + motor.set(RobotConstants.Arm.Claw.kIdleSpeed); break; case INTAKING: - motor.set(RobotConstants.CLAW_ROLLING_SPEED); - if (motor.getOutputCurrent() > RobotConstants.CLAW_CURRENT_MAX) { + motor.set(RobotConstants.Arm.Claw.kRollingSpeed); + if (motor + .getOutputCurrent() > RobotConstants.Arm.Claw.kCurrentMax) { currentSpikeCounter++; - if (currentSpikeCounter > RobotConstants.CLAW_CURRENT_SPIKE_ITERATIONS) { + if (currentSpikeCounter > RobotConstants.Arm.Claw.kCurrentSpikeIterations) { isFull = true; if (DriverStation.isTeleopEnabled()) { led.solidhsv(LEDColor.GREEN); @@ -138,7 +143,7 @@ public void mustangPeriodic() { case EJECTING: motor.set(ejectingSpeed); ejectCounter++; - if (ejectCounter > RobotConstants.CLAW_EJECT_ITERATIONS) { + if (ejectCounter > RobotConstants.Arm.Claw.kEjectIterations) { ejectCounter = 0; isFull = false; if (DriverStation.isTeleopEnabled()) { @@ -157,4 +162,4 @@ public void debugSubsystem() { SmartDashboard.putString(clawStateKey, status.toString()); } -} \ No newline at end of file +} diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java index ee72b77f..3a537630 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java @@ -4,140 +4,58 @@ package frc.team670.robot.subsystems; -import com.pathplanner.lib.PathPlannerTrajectory; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.commands.MustangScheduler; import frc.team670.mustanglib.commands.drive.teleop.XboxSwerveDrive; -import frc.team670.mustanglib.constants.SwerveConfig; import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; -import frc.team670.mustanglib.swervelib.SdsModuleConfigurations; import frc.team670.mustanglib.swervelib.SwerveModule; -import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand; import frc.team670.mustanglib.utils.MustangController; import frc.team670.robot.constants.RobotConstants; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class DriveBase extends SwerveDrive { - private final String pitchKey = "pitch"; - - /** - * The maximum voltage that will be delivered to the drive motors. This can be reduced to cap - * the robot's maximum speed. Typically, this is useful during initial testing of the robot. - */ - public static final double MAX_VOLTAGE = 12.0; + private static DriveBase mInstance; + private MustangCommand defaultCommand; + private MustangController mController; + public static synchronized DriveBase getInstance() { + mInstance = mInstance == null ? new DriveBase() : mInstance; + return mInstance; + } - // The formula for calculating the theoretical maximum velocity is: - // / 60 * * * - // pi - // An example of this constant for a Mk4 L2 module with NEOs to drive is: - // 5880.0 / 60.0 / SdsModuleConfigurations.MK4_L2.getDriveReduction() * - // SdsModuleConfigurations.MK4_L2.getWheelDiameter() * Math.PI - /** - * The maximum velocity of the robot in meters per second. This is a measure of how fast the - * robot should be able to drive in a straight line. - */ - public static final double MAX_VELOCITY_METERS_PER_SECOND = 5676.0 / 60.0 - * RobotConstants.SWERVE_MODULE_CONFIGURATION.getDriveReduction() - * RobotConstants.SWERVE_MODULE_CONFIGURATION.getWheelDiameter() * Math.PI; + public DriveBase() { + super(RobotConstants.DriveBase.kConfig); + } - /** - * The maximum angular velocity of the robot in radians per second. This is a measure of how - * fast the robot can rotate in place.` - */ - public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = - MAX_VELOCITY_METERS_PER_SECOND - / Math.hypot(RobotConstants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, - RobotConstants.DRIVETRAIN_WHEELBASE_METERS / 2.0); - private MustangCommand defaultCommand; - private MustangController mController; - - public DriveBase(MustangController mustangController) { - super(new SwerveConfig(RobotConstants.DRIVETRAIN_TRACKWIDTH_METERS, - RobotConstants.DRIVETRAIN_WHEELBASE_METERS, MAX_VELOCITY_METERS_PER_SECOND, - MAX_VOLTAGE, RobotConstants.NAVX_PORT, RobotConstants.SWERVE_GEAR_RATIO, - RobotConstants.FRONT_LEFT_MODULE_DRIVE_MOTOR, - RobotConstants.FRONT_LEFT_MODULE_STEER_MOTOR, - RobotConstants.FRONT_LEFT_MODULE_STEER_ENCODER, - RobotConstants.FRONT_LEFT_MODULE_STEER_OFFSET, - RobotConstants.FRONT_RIGHT_MODULE_DRIVE_MOTOR, - RobotConstants.FRONT_RIGHT_MODULE_STEER_MOTOR, - RobotConstants.FRONT_RIGHT_MODULE_STEER_ENCODER, - RobotConstants.FRONT_RIGHT_MODULE_STEER_OFFSET, - RobotConstants.BACK_LEFT_MODULE_DRIVE_MOTOR, - RobotConstants.BACK_LEFT_MODULE_STEER_MOTOR, - RobotConstants.BACK_LEFT_MODULE_STEER_ENCODER, - RobotConstants.BACK_LEFT_MODULE_STEER_OFFSET, - RobotConstants.BACK_RIGHT_MODULE_DRIVE_MOTOR, - RobotConstants.BACK_RIGHT_MODULE_STEER_MOTOR, - RobotConstants.BACK_RIGHT_MODULE_STEER_ENCODER, - RobotConstants.BACK_RIGHT_MODULE_STEER_OFFSET)); - this.mController = mustangController; - } - - /** - * Makes the DriveBase's default command initialize teleop - */ - public void initDefaultCommand() { // TODO: switch to super class's init default command + public void initDefaultCommand(DriveBase driveBase, MustangController controller) { // TODO: switch to super class's init default command // defaultCommand = new XboxSwerveDrive(this, mController, // MAX_VELOCITY_METERS_PER_SECOND, // MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND); - defaultCommand = new XboxSwerveDrive(this, mController, MAX_VELOCITY_METERS_PER_SECOND, - MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND); + defaultCommand = new XboxSwerveDrive(driveBase, mController); MustangScheduler.getInstance().setDefaultCommand(this, defaultCommand); } - public void cancelDefaultCommand() { - MustangScheduler.getInstance().cancel(defaultCommand); - } - - public MustangCommand getDefaultMustangCommand() { - return defaultCommand; - } - - public void mustangPeriodic() { - super.mustangPeriodic(); - SmartDashboard.putNumber(pitchKey, getPitch()); - } - - // @Override - // public void periodic() { - // return; - // } + public void mustangPeriodic() { + super.mustangPeriodic(); + SmartDashboard.putNumber("pitch", getPitch()); + } - @Override - public HealthState checkHealth() { - for (SwerveModule curr : getModules()) { - CANSparkMax motor = (CANSparkMax) curr.getDriveMotor(); - if (motor.getLastError() != REVLibError.kOk) { - SmartDashboard.putString("Swerve Module " + motor.getDeviceId() + " ERROR:", - motor.getLastError().toString()); - return HealthState.RED; - } + @Override + public HealthState checkHealth() { + for (SwerveModule curr : getModules()) { + CANSparkMax motor = (CANSparkMax) curr.getDriveMotor(); + if (motor.getLastError() != REVLibError.kOk) { + SmartDashboard.putString("Swerve Module " + motor.getDeviceId() + " ERROR:", + motor.getLastError().toString()); + return HealthState.RED; } - return HealthState.GREEN; - } - - @Override - public void debugSubsystem() { + } + return HealthState.GREEN; + } - } - - public MustangPPSwerveControllerCommand getFollowTrajectoryCommand( - PathPlannerTrajectory traj) { - setSwerveControllerCommand(new MustangPPSwerveControllerCommand(traj, this::getPose, - getSwerveKinematics(), RobotConstants.xController, - RobotConstants.yController, RobotConstants.thetaController, - this::setModuleStates, new Subsystem[] {this})); - return getSwerveControllerCommand(); - - } + @Override + public void debugSubsystem() {} } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/LED.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/LED.java index e8812a33..283e6413 100755 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/LED.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/LED.java @@ -1,27 +1,28 @@ package frc.team670.robot.subsystems; -import edu.wpi.first.wpilibj.DriverStation; import frc.team670.mustanglib.subsystems.LEDSubsystem; -import frc.team670.mustanglib.utils.LEDColor; -import frc.team670.mustanglib.utils.Logger; +import frc.team670.robot.constants.RobotConstants; public class LED extends LEDSubsystem { - + private static LED mInstance; private LEDColor allianceColor; + private int prevPath = -1; - public LED(int port, int startIndex, int endIndex) { - super(port, startIndex, endIndex); + public static synchronized LED getInstance() { + mInstance = mInstance == null ? new LED() : mInstance; + return mInstance; + } + public LED() { + super(RobotConstants.Led.kPort, RobotConstants.Led.kStartIndex, RobotConstants.Led.kEndindex); } - public void setColorPurple() { - // Logger.consoleLog("LED COPLOR BEING SET TO PURPLE IN LED SUBSYSTEM - // Pokjipguhygxdtfyu08tfyui[u/hyfygyuhGy/HIYUg'uhyG?SaJHKVGUYITDFTIGFGYF> timeDelays = Map.ofEntries( entry(ArmState.STOWED, Map.ofEntries( // From STOWED - entry(ArmState.SCORE_HIGH, new double[] { 300, 0, 250 }), - entry(ArmState.SCORE_MID, new double[] { 300, 0, 250 }), - entry(ArmState.HYBRID, new double[] { 0, 0, 0 }))), + entry(ArmState.SCORE_HIGH, new double[] {300, 0, 250}), + entry(ArmState.SCORE_MID, new double[] {300, 0, 250}), + entry(ArmState.HYBRID, new double[] {0, 0, 0}))), entry(ArmState.HYBRID, Map.ofEntries( // From HYBRID - entry(ArmState.STOWED, new double[] { 0, 250, 0 }), - entry(ArmState.SCORE_MID, new double[] { 0, 150, 400 }), - entry(ArmState.INTAKE_SHELF, new double[] { 0, 150, 400 }))), + entry(ArmState.STOWED, new double[] {0, 250, 0}), + entry(ArmState.SCORE_MID, new double[] {0, 150, 400}), + entry(ArmState.INTAKE_SHELF, new double[] {0, 150, 400}))), entry(ArmState.SCORE_MID, Map.ofEntries( // From SCORE_MID - entry(ArmState.STOWED, new double[] { 0, 250, 0 }), - entry(ArmState.HYBRID, new double[] { 0, 250, 0 }), - entry(ArmState.SCORE_HIGH, new double[] { 250, 0, 250 }))), + entry(ArmState.STOWED, new double[] {0, 250, 0}), + entry(ArmState.HYBRID, new double[] {0, 250, 0}), + entry(ArmState.SCORE_HIGH, new double[] {250, 0, 250}))), entry(ArmState.SCORE_HIGH, Map.ofEntries( // From SCORE_HIGH - entry(ArmState.STOWED, new double[] { 0, 0, 0 }), - entry(ArmState.HYBRID, new double[] { 250, 500, 0 }))), + entry(ArmState.STOWED, new double[] {0, 0, 0}), + entry(ArmState.HYBRID, new double[] {250, 500, 0}))), entry(ArmState.STARTING, Map.ofEntries( // From STARTING - entry(ArmState.SCORE_MID, new double[] { 500, 0, 500 }), - entry(ArmState.INTAKE_SHELF, new double[] { 500, 0, 500 }), - entry(ArmState.SCORE_HIGH, new double[] { 500, 0, 500 }))), + entry(ArmState.SCORE_MID, new double[] {500, 0, 500}), + entry(ArmState.INTAKE_SHELF, new double[] {500, 0, 500}), + entry(ArmState.SCORE_HIGH, new double[] {500, 0, 500}))), entry(ArmState.INTAKE_SHELF, Map.ofEntries( // From SCORE_MID - entry(ArmState.STOWED, new double[] { 0, 500, 0 }), - entry(ArmState.HYBRID, new double[] { 0, 250, 0 }))), + entry(ArmState.STOWED, new double[] {0, 500, 0}), + entry(ArmState.HYBRID, new double[] {0, 250, 0}))), entry(ArmState.UPRIGHT_GROUND, Map.ofEntries( // From UPRIGHT_GROUND - entry(ArmState.STOWED, new double[] { 0, 250, 0 }), - entry(ArmState.SCORE_MID, new double[] { 0, 150, 400 }), - entry(ArmState.INTAKE_SHELF, new double[] { 0, 150, 400 })))); + entry(ArmState.STOWED, new double[] {0, 250, 0}), + entry(ArmState.SCORE_MID, new double[] {0, 150, 400}), + entry(ArmState.INTAKE_SHELF, new double[] {0, 150, 400})))); + + private static ArmState VALID_PATHS[][][] = + new ArmState[VALID_PATHS_GRAPH.length][VALID_PATHS_GRAPH.length][]; - private static ArmState VALID_PATHS[][][] = new ArmState[VALID_PATHS_GRAPH.length][VALID_PATHS_GRAPH.length][]; + private static Arm mInstance; + + public static synchronized Arm getInstance() { + mInstance = mInstance == null ? new Arm() : mInstance; + return mInstance; + } public Arm() { this.shoulder = new Shoulder(); @@ -90,15 +94,17 @@ public Arm() { this.wrist = new Wrist(); this.targetState = ArmState.STOWED; this.initializedState = false; - this.voltageCalculator = new VoltageCalculator(RobotConstants.SHOULDER_SEGMENT, RobotConstants.ELBOW_SEGMENT, - RobotConstants.WRIST_SEGMENT); + this.voltageCalculator = new VoltageCalculator( + RobotConstants.Arm.Shoulder.kSegment, + RobotConstants.Arm.Elbow.kSegment, + RobotConstants.Arm.Wrist.kWristSegment); init(); } /** - * Private initialization method that populates the grpah of valid paths with - * every possible state transition + * Private initialization method that populates the grpah of valid paths with every possible + * state transition */ private static void init() { for (int i = 0; i < VALID_PATHS_GRAPH.length; i++) { @@ -118,7 +124,8 @@ public HealthState checkHealth() { } // If one or more joints are YELLOW, and none are RED, then Arm is YELLOW - if (shoulder.checkHealth() == HealthState.YELLOW || elbow.checkHealth() == HealthState.YELLOW + if (shoulder.checkHealth() == HealthState.YELLOW + || elbow.checkHealth() == HealthState.YELLOW || wrist.checkHealth() == HealthState.YELLOW) { return HealthState.YELLOW; } @@ -133,7 +140,8 @@ public void setStateToStarting() { @Override public void mustangPeriodic() { if (!initializedState) { - if (elbow.isRelativePositionSet() && shoulder.isRelativePositionSet() && wrist.isRelativePositionSet()) { + if (elbow.isRelativePositionSet() && shoulder.isRelativePositionSet() + && wrist.isRelativePositionSet()) { initializedState = true; this.targetState = getClosestState(); } @@ -157,9 +165,8 @@ public void mustangPeriodic() { } /** - * Public method to reset the arm's state from its absolute position. - * This will also temporarily set the Arm's HealthState to YELLOW - * until the relative positions have been properly reset + * Public method to reset the arm's state from its absolute position. This will also temporarily + * set the Arm's HealthState to YELLOW until the relative positions have been properly reset */ public void resetPositionFromAbsolute() { // initializedState = false; @@ -169,13 +176,12 @@ public void resetPositionFromAbsolute() { } /** - * This moves DIRECTLY to the target ArmState - * We must handle checking for valid paths ELSEWHERE. + * This moves DIRECTLY to the target ArmState We must handle checking for valid paths ELSEWHERE. */ public void moveToTarget(ArmState target) { if (checkHealth() == HealthState.GREEN) { - this.elbowOffset = 0; - this.shoulderOffset = 0; + // double elbowOffset = 0; + // double shoulderOffset = 0; hasSetShoulderTarget = false; hasSetElbowTarget = false; @@ -188,11 +194,11 @@ public void moveToTarget(ArmState target) { } if (currentTimeDelays == null) { - currentTimeDelays = new double[] { 0, 0, 0 }; + currentTimeDelays = new double[] {0, 0, 0}; } - SmartDashboard.putString("Arm moveToTarget()", - "from " + this.targetState + " to " + target + " is " + Arrays.toString(currentTimeDelays)); + SmartDashboard.putString("Arm moveToTarget()", "from " + this.targetState + " to " + + target + " is " + Arrays.toString(currentTimeDelays)); this.targetState = target; @@ -205,8 +211,9 @@ public void moveToTarget(ArmState target) { */ public void updateArbitraryFeedForward() { // The -90 here is so that the first joint's position is relative to the GROUND. - ArrayList voltages = voltageCalculator.calculateVoltages(shoulder.getCurrentAngleInDegrees() - 90, - elbow.getCurrentAngleInDegrees(), wrist.getCurrentAngleInDegrees()); + ArrayList voltages = + voltageCalculator.calculateVoltages(shoulder.getCurrentAngleInDegrees() - 90, + elbow.getCurrentAngleInDegrees(), wrist.getCurrentAngleInDegrees()); elbow.updateArbitraryFeedForward(voltages.get(0)); shoulder.updateArbitraryFeedForward(voltages.get(1)); @@ -214,8 +221,7 @@ public void updateArbitraryFeedForward() { } /** - * Returns the state we're moving towards - * Ex: If we're moving from A to B, this returns B + * Returns the state we're moving towards Ex: If we're moving from A to B, this returns B */ public ArmState getTargetState() { return targetState; @@ -230,8 +236,8 @@ public boolean hasReachedTargetPosition() { } /** - * Returns a valid list of states from the starting State to the ending State - * This path should include BOTH the starting state AND the ending state + * Returns a valid list of states from the starting State to the ending State This path should + * include BOTH the starting state AND the ending state */ public static ArmState[] getValidPath(ArmState start, ArmState finish) { // retrieve the list of intermediate states from VALID_PATHS @@ -283,10 +289,8 @@ public void debugSubsystem() { } /** - * Gets the closest state, using angles. - * It prefers prefers to move smaller joints than larger ones - * (i.e. it would rather choose to have the elbow be off by 20 - * degrees than the shoulder) + * Gets the closest state, using angles. It prefers prefers to move smaller joints than larger + * ones (i.e. it would rather choose to have the elbow be off by 20 degrees than the shoulder) * * @return */ @@ -296,7 +300,8 @@ public ArmState getClosestState() { double wristAngle = wrist.getCurrentAngleInDegrees(); ArmState closestState = ArmState.STOWED; - double closestStateDistance = 10000; // Intentionally high number. The first state checked will be less + double closestStateDistance = 10000; // Intentionally high number. The first state checked + // will be less for (ArmState state : ArmState.values()) { double shoulderDistance = Math.abs(state.getShoulderAngle() - shoulderAngle); diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java index 9d47d2ce..55ffe4cf 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java @@ -1,19 +1,13 @@ package frc.team670.robot.subsystems.arm; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.REVLibError; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; import frc.team670.mustanglib.utils.Logger; import frc.team670.mustanglib.utils.functions.MathUtils; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig.Motor_Type; -import frc.team670.mustanglib.utils.motorcontroller.SparkMAXLite; import frc.team670.robot.constants.RobotConstants; -import frc.team670.robot.constants.RobotMap; /** * Represents the Elbow joint. Uses only one motor @@ -38,143 +32,34 @@ public class Elbow extends SparkMaxRotatingSubsystem { private final String setpointRot = "Elbow setpoint (rotations)"; private final String current = "Elbow current"; - /* - * PID and SmartMotion constants for the Elbow joint - */ - public static class Config extends SparkMaxRotatingSubsystem.Config { - - public int getDeviceID() { - return RobotMap.ELBOW_MOTOR; - } - - public int getSlot() { - return 0; - } - - public Motor_Type getMotorType() { - return MotorConfig.Motor_Type.NEO; - } - - public double getP() { - // return 0.0011; //Value when chain popped on 3/1 and 3/2 - return 0.0007; - } - - public double getI() { - return 0; - } - - public double getD() { - // return 0.00015; - return 0.00015; - } - - public double getFF() { - return 0.000176; - } - - public double getIz() { - return 0; - } - - public double getMaxOutput() { - return 1; - } - - public double getMinOutput() { - return -1; - } - - public double getMaxAcceleration() { - return 4000; - } - - public double getAllowedError() { - return RobotConstants.ELBOW_GEAR_RATIO * 0.2 / 360; - } - - public boolean enableSoftLimits() { - return true; - } - - public float[] getSoftLimits() { - return new float[] { convertDegreesToRotations(RobotConstants.ELBOW_SOFT_LIMIT_MAX), - convertDegreesToRotations(RobotConstants.ELBOW_SOFT_LIMIT_MIN) }; - } - - public int getContinuousCurrent() { - return 20; - } - - public int getPeakCurrent() { - return 60; - } - - public double getRotatorGearRatio() { - return RobotConstants.ELBOW_GEAR_RATIO; - } - - public IdleMode setRotatorIdleMode() { - return IdleMode.kBrake; - } - - @Override - public double getMaxRotatorRPM() { - return 3000; - } - - @Override - public double getMinRotatorRPM() { - return 0; - } - - } - // constructor that inits motors and stuff - public static final Config ELBOW_CONFIG = new Config(); public Elbow() { - super(ELBOW_CONFIG); - absEncoder = new DutyCycleEncoder(RobotMap.ELBOW_ABSOLUTE_ENCODER); + super(RobotConstants.Arm.Elbow.kConfig); + absEncoder = new DutyCycleEncoder(RobotConstants.Arm.Elbow.kAbsoluteEncoderID); super.getRotator().setInverted(true); } /** - * Updates the arbitraryFF value to counteract gravity - * - * @param voltage The calculated voltage, returned from VoltageCalculator - */ - public void updateArbitraryFeedForward(double voltage) { - if (setpoint != SparkMaxRotatingSubsystem.NO_SETPOINT) { - rotator_controller.setReference(setpoint, - SparkMAXLite.ControlType.kSmartMotion, super.SMARTMOTION_SLOT, - voltage); - } - } - - // TODO: Move to mustang lib after testing - public double getSetpoint() { - return setpoint; - } - - /** - * PRIVATE method to set position from absolute. - * Do not use directly. Instead, use resetPositionFromAbsolute() + * PRIVATE method to set position from absolute. Do not use directly. Instead, use + * resetPositionFromAbsolute() */ private void setEncoderPositionFromAbsolute() { double absEncoderPosition = absEncoder.getAbsolutePosition(); - double previousPositionRot = super.rotator_encoder.getPosition(); + double previousPositionRot = super.mEncoder.getPosition(); if (absEncoderPosition != 0.0) { - double relativePosition = ((-1 - * (absEncoderPosition - (RobotConstants.ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL - 0.5)) + 1) - * RobotConstants.ELBOW_GEAR_RATIO) % RobotConstants.ELBOW_GEAR_RATIO; + double relativePosition = ((-1 * (absEncoderPosition + - (RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffset - 0.5)) + 2) + * RobotConstants.Arm.Elbow.kGearRatio) % RobotConstants.Arm.Elbow.kGearRatio; if (calculatedRelativePosition == 0.0 - || Math.abs(360 * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 10.0) { + || Math.abs(360 * ((previousPositionRot - relativePosition) + / kConfig.kRotatorGearRatio())) < 10.0) { clearSetpoint(); - REVLibError error = rotator_encoder.setPosition(relativePosition); - SmartDashboard.putNumber("Elbow absEncoder position when reset", absEncoderPosition); + REVLibError error = mEncoder.setPosition(relativePosition); + SmartDashboard.putNumber("Elbow absEncoder position when reset", + absEncoderPosition); SmartDashboard.putNumber("Elbow relEncoder position when reset", relativePosition); SmartDashboard.putString("Elbow error", error.toString()); calculatedRelativePosition = relativePosition; @@ -190,7 +75,7 @@ public boolean getTimeout() { @Override public HealthState checkHealth() { - REVLibError rotatorError = super.rotator.getLastError(); + REVLibError rotatorError = super.mRotator.getLastError(); if (rotatorError != null && rotatorError != REVLibError.kOk) { Logger.consoleError("Elbow error! Rotator error is " + rotatorError.toString()); @@ -211,9 +96,8 @@ public HealthState checkHealth() { } /** - * Returns whether or not the relative position has been properly set from the - * absEncoder. - * When resetPositionFromAbsolute() gets called, this will temporarily be false. + * Returns whether or not the relative position has been properly set from the absEncoder. When + * resetPositionFromAbsolute() gets called, this will temporarily be false. */ public boolean isRelativePositionSet() { return relativePositionIsSet; @@ -226,24 +110,26 @@ public void resetPositionFromAbsolute() { setEncoderPositionFromAbsolute(); } - @Override + // @Override public boolean hasReachedTargetPosition() { - return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, RobotConstants.ELBOW_ALLOWED_ERR_DEG)); + return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, + RobotConstants.Arm.Elbow.kAllowedErrorRotations)); } @Override public void setSystemTargetAngleInDegrees(double targetAngle) { orgTargetAngle = targetAngle; - super.setSystemTargetAngleInDegrees(orgTargetAngle + offset); + super.setSystemTargetAngleInDegrees(targetAngle); } private void setOffset(double offset) { - if (Math.abs(offset) > RobotConstants.ELBOW_MAX_OVERRIDE_DEGREES) { - this.offset = RobotConstants.ELBOW_MAX_OVERRIDE_DEGREES * this.offset / Math.abs(this.offset); + if (Math.abs(offset) > RobotConstants.Arm.Elbow.kMaxOverrideDegreees) { + this.offset = RobotConstants.Arm.Elbow.kMaxOverrideDegreees * this.offset + / Math.abs(this.offset); } else { this.offset = offset; } - setSystemTargetAngleInDegrees(orgTargetAngle); + // setSystemTargetAngleInDegrees(orgTargetAngle); } @@ -258,22 +144,27 @@ public void addOffset(double offset) { @Override public void debugSubsystem() { - double relativePosition = super.rotator_encoder.getPosition(); + double relativePosition = super.mEncoder.getPosition(); SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); - SmartDashboard.putNumber(setpointRot, setpoint); + SmartDashboard.putNumber(setpointRot, mSetpoint); SmartDashboard.putNumber(current, super.getRotator().getOutputCurrent()); + SmartDashboard.putNumber("Elbow motor power: ", super.mRotator.get()); } @Override public void mustangPeriodic() { if (!hasSetAbsolutePosition) { // before it's set an absolute position... double position = absEncoder.getAbsolutePosition(); - if (Math.abs(previousReading - position) < 0.02 && position != 0.0) { // If the current reading is PRECISELY - // 0, then it's not valid. - counter++; // increases the counter if the current reading is close enough to the last + if (Math.abs(previousReading - position) < 0.02 && position != 0.0) { // If the current + // reading is + // PRECISELY + // 0, then it's + // not valid. + counter++; // increases the counter if the current reading is close enough to the + // last // reading. // We do this because when the absEncoder gets initialized, its reading // fluctuates drastically at the start. @@ -286,13 +177,13 @@ public void mustangPeriodic() { hasSetAbsolutePosition = true; } } else if (!relativePositionIsSet) { - double position = super.rotator_encoder.getPosition(); - Logger.consoleLog("Elbow relative position = " + position + ", calculatedRelativePosition = " - + calculatedRelativePosition); + double position = super.mEncoder.getPosition(); + Logger.consoleLog("Elbow relative position = " + position + + ", calculatedRelativePosition = " + calculatedRelativePosition); if (Math.abs(position - calculatedRelativePosition) < 0.5) { relativePositionIsSet = true; } else { - super.rotator_encoder.setPosition(calculatedRelativePosition); + super.mEncoder.setPosition(calculatedRelativePosition); } Logger.consoleLog("Elbow relativePositionIsSet = " + this.relativePositionIsSet); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java index 01d20c5f..888a7136 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java @@ -1,24 +1,18 @@ package frc.team670.robot.subsystems.arm; import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; - -import frc.team670.mustanglib.utils.Logger; -import frc.team670.mustanglib.utils.functions.MathUtils; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; +import com.revrobotics.REVLibError; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; +import frc.team670.mustanglib.utils.Logger; +import frc.team670.mustanglib.utils.functions.MathUtils; import frc.team670.mustanglib.utils.motorcontroller.SparkMAXFactory; import frc.team670.mustanglib.utils.motorcontroller.SparkMAXLite; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig.Motor_Type; import frc.team670.robot.constants.RobotConstants; -import frc.team670.robot.constants.RobotMap; -import com.revrobotics.REVLibError; /** - * Represents the shoulder joint. The shoulder uses a leader-follower SparkMax - * pair + * Represents the shoulder joint. The shoulder uses a leader-follower SparkMax pair * * @author Armaan, Kedar, Aditi, Justin, Alexander, Gabriel, Srinish, Sanatan */ @@ -27,10 +21,10 @@ public class Shoulder extends SparkMaxRotatingSubsystem { private DutyCycleEncoder absEncoder; private SparkMAXLite follower; private boolean hasSetAbsolutePosition = false; - int counter = 0; - double previousReading = 0.0; - double calculatedRelativePosition = 0.0; - boolean relativePositionIsSet = false; + private int counter = 0; + private double previousReading = 0.0; + private double calculatedRelativePosition = 0.0; + private boolean relativePositionIsSet = false; private double offset = 0; private double orgTargetAngle = 0; @@ -38,129 +32,26 @@ public class Shoulder extends SparkMaxRotatingSubsystem { private final String absEncoderPos = "Shoulder abs encoder position"; private final String positionRot = "Shoulder position (rotations)"; private final String setpointRot = "Shoulder setpoint (rotations)"; + public static final double SHOULDER_ARBITRARY_FF = 0.5; - /* - * PID and SmartMotion constants for the Shoulder joint - */ - public static class Config extends SparkMaxRotatingSubsystem.Config { - - public int getDeviceID() { - return RobotMap.SHOULDER_LEADER_MOTOR; - } - - public int getSlot() { - return 0; - } - - public Motor_Type getMotorType() { - return MotorConfig.Motor_Type.NEO; - } - - public double getP() { - return 0.0005; - } - - public double getI() { - return 0; - } - - public double getD() { - return 0.00005; - } - - public double getFF() { - return 0.00017618; - } - - public double getIz() { - return 0; - } - - public double getMaxOutput() { - return 1; - } - - public double getMinOutput() { - return -1; - } - - public double getMaxAcceleration() { - return 2500; - } - - public double getAllowedError() { - return RobotConstants.SHOULDER_GEAR_RATIO * 0.2 / 360; - } - - public boolean enableSoftLimits() { - return true; - } - - public float[] getSoftLimits() { - return new float[] { convertDegreesToRotations(RobotConstants.SHOULDER_SOFT_LIMIT_MAX), - convertDegreesToRotations(RobotConstants.SHOULDER_SOFT_LIMIT_MIN) }; - } - - public int getContinuousCurrent() { - return 20; - } - - public int getPeakCurrent() { - return 60; - } - - public double getRotatorGearRatio() { - return RobotConstants.SHOULDER_GEAR_RATIO; - } - - public IdleMode setRotatorIdleMode() { - return IdleMode.kBrake; - } - - @Override - public double getMaxRotatorRPM() { - return 1500; - } - - @Override - public double getMinRotatorRPM() { - return 0; - } - } - - // constructor that inits motors and stuff - public static final Config SHOULDER_CONFIG = new Config(); public Shoulder() { - super(SHOULDER_CONFIG); + super(RobotConstants.Arm.Shoulder.kConfig); super.getRotator().setInverted(true); - follower = SparkMAXFactory.setPermanentFollower(RobotMap.SHOULDER_FOLLOWER_MOTOR, rotator, true); + follower = SparkMAXFactory + .setPermanentFollower(RobotConstants.Arm.Shoulder.kFollowerMotorID, mRotator, true); follower.setIdleMode(IdleMode.kBrake); - absEncoder = new DutyCycleEncoder(RobotMap.SHOULDER_ABSOLUTE_ENCODER); + absEncoder = new DutyCycleEncoder(RobotConstants.Arm.Shoulder.kAbsoluteEncoderID); } /** - * Returns whether or not the relative position has been properly set from the - * absEncoder. - * When resetPositionFromAbsolute() gets called, this will temporarily be false. + * Returns whether or not the relative position has been properly set from the absEncoder. When + * resetPositionFromAbsolute() gets called, this will temporarily be false. */ public boolean isRelativePositionSet() { return relativePositionIsSet; } - /** - * Updates the arbitraryFF value to counteract gravity - * - * @param voltage The calculated voltage, returned from VoltageCalculator - */ - public void updateArbitraryFeedForward(double voltage) { - if (setpoint != SparkMaxRotatingSubsystem.NO_SETPOINT) { - rotator_controller.setReference(setpoint, - SparkMAXLite.ControlType.kSmartMotion, super.SMARTMOTION_SLOT, - voltage); - } - } - @Override public boolean getTimeout() { return false; @@ -169,16 +60,17 @@ public boolean getTimeout() { @Override public void setSystemTargetAngleInDegrees(double targetAngle) { orgTargetAngle = targetAngle; - super.setSystemTargetAngleInDegrees(orgTargetAngle + offset); + super.setSystemTargetAngleInDegrees(targetAngle); } private void setOffset(double offset) { - if (Math.abs(offset) > RobotConstants.SHOULDER_MAX_OVERRIDE_DEGREES) { - this.offset = RobotConstants.SHOULDER_MAX_OVERRIDE_DEGREES * this.offset / Math.abs(this.offset); + if (Math.abs(offset) > RobotConstants.Arm.Shoulder.kMaxOverrideDegrees) { + this.offset = RobotConstants.Arm.Shoulder.kMaxOverrideDegrees * this.offset + / Math.abs(this.offset); } else { this.offset = offset; } - setSystemTargetAngleInDegrees(orgTargetAngle); + // setSystemTargetAngleInDegrees(orgTargetAngle); } @@ -193,7 +85,7 @@ public void addOffset(double offset) { @Override public HealthState checkHealth() { - REVLibError leaderRotatorError = super.rotator.getLastError(); + REVLibError leaderRotatorError = super.mRotator.getLastError(); REVLibError followerRotatorError = follower.getLastError(); boolean leaderOK = (leaderRotatorError == REVLibError.kOk); @@ -201,7 +93,8 @@ public HealthState checkHealth() { if (!leaderOK && !followerOK) { Logger.consoleError("Shoulder error! Leader error is " + leaderRotatorError.toString()); - Logger.consoleError("Shoulder error! Follower error is " + followerRotatorError.toString()); + Logger.consoleError( + "Shoulder error! Follower error is " + followerRotatorError.toString()); return HealthState.RED; } @@ -223,33 +116,38 @@ public void resetPositionFromAbsolute() { @Override public void debugSubsystem() { - double relativePosition = super.rotator_encoder.getPosition(); + double relativePosition = super.mEncoder.getPosition(); SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); - SmartDashboard.putNumber(setpointRot, setpoint); + SmartDashboard.putNumber(setpointRot, mSetpoint); } /** - * PRIVATE method to set position from absolute. - * DO NOT USE DIRECTLY. Instead, use resetPositionFromAbsolute() + * PRIVATE method to set position from absolute. DO NOT USE DIRECTLY. Instead, use + * resetPositionFromAbsolute() */ private void setEncoderPositionFromAbsolute() { double absEncoderPosition = absEncoder.getAbsolutePosition(); - double previousPositionRot = super.rotator_encoder.getPosition(); + double previousPositionRot = super.mEncoder.getPosition(); if (absEncoderPosition != 0.0) { double relativePosition = ((-1 - * (absEncoderPosition - (RobotConstants.SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL - 0.5)) + 1) - * RobotConstants.SHOULDER_GEAR_RATIO) % RobotConstants.SHOULDER_GEAR_RATIO; + * (absEncoderPosition + - (RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffset - 0.5)) + + 1) * RobotConstants.Arm.Shoulder.kGearRatio) + % RobotConstants.Arm.Shoulder.kGearRatio; if (calculatedRelativePosition == 0.0 - || Math.abs(360 * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 5.0) { + || Math.abs(360 * ((previousPositionRot - relativePosition) + / kConfig.kRotatorGearRatio())) < 5.0) { clearSetpoint(); - REVLibError error = rotator_encoder.setPosition(relativePosition); - SmartDashboard.putNumber("Shoulder absEncoder position when reset", absEncoderPosition); - SmartDashboard.putNumber("Shoulder relEncoder position when reset", relativePosition); + REVLibError error = mEncoder.setPosition(relativePosition); + SmartDashboard.putNumber("Shoulder absEncoder position when reset", + absEncoderPosition); + SmartDashboard.putNumber("Shoulder relEncoder position when reset", + relativePosition); SmartDashboard.putString("Shoulder error", error.toString()); calculatedRelativePosition = relativePosition; } @@ -257,24 +155,23 @@ private void setEncoderPositionFromAbsolute() { } } - // TODO: Move to mustang lib after testing; - public double getSetpoint() { - return setpoint; - } - @Override public boolean hasReachedTargetPosition() { - return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, - RobotConstants.SHOULDER_ALLOWED_ERR_DEG)); + return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, + RobotConstants.Arm.Shoulder.kAllowedErrorRotations)); } @Override public void mustangPeriodic() { if (!hasSetAbsolutePosition) { // before it's set an absolute position... double position = absEncoder.getAbsolutePosition(); - if (Math.abs(previousReading - position) < 0.02 && position != 0.0) { // If the current reading is PRECISELY - // 0, then it's not valid. - counter++; // increases the counter if the current reading is close enough to the last + if (Math.abs(previousReading - position) < 0.02 && position != 0.0) { // If the current + // reading is + // PRECISELY + // 0, then it's + // not valid. + counter++; // increases the counter if the current reading is close enough to the + // last // reading. // We do this because when the absEncoder gets initialized, its reading // fluctuates drastically at the start. @@ -287,14 +184,14 @@ public void mustangPeriodic() { hasSetAbsolutePosition = true; } } else if (!relativePositionIsSet) { - double position = super.rotator_encoder.getPosition(); - Logger.consoleLog("Shoulder relative position = " + position + ", calculatedRelativePosition = " - + calculatedRelativePosition); + double position = super.mEncoder.getPosition(); + Logger.consoleLog("Shoulder relative position = " + position + + ", calculatedRelativePosition = " + calculatedRelativePosition); Logger.consoleLog("Shoulder relativePositionIsSet = " + this.relativePositionIsSet); if (Math.abs(position - calculatedRelativePosition) < 0.5) { relativePositionIsSet = true; } else { - super.rotator_encoder.setPosition(calculatedRelativePosition); + super.mEncoder.setPosition(calculatedRelativePosition); } } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Wrist.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Wrist.java index 18bb56d9..3f50779d 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Wrist.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Wrist.java @@ -1,20 +1,13 @@ package frc.team670.robot.subsystems.arm; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.REVLibError; - import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; import frc.team670.mustanglib.utils.Logger; import frc.team670.mustanglib.utils.functions.MathUtils; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig.Motor_Type; -import frc.team670.mustanglib.utils.motorcontroller.SparkMAXLite; import frc.team670.robot.constants.RobotConstants; -import frc.team670.robot.constants.RobotMap; /** * Represents the wrist joint. Uses only one motor @@ -36,135 +29,37 @@ public class Wrist extends SparkMaxRotatingSubsystem { private final String positionRot = "Wrist position (rotations)"; private final String setpointRot = "Wrist setpoint (rotations)"; - /* - * PID and SmartMotion constants for the Wrist joint - */ - public static class Config extends SparkMaxRotatingSubsystem.Config { - - public int getDeviceID() { - return RobotMap.WRIST_MOTOR; - } - - public int getSlot() { - return 0; - } - - public Motor_Type getMotorType() { - return MotorConfig.Motor_Type.NEO_550; - } - - public double getP() { - return 0.00011; - } - - public double getI() { - return 0; - } - - public double getD() { - return 0.00015; - } - - public double getFF() { - return 0.000176; - } - - public double getIz() { - return 0; - } - - public double getMaxOutput() { - return 1; - } - - public double getMinOutput() { - return -1; - } - - public double getMaxAcceleration() { - return 15000; - } - - public double getAllowedError() { - return RobotConstants.WRIST_GEAR_RATIO * 0.2 / 360; - } - - public boolean enableSoftLimits() { - return false; - } - - public float[] getSoftLimits() { - return null; - } - - public int getContinuousCurrent() { - return 20; - } - - public int getPeakCurrent() { - return 20; - } - - public double getRotatorGearRatio() { - return RobotConstants.WRIST_GEAR_RATIO; - } - - public IdleMode setRotatorIdleMode() { - return IdleMode.kBrake; - } - - @Override - public double getMaxRotatorRPM() { - return 6000; - } - - @Override - public double getMinRotatorRPM() { - return 0; - } - - } - - public static final Config WRIST_CONFIG = new Config(); public Wrist() { - super(WRIST_CONFIG); - absEncoder = new DutyCycleEncoder(RobotMap.WRIST_ABSOLUTE_ENCODER); + super(RobotConstants.Arm.Wrist.kConfig); + absEncoder = new DutyCycleEncoder(RobotConstants.Arm.Wrist.kAbsoluteEncoderID); super.getRotator().setInverted(false); } - /** - * Calculated voltage using VoltageCalculator - * @param voltage - */ - public void updateArbitraryFeedForward(double voltage) { - if(setpoint != SparkMaxRotatingSubsystem.NO_SETPOINT){ - rotator_controller.setReference(setpoint, - SparkMAXLite.ControlType.kSmartMotion, super.SMARTMOTION_SLOT, - voltage); - } - } /** - * PRIVATE method to set position from absolute. - * DO NOT USE DIRECTLY. Instead, use resetPositionFromAbsolute() + * PRIVATE method to set position from absolute. DO NOT USE DIRECTLY. Instead, use + * resetPositionFromAbsolute() */ - public void setEncoderPositionFromAbsolute() { + private void setEncoderPositionFromAbsolute() { double absEncoderPosition = absEncoder.getAbsolutePosition(); - double previousPositionRot = super.rotator_encoder.getPosition(); + double previousPositionRot = super.mEncoder.getPosition(); - if(absEncoderPosition != 0.0) { - double relativePosition = ((1 - * (absEncoderPosition - (RobotConstants.WRIST_ABSOLUTE_ENCODER_AT_VERTICAL - 0.5)) + 1) - * RobotConstants.WRIST_GEAR_RATIO) % RobotConstants.WRIST_GEAR_RATIO; - - if(calculatedRelativePosition == 0.0 || Math.abs(360 * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 20.0) { + if (absEncoderPosition != 0.0) { + double relativePosition = ((1 * (absEncoderPosition + - (RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffset - 0.5)) + 1) + * RobotConstants.Arm.Wrist.kGearRatio) % RobotConstants.Arm.Wrist.kGearRatio; + + if (calculatedRelativePosition == 0.0 + || Math.abs(360 * ((previousPositionRot - relativePosition) + / kConfig.kRotatorGearRatio())) < 20.0) { clearSetpoint(); - - REVLibError error = rotator_encoder.setPosition(relativePosition); - SmartDashboard.putNumber("Wrist absEncoder position when reset", absEncoderPosition); + + REVLibError error = mEncoder.setPosition(relativePosition); + SmartDashboard.putNumber("Wrist absEncoder position when reset", + absEncoderPosition); SmartDashboard.putNumber("Wrist relEncoder position when reset", relativePosition); SmartDashboard.putString("Wrist error", error.toString()); calculatedRelativePosition = relativePosition; @@ -180,30 +75,30 @@ public boolean getTimeout() { @Override public HealthState checkHealth() { - REVLibError rotatorError = super.rotator.getLastError(); + REVLibError rotatorError = super.mRotator.getLastError(); - if (rotatorError != null && rotatorError != REVLibError.kOk) { - Logger.consoleError("Wrist error! Rotator Error is " + rotatorError.toString()); - errorCounter++; - } else { - errorCounter = 0; - } + if (rotatorError != null && rotatorError != REVLibError.kOk) { + Logger.consoleError("Wrist error! Rotator Error is " + rotatorError.toString()); + errorCounter++; + } else { + errorCounter = 0; + } - if (errorCounter >= 20){ - return HealthState.RED; - } + if (errorCounter >= 20) { + return HealthState.RED; + } - if (!hasSetAbsolutePosition || !relativePositionIsSet) { - return HealthState.YELLOW; - } + if (!hasSetAbsolutePosition || !relativePositionIsSet) { + return HealthState.YELLOW; + } return HealthState.GREEN; } /** - * Returns whether or not the relative position has been properly set from the absEncoder. - * When resetPositionFromAbsolute() gets called, this will temporarily be false. + * Returns whether or not the relative position has been properly set from the absEncoder. When + * resetPositionFromAbsolute() gets called, this will temporarily be false. */ public boolean isRelativePositionSet() { return relativePositionIsSet; @@ -218,17 +113,18 @@ public void resetPositionFromAbsolute() { @Override public boolean hasReachedTargetPosition() { - return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, RobotConstants.WRIST_ALLOWED_ERR_DEG)); + return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, + RobotConstants.Arm.Wrist.kAllowedErrorRotations)); } @Override public void debugSubsystem() { - double relativePosition = super.rotator_encoder.getPosition(); - + double relativePosition = super.mEncoder.getPosition(); + SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); - SmartDashboard.putNumber(setpointRot, setpoint); + SmartDashboard.putNumber(setpointRot, mSetpoint); } @@ -236,9 +132,13 @@ public void debugSubsystem() { public void mustangPeriodic() { if (!hasSetAbsolutePosition) { // before it's set an absolute position... double position = absEncoder.getAbsolutePosition(); - if (Math.abs(previousReading - position) < 0.02 && position != 0.0) { // If the current reading is PRECISELY - // 0, then it's not valid. - counter++; // increases the counter if the current reading is close enough to the last + if (Math.abs(previousReading - position) < 0.02 && position != 0.0) { // If the current + // reading is + // PRECISELY + // 0, then it's + // not valid. + counter++; // increases the counter if the current reading is close enough to the + // last // reading. // We do this because when the absEncoder gets initialized, its reading // fluctuates drastically at the start. @@ -251,14 +151,14 @@ public void mustangPeriodic() { hasSetAbsolutePosition = true; } } else if (!relativePositionIsSet) { - if (Math.abs(super.rotator_encoder.getPosition() - calculatedRelativePosition) < 0.5) { + if (Math.abs(super.mEncoder.getPosition() - calculatedRelativePosition) < 0.5) { relativePositionIsSet = true; } else { - super.rotator_encoder.setPosition(calculatedRelativePosition); + super.mEncoder.setPosition(calculatedRelativePosition); } } } - + public void sendAngleToDashboard() { SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); }