From 7befedaf2e1483a255a8c23f6bb72c0644d9dbf1 Mon Sep 17 00:00:00 2001 From: Ethan Date: Fri, 7 Apr 2023 23:39:44 -0700 Subject: [PATCH 01/33] constants refactor --- 2023-Robot/build.gradle | 4 +- .../src/main/java/frc/team670/mustanglib | 2 +- .../frc/team670/robot/RobotContainer.java | 14 +- .../robot/commands/drivebase/MoveToPose.java | 3 +- .../commands/drivebase/MoveToPosePID.java | 8 +- .../drivebase/PathFindMoveToPose.java | 3 +- .../robot/commands/drivebase/TurnToAngle.java | 8 +- .../pathplanner/AutonCalibration.java | 10 +- .../commands/pathplanner/CenterEngage.java | 10 +- .../pathplanner/CenterEngageSequential.java | 24 +- .../commands/pathplanner/CenterIntake.java | 12 +- .../robot/commands/pathplanner/ConeCube.java | 15 +- .../commands/pathplanner/ConeCubeCube.java | 9 +- .../commands/pathplanner/ConeCubeEngage.java | 17 +- .../commands/pathplanner/CubeEngage.java | 7 +- .../commands/pathplanner/ScoreEngage.java | 12 +- .../robot/commands/vision/IsLockedOn.java | 29 +- .../robot/constants/FieldConstants.java | 200 +++--- .../java/frc/team670/robot/constants/OI.java | 2 +- .../robot/constants/RobotConstants.java | 612 ++++++++++-------- .../frc/team670/robot/constants/RobotMap.java | 24 - .../frc/team670/robot/subsystems/Claw.java | 31 +- .../team670/robot/subsystems/DriveBase.java | 171 ++--- .../frc/team670/robot/subsystems/Vision.java | 8 +- .../frc/team670/robot/subsystems/arm/Arm.java | 125 ++-- .../team670/robot/subsystems/arm/Elbow.java | 159 ++--- .../robot/subsystems/arm/Shoulder.java | 163 ++--- .../team670/robot/subsystems/arm/Wrist.java | 176 ++--- 28 files changed, 818 insertions(+), 1040 deletions(-) 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..9b01debe 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 9b01debe123386f7b25a6feaf5f17747cdffc9bb 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..ddd899ab 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -38,7 +38,7 @@ 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; @@ -46,8 +46,7 @@ import frc.team670.robot.subsystems.arm.Arm; /** - * RobotContainer is where we put the high-level code for the robot. It contains - * subsystems, OI + * RobotContainer is where we put the high-level code for the robot. It contains subsystems, OI * devices, etc, and has required methods (autonomousInit, periodic, etc) */ @@ -58,7 +57,7 @@ public class RobotContainer extends RobotContainerBase { 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 LED led = new LED(RobotConstants.kLEDPort, 0, 61); private final Claw claw = new Claw(led); private MustangCommand cableScore, cableEngage, stationScore, stationEngage, centerEngage, @@ -158,14 +157,14 @@ public MustangCommand getAutonomousCommand() { // LEAVE COMMENTED // greturn new ConeCube(driveBase, claw, arm, "CableScore"); - //return new AutonCalibration(driveBase, "Straight180"); // TODO: use curve + // 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 CenterEngage(driveBase, claw, arm, "CenterEngage"); // return new ConeCube(driveBase, claw, arm, "CableScore"); // return new ConeCube(driveBase, claw, arm, "RightConeCube"); @@ -183,8 +182,7 @@ public void teleopInit() { } @Override - public void testInit() { - } + public void testInit() {} @Override public void disabled() { 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..4513c9df 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 @@ -18,7 +18,6 @@ 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 +57,7 @@ public Map getHealthRequirements() { @Override public void initialize() { this.startPose = driveBase.getPose(); - PathPlannerTrajectory traj = PathPlanner.generatePath(RobotConstants.kAutoPathConstraints, + PathPlannerTrajectory traj = PathPlanner.generatePath(frc.team670.robot.constants.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..60ef4278 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 @@ -15,7 +15,6 @@ 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; @@ -48,7 +47,7 @@ public void initialize() { PathPoint[] fullPathPoints = getPathPointsFromNodes(fullPath); PathPlannerTrajectory trajectory = PathPlanner - .generatePath(RobotConstants.kAutoPathConstraints, Arrays.asList(fullPathPoints)); + .generatePath(frc.team670.robot.constants.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/pathplanner/AutonCalibration.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/AutonCalibration.java index e5e8280c..fd6f4e69 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 @@ -37,12 +37,14 @@ public AutonCalibration(DriveBase driveBase, String pathName) { List trajectoryGroup = PathPlanner.loadPathGroup(pathName, 2.5, 2); HashMap eventMap = new HashMap<>(); - + 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); 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..64402620 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,12 +44,14 @@ 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); 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..ab04de7c 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 @@ -29,7 +29,7 @@ import frc.team670.robot.subsystems.arm.ArmState; public class CenterEngageSequential extends SequentialCommandGroup implements MustangCommand { - + public Map getHealthRequirements() { return new HashMap(); } @@ -38,17 +38,17 @@ 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))); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, 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))); } } 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..db1bdcd5 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,12 +47,14 @@ 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); 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..d4564fe0 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,18 +40,21 @@ 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); 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..0cdcc99b 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 @@ -51,10 +51,11 @@ public ConeCubeCube(DriveBase driveBase, Claw claw, Arm arm, String pathName) { 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); 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..297fa3b4 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,28 @@ 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.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..7f670d0d 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<>(); 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..100678ff 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 @@ -54,13 +54,15 @@ public ScoreEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { 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}); + SwerveAutoBuilder autoBuilder = + new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, + driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, + RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, + eventMap, true, new Subsystem[] {driveBase}); CommandBase fullAuto = autoBuilder.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..c2f49631 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 @@ -29,13 +29,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 +45,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 +64,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..2eba37c3 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 @@ -45,29 +45,33 @@ public static final class Community { new Translation2d(innerX, rightY), new Translation2d(innerX, leftY), new Translation2d(midX, leftY), new Translation2d(midX, midY), new Translation2d(outerX, midY), - new Translation2d(outerX, rightY), }; + 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 chargingStationInnerX = + chargingStationOuterX - chargingStationLength; public static final double chargingStationLeftY = midY - tapeWidth; - public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth; + 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) }; + 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 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) }; + new Translation2d(cableBumpOuterX, chargingStationRightY)}; } // Dimensions for grids and nodes @@ -95,11 +99,16 @@ public static final class Grids { 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]; + 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++) { @@ -121,11 +130,12 @@ public static final class Grids { // 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 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); @@ -148,14 +158,15 @@ public static final class Grids { new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 7), new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 8 - + complexLowOuterYOffset) }; + + complexLowOuterYOffset)}; - public static final Pose2d[] scoringPoses = new Pose2d[complexLowTranslations.length]; + 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, + RobotConstants.DriveBase.kClearance, 0)), new Rotation2d()); } @@ -178,7 +189,7 @@ public static final class LoadingZone { // community new Translation2d(midX, midY), new Translation2d(outerX, midY), new Translation2d(outerX, leftY), new Translation2d(innerX, leftY), - new Translation2d(innerX, rightY), }; + new Translation2d(innerX, rightY),}; // Double substation dimensions public static final double doubleSubstationLength = Units.inchesToMeters(14.0); @@ -189,36 +200,38 @@ public static final class LoadingZone { 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 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 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 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_CLEARANCE, 6.01 + 1.31, - new Rotation2d(Math.PI)), - new Pose2d(fieldLength - 0.31 - RobotConstants.DRIVEBASE_CLEARANCE, 6.01, - new Rotation2d(Math.PI)) - }; + 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 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]; @@ -231,8 +244,7 @@ public static final class StagingLocations { } } - public static final Map blueAprilTags = Map.of( - 1, + 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)), @@ -240,7 +252,8 @@ public static final class StagingLocations { 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), + 3, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), Units.inchesToMeters(18.22), new Rotation3d(0.0, 0.0, Math.PI)), 4, @@ -248,11 +261,11 @@ public static final class StagingLocations { Units.inchesToMeters(27.38), new Rotation3d(0.0, 0.0, Math.PI))); - public static final Map redAprilTags = Map.of( - 5, + 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), + 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), @@ -270,7 +283,8 @@ public static final class StagingLocations { 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), + 3, + new Pose3d(Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), Units.inchesToMeters(18.22), new Rotation3d(0.0, 0.0, Math.PI)), 4, @@ -280,7 +294,8 @@ public static final class StagingLocations { 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), + 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), @@ -290,83 +305,70 @@ public static final class StagingLocations { public static final Translation2d[] obstacleCorners = { // right inner - new Translation2d( - FieldConstants.Community.chargingStationCorners[0].getX(), + new Translation2d(FieldConstants.Community.chargingStationCorners[0].getX(), FieldConstants.Community.chargingStationCorners[0].getY()), - new Translation2d( - FieldConstants.Community.chargingStationCorners[1].getX(), + new Translation2d(FieldConstants.Community.chargingStationCorners[1].getX(), FieldConstants.Community.chargingStationCorners[1].getY()), - new Translation2d( - FieldConstants.Community.chargingStationCorners[3].getX(), + new Translation2d(FieldConstants.Community.chargingStationCorners[3].getX(), FieldConstants.Community.chargingStationCorners[3].getY()), - new Translation2d( - FieldConstants.Community.chargingStationCorners[2].getX(), - FieldConstants.Community.chargingStationCorners[2] - .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); + 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]), + 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]) })); + 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 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_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE))), + 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_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE))), + 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_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE))), + 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_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE)), + 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_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE)), + 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_CLEARANCE, - RobotConstants.DRIVEBASE_CLEARANCE)), + 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_CLEARANCE, - -RobotConstants.DRIVEBASE_CLEARANCE)), + 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 + * 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) { @@ -383,7 +385,8 @@ public static Translation2d allianceFlip(Translation2d translation) { */ public static Pose2d allianceFlip(Pose2d pose) { if (DriverStation.getAlliance() == Alliance.Red) { - return new Pose2d(fieldLength - pose.getX(), pose.getY(), pose.getRotation().times(-1)); + return new Pose2d(fieldLength - pose.getX(), pose.getY(), + pose.getRotation().times(-1)); } else { return pose; } @@ -397,6 +400,7 @@ public static Pose2d allianceOrientedAllianceFlip(Pose2d pose) { return pose; } } + public static Translation2d allianceOrientedAllianceFlip(Translation2d t) { if (DriverStation.getAlliance() == Alliance.Red) { return new Translation2d(fieldLength - t.getX(), fieldWidth - t.getY()); 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..3d150fb3 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 @@ -27,7 +27,7 @@ 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); 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..6c74fa45 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 @@ -9,10 +9,11 @@ import java.net.SocketException; import java.util.Enumeration; import java.util.Map; - +import com.fasterxml.jackson.annotation.Nulls; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.auto.PIDConstants; - +import com.pathplanner.lib.auto.SwerveAutoBuilder; +import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -20,293 +21,374 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SerialPort; import frc.team670.mustanglib.constants.RobotConstantsBase; +import frc.team670.mustanglib.constants.SwerveConfig; +import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper; import frc.team670.mustanglib.swervelib.ModuleConfiguration; import frc.team670.mustanglib.swervelib.SdsModuleConfigurations; +import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; +import frc.team670.mustanglib.utils.motorcontroller.MotorConfig.Motor_Type; import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper.GearRatio; import frc.team670.robot.subsystems.arm.ArmSegment; /** - * The Constants class provides a convenient place for teams to hold robot-wide - * numerical or boolean - * constants. This class should not be used for any other purpose. All constants - * should be declared + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared * globally (i.e. public static). Do not put anything functional in this class. * - * It is advised to statically import this class (or one of its inner classes) - * wherever the + * It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. */ public final class RobotConstants extends RobotConstantsBase { + /** + * 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 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(); + private 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("kShoulderAbsoluteEncoderVerticalOffsetRadians", 0.484233), // 0.47777 + entry("kElbowAbsoluteEncoderVerticalOffsetRadians", 0.004892), + entry("kWristAbsoluteEncoderVerticalOffsetRadians", 0.337308), + entry("kShoulderGearRatio", 96.0), entry("kElbowGearRatio", 70.833333333333), + entry("kSwerveModuleConfig", 2.0), entry("kWristGearRatio", 125.0))), + entry(kSkipperAddress, + Map.ofEntries( + entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(82.694)), + entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(233.29)), + entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(225.77)), + entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(112.53)), + entry("kShoulderAbsoluteEncoderVerticalOffsetRadians", 0.895), + entry("kElbowAbsoluteEncoderVerticalOffsetRadians", 0.588), + entry("kWristAbsoluteEncoderVerticalOffsetRadians", 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 kBackLeftModuleSteerMotorID = 26; + public static final int kBackLeftModuleDriveMotorID = 27; + public static final int kBackLeftModuleSteerEncoderID = 36; + public static final double kBackLeftModuleSteerOffsetRadians = + robotSpecificConstants.get("kBackLeftModuleSteerOffsetRadians"); + + 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 final static SerialPort.Port kNAVXPort = SerialPort.Port.kMXP; + public static final double kPitchOffset = 2; - 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) - - ); - - /** - * 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; + + // 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 SwerveConfig kConfig = new SwerveConfig(kTrackWidthMeters, + kWheelBaseMeters, kMaxVelocityMetersPerSecond, kMaxVoltage, 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); + + // public static final SwerveAutoBuilder autoBuilder = + } + + + // 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 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 kAbsoluteEncoderVerticalOffsetRadians = + RobotConstants.robotSpecificConstants + .get("kShoulderAbsoluteEncoderVerticalOffsetRadians"); + 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 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 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, kAllowedErrorDegrees, + kSoftLimits, kContinuousCurrent, kPeakCurrent); + + } + + public static final class Elbow { + public static final double kAbsoluteEncoderVerticalOffsetRadians = + RobotConstants.robotSpecificConstants + .get("kElbowAbsoluteEncoderVerticalOffsetRadians"); + 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 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 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); + } - // 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 Wrist { + + // Wrist + public static final double kAbsoluteEncoderVerticalOffsetRadians = + RobotConstants.robotSpecificConstants + .get("kWristAbsoluteEncoderVerticalOffsetRadians"); + 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 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, kAllowedErrorDegrees, + kSoftLimits, kContinuousCurrent, kPeakCurrent); + + } + + 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 double kConeMassLb = 1.4; + public static final int kLEDPort = 0; + + + + /** + * 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 index 42e9d094..235f5fd0 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java +++ b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java @@ -2,30 +2,6 @@ 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/subsystems/Claw.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java index 384aa581..507a1e45 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 @@ -6,9 +6,6 @@ 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,12 +26,15 @@ 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 = + frc.team670.robot.constants.RobotConstants.Arm.Claw.kEjectingSpeed; private LED led; public Claw(LED led) { - motor = SparkMAXFactory.buildSparkMAX(RobotMap.CLAW_MOTOR, SparkMAXFactory.defaultConfig, Motor_Type.NEO); + motor = SparkMAXFactory.buildSparkMAX( + frc.team670.robot.constants.RobotConstants.Arm.Claw.kMotorID, + SparkMAXFactory.defaultConfig, Motor_Type.NEO); status = Status.IDLE; this.led = led; @@ -56,14 +56,13 @@ public boolean isFull() { } public void startEjecting() { - this.startEjecting(RobotConstants.CLAW_EJECTING_SPEED); + this.startEjecting(frc.team670.robot.constants.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 +80,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 +111,14 @@ public void mustangPeriodic() { switch (status) { case IDLE: - motor.set(RobotConstants.CLAW_IDLE_SPEED); + motor.set(frc.team670.robot.constants.RobotConstants.Arm.Claw.kIdleSpeed); break; case INTAKING: - motor.set(RobotConstants.CLAW_ROLLING_SPEED); - if (motor.getOutputCurrent() > RobotConstants.CLAW_CURRENT_MAX) { + motor.set(frc.team670.robot.constants.RobotConstants.Arm.Claw.kRollingSpeed); + if (motor + .getOutputCurrent() > frc.team670.robot.constants.RobotConstants.Arm.Claw.kCurrentMax) { currentSpikeCounter++; - if (currentSpikeCounter > RobotConstants.CLAW_CURRENT_SPIKE_ITERATIONS) { + if (currentSpikeCounter > frc.team670.robot.constants.RobotConstants.Arm.Claw.kCurrentSpikeIterations) { isFull = true; if (DriverStation.isTeleopEnabled()) { led.solidhsv(LEDColor.GREEN); @@ -138,7 +137,7 @@ public void mustangPeriodic() { case EJECTING: motor.set(ejectingSpeed); ejectCounter++; - if (ejectCounter > RobotConstants.CLAW_EJECT_ITERATIONS) { + if (ejectCounter > frc.team670.robot.constants.RobotConstants.Arm.Claw.kEjectIterations) { ejectCounter = 0; isFull = false; if (DriverStation.isTeleopEnabled()) { @@ -157,4 +156,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..ae363d9a 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 @@ -25,119 +25,72 @@ 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; - - // 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; - - /** - * 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 - // 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); - 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; - // } - - @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; - } + + + private MustangCommand defaultCommand; + private MustangController mController; + + public DriveBase(MustangController mustangController) { + super(RobotConstants.DriveBase.kConfig); + this.mController = mustangController; + } + + /** + * Makes the DriveBase's default command initialize teleop + */ + public void initDefaultCommand() { // 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, + RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, + RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond); + MustangScheduler.getInstance().setDefaultCommand(this, defaultCommand); + } + + public void cancelDefaultCommand() { + MustangScheduler.getInstance().cancel(defaultCommand); + } + + public MustangCommand getDefaultMustangCommand() { + return defaultCommand; + } + + public void mustangPeriodic() { + super.mustangPeriodic(); + SmartDashboard.putNumber("pitch", getPitch()); + } + + // @Override + // public void periodic() { + // return; + // } + + @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; - } + } + return HealthState.GREEN; + } - @Override - public void debugSubsystem() { + @Override + public void debugSubsystem() { - } + } - 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(); + public MustangPPSwerveControllerCommand getFollowTrajectoryCommand(PathPlannerTrajectory traj) { + setSwerveControllerCommand(new MustangPPSwerveControllerCommand(traj, this::getPose, + getSwerveKinematics(), RobotConstants.DriveBase.xController, + RobotConstants.DriveBase.yController, RobotConstants.DriveBase.thetaController, + this::setModuleStates, new Subsystem[] {this})); + return getSwerveControllerCommand(); - } + } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java index b78a1e72..8beee6ad 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java @@ -11,9 +11,9 @@ public class Vision extends VisionSubsystemBase { public Vision(PowerDistribution pd) { super(pd, FieldConstants.getFieldLayout(FieldConstants.aprilTags), - new PhotonCamera[] {new PhotonCamera(RobotConstants.VISION_CAMERA_NAMES[0]), - new PhotonCamera(RobotConstants.VISION_CAMERA_NAMES[1])}, - RobotConstants.CAMERA_OFFSETS); + new PhotonCamera[] {new PhotonCamera(RobotConstants.Vision.kVisionCameraIDs[0]), + new PhotonCamera(RobotConstants.Vision.kVisionCameraIDs[1])}, + RobotConstants.Vision.kCameraOffsets); setName("Vision"); } @@ -22,5 +22,5 @@ public void mustangPeriodic() {} @Override public void debugSubsystem() {} - + } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 739579c2..e432c542 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -7,11 +7,9 @@ import java.util.PriorityQueue; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; -import frc.team670.robot.constants.RobotConstants; /** - * Represents the whole Arm system, containing multiple joints. - * Models the arm as a state machine. + * Represents the whole Arm system, containing multiple joints. Models the arm as a state machine. * * @author Armaan, Aditi, Alexander, Gabriel, Kedar, Justin, Sanatan, Srinish */ @@ -35,54 +33,56 @@ public class Arm extends MustangSubsystemBase { private long startingTime = 0; - private double[] currentTimeDelays = new double[] { 0, 0, 0 }; + private double[] currentTimeDelays = new double[] {0, 0, 0}; private static final ArmState[][] VALID_PATHS_GRAPH = new ArmState[][] { - { ArmState.TUNING, ArmState.SCORE_MID, ArmState.SINGLE_STATION, ArmState.SCORE_HIGH, ArmState.HYBRID, - ArmState.INTAKE_SHELF, ArmState.UPRIGHT_GROUND }, // STOWED - { ArmState.STOWED, ArmState.INTAKE_SHELF, ArmState.UPRIGHT_GROUND, ArmState.SCORE_HIGH, - ArmState.SCORE_MID }, // HYBRID - { ArmState.SCORE_HIGH, ArmState.STOWED, ArmState.STARTING, ArmState.SINGLE_STATION, ArmState.HYBRID }, // SCORE_MID - { ArmState.SCORE_MID, ArmState.STOWED, ArmState.SINGLE_STATION, ArmState.HYBRID, ArmState.INTAKE_SHELF, - ArmState.STARTING }, // SCORE_HIGH - { ArmState.SCORE_MID, ArmState.INTAKE_SHELF, ArmState.SCORE_HIGH }, // STARTING - { ArmState.STOWED }, // TUNING - { ArmState.STOWED, ArmState.SCORE_MID, ArmState.SCORE_HIGH, ArmState.INTAKE_SHELF }, // SINGLE_STATION - { ArmState.SCORE_HIGH, ArmState.STOWED, ArmState.STARTING, ArmState.HYBRID, ArmState.SINGLE_STATION, - ArmState.UPRIGHT_GROUND }, // INTAKE_SHELF - { ArmState.STOWED, ArmState.INTAKE_SHELF, ArmState.HYBRID }, // UPRIGHT_GROUND + {ArmState.TUNING, ArmState.SCORE_MID, ArmState.SINGLE_STATION, ArmState.SCORE_HIGH, + ArmState.HYBRID, ArmState.INTAKE_SHELF, ArmState.UPRIGHT_GROUND}, // STOWED + {ArmState.STOWED, ArmState.INTAKE_SHELF, ArmState.UPRIGHT_GROUND, ArmState.SCORE_HIGH, + ArmState.SCORE_MID}, // HYBRID + {ArmState.SCORE_HIGH, ArmState.STOWED, ArmState.STARTING, ArmState.SINGLE_STATION, + ArmState.HYBRID}, // SCORE_MID + {ArmState.SCORE_MID, ArmState.STOWED, ArmState.SINGLE_STATION, ArmState.HYBRID, + ArmState.INTAKE_SHELF, ArmState.STARTING}, // SCORE_HIGH + {ArmState.SCORE_MID, ArmState.INTAKE_SHELF, ArmState.SCORE_HIGH}, // STARTING + {ArmState.STOWED}, // TUNING + {ArmState.STOWED, ArmState.SCORE_MID, ArmState.SCORE_HIGH, ArmState.INTAKE_SHELF}, // SINGLE_STATION + {ArmState.SCORE_HIGH, ArmState.STOWED, ArmState.STARTING, ArmState.HYBRID, + ArmState.SINGLE_STATION, ArmState.UPRIGHT_GROUND}, // INTAKE_SHELF + {ArmState.STOWED, ArmState.INTAKE_SHELF, ArmState.HYBRID}, // UPRIGHT_GROUND }; private static final Map> 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][]; public Arm() { this.shoulder = new Shoulder(); @@ -90,15 +90,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( + frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kSegment, + frc.team670.robot.constants.RobotConstants.Arm.Elbow.kSegment, + frc.team670.robot.constants.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 +120,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 +136,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 +161,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,8 +172,7 @@ 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) { @@ -188,11 +190,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 +207,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 +217,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 +232,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 +285,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 +296,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..e0c64da0 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,7 +1,6 @@ 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; @@ -9,11 +8,8 @@ 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,104 +34,12 @@ 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( + frc.team670.robot.constants.RobotConstants.Arm.Elbow.kAbsoluteEncoderID); super.getRotator().setInverted(true); } @@ -146,9 +50,8 @@ public Elbow() { */ public void updateArbitraryFeedForward(double voltage) { if (setpoint != SparkMaxRotatingSubsystem.NO_SETPOINT) { - rotator_controller.setReference(setpoint, - SparkMAXLite.ControlType.kSmartMotion, super.SMARTMOTION_SLOT, - voltage); + rotator_controller.setReference(setpoint, SparkMAXLite.ControlType.kSmartMotion, + super.SMARTMOTION_SLOT, voltage); } } @@ -158,23 +61,27 @@ public double getSetpoint() { } /** - * 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(); 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 + - (frc.team670.robot.constants.RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffsetRadians + - 0.5)) + + 1) * frc.team670.robot.constants.RobotConstants.Arm.Elbow.kGearRatio) + % frc.team670.robot.constants.RobotConstants.Arm.Elbow.kGearRatio; if (calculatedRelativePosition == 0.0 - || Math.abs(360 * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 10.0) { + || Math.abs(360 * ((previousPositionRot - relativePosition) + / this.ROTATOR_GEAR_RATIO)) < 10.0) { clearSetpoint(); REVLibError error = rotator_encoder.setPosition(relativePosition); - SmartDashboard.putNumber("Elbow absEncoder position when reset", absEncoderPosition); + SmartDashboard.putNumber("Elbow absEncoder position when reset", + absEncoderPosition); SmartDashboard.putNumber("Elbow relEncoder position when reset", relativePosition); SmartDashboard.putString("Elbow error", error.toString()); calculatedRelativePosition = relativePosition; @@ -211,9 +118,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,10 +132,11 @@ public void resetPositionFromAbsolute() { setEncoderPositionFromAbsolute(); } - @Override - public boolean hasReachedTargetPosition() { - return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, RobotConstants.ELBOW_ALLOWED_ERR_DEG)); - } + // @Override + // public boolean hasReachedTargetPosition() { + // return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, + // frc.team670.robot.constants.RobotConstants.Arm.Elbow.kAllowedErrorDegrees)); + // } @Override public void setSystemTargetAngleInDegrees(double targetAngle) { @@ -238,8 +145,10 @@ public void setSystemTargetAngleInDegrees(double 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) > frc.team670.robot.constants.RobotConstants.Arm.Elbow.kMaxOverrideDegreees) { + this.offset = frc.team670.robot.constants.RobotConstants.Arm.Elbow.kMaxOverrideDegreees + * this.offset / Math.abs(this.offset); } else { this.offset = offset; } @@ -271,9 +180,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. @@ -287,8 +200,8 @@ public void mustangPeriodic() { } } else if (!relativePositionIsSet) { double position = super.rotator_encoder.getPosition(); - Logger.consoleLog("Elbow relative position = " + position + ", calculatedRelativePosition = " - + calculatedRelativePosition); + Logger.consoleLog("Elbow relative position = " + position + + ", calculatedRelativePosition = " + calculatedRelativePosition); if (Math.abs(position - calculatedRelativePosition) < 0.5) { relativePositionIsSet = true; } else { 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..15036a79 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 @@ -13,12 +13,10 @@ 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 */ @@ -38,111 +36,23 @@ 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( + frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kFollowerMotorID, rotator, + true); follower.setIdleMode(IdleMode.kBrake); - absEncoder = new DutyCycleEncoder(RobotMap.SHOULDER_ABSOLUTE_ENCODER); + absEncoder = new DutyCycleEncoder( + frc.team670.robot.constants.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; @@ -155,9 +65,8 @@ public boolean isRelativePositionSet() { */ public void updateArbitraryFeedForward(double voltage) { if (setpoint != SparkMaxRotatingSubsystem.NO_SETPOINT) { - rotator_controller.setReference(setpoint, - SparkMAXLite.ControlType.kSmartMotion, super.SMARTMOTION_SLOT, - voltage); + rotator_controller.setReference(setpoint, SparkMAXLite.ControlType.kSmartMotion, + super.SMARTMOTION_SLOT, voltage); } } @@ -173,8 +82,11 @@ public void setSystemTargetAngleInDegrees(double 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) > frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kMaxOverrideDegrees) { + this.offset = + frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kMaxOverrideDegrees + * this.offset / Math.abs(this.offset); } else { this.offset = offset; } @@ -201,7 +113,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; } @@ -232,24 +145,28 @@ public void debugSubsystem() { } /** - * 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(); 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; + double relativePosition = ((-1 * (absEncoderPosition + - (frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffsetRadians + - 0.5)) + + 1) * frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kGearRatio) + % frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kGearRatio; - if (calculatedRelativePosition == 0.0 - || Math.abs(360 * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 5.0) { + if (calculatedRelativePosition == 0.0 || Math.abs(360 + * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 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); + SmartDashboard.putNumber("Shoulder absEncoder position when reset", + absEncoderPosition); + SmartDashboard.putNumber("Shoulder relEncoder position when reset", + relativePosition); SmartDashboard.putString("Shoulder error", error.toString()); calculatedRelativePosition = relativePosition; } @@ -265,16 +182,20 @@ public double getSetpoint() { @Override public boolean hasReachedTargetPosition() { return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, - RobotConstants.SHOULDER_ALLOWED_ERR_DEG)); + frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kAllowedErrorDegrees)); } @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. @@ -288,8 +209,8 @@ public void mustangPeriodic() { } } else if (!relativePositionIsSet) { double position = super.rotator_encoder.getPosition(); - Logger.consoleLog("Shoulder relative position = " + position + ", calculatedRelativePosition = " - + calculatedRelativePosition); + Logger.consoleLog("Shoulder relative position = " + position + + ", calculatedRelativePosition = " + calculatedRelativePosition); Logger.consoleLog("Shoulder relativePositionIsSet = " + this.relativePositionIsSet); if (Math.abs(position - calculatedRelativePosition) < 0.5) { relativePositionIsSet = true; 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..77012315 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 @@ -12,9 +12,8 @@ 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; +import frc.team670.mustanglib.utils.motorcontroller.SparkMAXLite; /** * Represents the wrist joint. Uses only one motor @@ -36,100 +35,11 @@ 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( + frc.team670.robot.constants.RobotConstants.Arm.Wrist.kAbsoluteEncoderID); super.getRotator().setInverted(false); @@ -137,34 +47,39 @@ public Wrist() { /** * 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); + 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() { double absEncoderPosition = absEncoder.getAbsolutePosition(); double previousPositionRot = super.rotator_encoder.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 + - (frc.team670.robot.constants.RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffsetRadians + - 0.5)) + + 1) * frc.team670.robot.constants.RobotConstants.Arm.Wrist.kGearRatio) + % frc.team670.robot.constants.RobotConstants.Arm.Wrist.kGearRatio; + + if (calculatedRelativePosition == 0.0 + || Math.abs(360 * ((previousPositionRot - relativePosition) + / this.ROTATOR_GEAR_RATIO)) < 20.0) { clearSetpoint(); - + REVLibError error = rotator_encoder.setPosition(relativePosition); - SmartDashboard.putNumber("Wrist absEncoder position when reset", absEncoderPosition); + SmartDashboard.putNumber("Wrist absEncoder position when reset", + absEncoderPosition); SmartDashboard.putNumber("Wrist relEncoder position when reset", relativePosition); SmartDashboard.putString("Wrist error", error.toString()); calculatedRelativePosition = relativePosition; @@ -182,28 +97,28 @@ public boolean getTimeout() { public HealthState checkHealth() { REVLibError rotatorError = super.rotator.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,13 +133,14 @@ public void resetPositionFromAbsolute() { @Override public boolean hasReachedTargetPosition() { - return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, RobotConstants.WRIST_ALLOWED_ERR_DEG)); + return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, + frc.team670.robot.constants.RobotConstants.Arm.Wrist.kAllowedErrorDegrees)); } @Override public void debugSubsystem() { double relativePosition = super.rotator_encoder.getPosition(); - + SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); @@ -236,9 +152,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. @@ -258,7 +178,7 @@ public void mustangPeriodic() { } } } - + public void sendAngleToDashboard() { SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); } From 0fbc6686a3830862d191b0da85d39676f634cffe Mon Sep 17 00:00:00 2001 From: Ethan Date: Fri, 7 Apr 2023 23:42:22 -0700 Subject: [PATCH 02/33] removed robot map --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- .../main/java/frc/team670/robot/constants/RobotMap.java | 9 --------- 2 files changed, 1 insertion(+), 10 deletions(-) delete mode 100644 2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 9b01debe..775595e8 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 9b01debe123386f7b25a6feaf5f17747cdffc9bb +Subproject commit 775595e8940497ff801af66845f080e9837ed24b 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 235f5fd0..00000000 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotMap.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.team670.robot.constants; - -public class RobotMap { - - - - - -} From 137fcf4f3e176f8b0e16bd0d05aa31375c143430 Mon Sep 17 00:00:00 2001 From: Ethan Date: Sat, 8 Apr 2023 00:04:16 -0700 Subject: [PATCH 03/33] swerve drive config refactor --- .../team670/robot/commands/drivebase/MoveToPose.java | 3 ++- .../robot/commands/drivebase/PathFindMoveToPose.java | 5 +++-- .../frc/team670/robot/constants/RobotConstants.java | 10 +++------- .../java/frc/team670/robot/subsystems/DriveBase.java | 12 +++--------- .../frc/team670/robot/subsystems/arm/Shoulder.java | 10 +++------- 5 files changed, 14 insertions(+), 26 deletions(-) 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 4513c9df..cbc431a2 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 @@ -57,7 +57,8 @@ public Map getHealthRequirements() { @Override public void initialize() { this.startPose = driveBase.getPose(); - PathPlannerTrajectory traj = PathPlanner.generatePath(frc.team670.robot.constants.RobotConstants.DriveBase.kAutoPathConstraints, + PathPlannerTrajectory traj = PathPlanner.generatePath( + frc.team670.robot.constants.RobotConstants.DriveBase.kAutoPathConstraints, calcStartPoint(endPose), calcEndPoint(startPose)); driveBase.getPoseEstimator().addTrajectory(traj); 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 60ef4278..c6e1db31 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 @@ -46,8 +46,9 @@ public void initialize() { // points in the path. PathPoint[] fullPathPoints = getPathPointsFromNodes(fullPath); - PathPlannerTrajectory trajectory = PathPlanner - .generatePath(frc.team670.robot.constants.RobotConstants.DriveBase.kAutoPathConstraints, Arrays.asList(fullPathPoints)); + PathPlannerTrajectory trajectory = PathPlanner.generatePath( + frc.team670.robot.constants.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/constants/RobotConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java index 6c74fa45..a7e26584 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 @@ -9,10 +9,8 @@ import java.net.SocketException; import java.util.Enumeration; import java.util.Map; -import com.fasterxml.jackson.annotation.Nulls; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.auto.PIDConstants; -import com.pathplanner.lib.auto.SwerveAutoBuilder; import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation3d; @@ -21,14 +19,12 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SerialPort; import frc.team670.mustanglib.constants.RobotConstantsBase; -import frc.team670.mustanglib.constants.SwerveConfig; import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; -import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper; +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.utils.motorcontroller.MotorConfig; -import frc.team670.mustanglib.utils.motorcontroller.MotorConfig.Motor_Type; -import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper.GearRatio; import frc.team670.robot.subsystems.arm.ArmSegment; /** @@ -146,7 +142,7 @@ public static final class DriveBase { public static final double kMaxAngularVelocityRadiansPerSecond = kMaxVelocityMetersPerSecond / Math.hypot(kTrackWidthMeters / 2.0, kWheelBaseMeters / 2.0); - public static final SwerveConfig kConfig = new SwerveConfig(kTrackWidthMeters, + public static final SwerveDrive.Config kConfig = new SwerveDrive.Config(kTrackWidthMeters, kWheelBaseMeters, kMaxVelocityMetersPerSecond, kMaxVoltage, kNAVXPort, kSwerveModuleGearRatio, kFrontLeftModuleDriveMotorID, kFrontLeftModuleSteerMotorID, kFrontLeftModuleSteerEncoderID, kFrontLeftModuleSteerOffsetRadians, 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 ae363d9a..367241ec 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 @@ -7,22 +7,16 @@ import com.pathplanner.lib.PathPlannerTrajectory; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; 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 { @@ -85,11 +79,11 @@ public void debugSubsystem() { } public MustangPPSwerveControllerCommand getFollowTrajectoryCommand(PathPlannerTrajectory traj) { - setSwerveControllerCommand(new MustangPPSwerveControllerCommand(traj, this::getPose, + setmSwerveControllerCommand(new MustangPPSwerveControllerCommand(traj, this::getPose, getSwerveKinematics(), RobotConstants.DriveBase.xController, RobotConstants.DriveBase.yController, RobotConstants.DriveBase.thetaController, this::setModuleStates, new Subsystem[] {this})); - return getSwerveControllerCommand(); + return getmSwerveControllerCommand(); } 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 15036a79..bdccd68b 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,19 +1,15 @@ 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 com.revrobotics.REVLibError; /** * Represents the shoulder joint. The shoulder uses a leader-follower SparkMax pair From 34afc7fd2d6e7acc9b0938b392d6bd425bcee016 Mon Sep 17 00:00:00 2001 From: Ethan Date: Sat, 8 Apr 2023 00:05:53 -0700 Subject: [PATCH 04/33] refactor fix --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- .../src/main/java/frc/team670/robot/subsystems/DriveBase.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 775595e8..68b9b01d 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 775595e8940497ff801af66845f080e9837ed24b +Subproject commit 68b9b01dec9cb7dcb44a6e853a3ec007f3d30d2e 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 367241ec..1182b100 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 @@ -83,7 +83,7 @@ public MustangPPSwerveControllerCommand getFollowTrajectoryCommand(PathPlannerTr getSwerveKinematics(), RobotConstants.DriveBase.xController, RobotConstants.DriveBase.yController, RobotConstants.DriveBase.thetaController, this::setModuleStates, new Subsystem[] {this})); - return getmSwerveControllerCommand(); + return getSwerveControllerCommand(); } From 6adce024bb877e69c8951e9f968c251a97a5b719 Mon Sep 17 00:00:00 2001 From: Ethan Date: Sat, 8 Apr 2023 13:51:04 -0700 Subject: [PATCH 05/33] cleaned redundant code --- .../src/main/java/frc/team670/mustanglib | 2 +- .../frc/team670/robot/RobotContainer.java | 105 ++++++++---------- .../robot/commands/drivebase/MoveToPose.java | 7 +- .../drivebase/PathFindMoveToPose.java | 3 +- .../pathplanner/AutonCalibration.java | 24 +--- .../commands/pathplanner/CenterEngage.java | 10 +- .../pathplanner/CenterEngageSequential.java | 21 +--- .../commands/pathplanner/CenterIntake.java | 11 +- .../robot/commands/pathplanner/ConeCube.java | 10 +- .../commands/pathplanner/ConeCubeCube.java | 14 +-- .../commands/pathplanner/ConeCubeEngage.java | 11 +- .../commands/pathplanner/CubeEngage.java | 9 +- .../commands/pathplanner/ScoreEngage.java | 15 +-- .../robot/commands/vision/IsLockedOn.java | 9 +- .../robot/constants/RobotConstants.java | 21 ++-- .../frc/team670/robot/subsystems/Claw.java | 18 +-- .../team670/robot/subsystems/DriveBase.java | 12 +- .../frc/team670/robot/subsystems/arm/Arm.java | 15 +-- .../team670/robot/subsystems/arm/Elbow.java | 56 +++------- .../robot/subsystems/arm/Shoulder.java | 53 +++------ .../team670/robot/subsystems/arm/Wrist.java | 51 +++------ 21 files changed, 156 insertions(+), 321 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 68b9b01d..cb2239dd 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 68b9b01dec9cb7dcb44a6e853a3ec007f3d30d2e +Subproject commit cb2239dd8e2902c6880040c12b3657f497d2d98c 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 ddd899ab..f01da9b9 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -7,36 +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.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.RobotConstants; import frc.team670.robot.subsystems.Claw; @@ -54,38 +43,38 @@ 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(RobotConstants.kLEDPort, 0, 61); - private final Claw claw = new Claw(led); + private final Vision mVision = new Vision(pd); + private final DriveBase mDriveBase = new DriveBase(); + private final LED mLed = new LED(RobotConstants.kLEDPort, 0, 61); + private final Arm mArm = new Arm(); + private final Claw mClaw = new Claw(mLed); private MustangCommand cableScore, cableEngage, stationScore, stationEngage, centerEngage, centerIntake, scoreMid; - private static OI oi = new OI(); + private static OI kOi = 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, mVision, mArm, mArm.getShoulder(), mArm.getElbow(), mArm.getWrist(), + mClaw, mLed); + kOi.configureButtonBindings(mDriveBase, mVision, mArm, mClaw, mLed); 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); } @@ -93,11 +82,11 @@ public RobotContainer() { public void robotInit() { CameraServer.startAutomaticCapture().setVideoMode(PixelFormat.kYUYV, 160, 120, 30); - driveBase.initVision(vision); - SmartDashboard.putNumber(autonChooser, 0); + mDriveBase.initVision(mVision); + SmartDashboard.putNumber(kAutonChooserString, 0); updateArbitraryFeedForward = new Notifier(new Runnable() { public void run() { - arm.updateArbitraryFeedForward(); + mArm.updateArbitraryFeedForward(); } }); @@ -114,43 +103,43 @@ public MustangCommand getAutonomousCommand() { // return new NonPidAutoLevel(driveBase, true); // return new ConeCube(driveBase, claw, arm, "StationScoreShort"); - SmartDashboard.putBoolean(matchStarted, true); + SmartDashboard.putBoolean(kMatchStartedString, 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); + mLed.solidhsv(LEDColor.LIGHT_BLUE); break; case 1: autonCommand = stationScore; - led.solidhsv(LEDColor.SEXY_YELLOW); + mLed.solidhsv(LEDColor.SEXY_YELLOW); break; case 2: autonCommand = cableEngage; - led.solidhsv(LEDColor.SEXY_PURPLE); + mLed.solidhsv(LEDColor.SEXY_PURPLE); break; case 3: autonCommand = stationEngage; - led.solidhsv(LEDColor.GREEN); + mLed.solidhsv(LEDColor.GREEN); break; case 4: autonCommand = centerEngage; - led.animatedRainbow(false, 10, 10); + mLed.animatedRainbow(false, 10, 10); break; case 5: autonCommand = centerIntake; - led.solidhsv(LEDColor.PINK); + mLed.solidhsv(LEDColor.PINK); break; case 6: autonCommand = scoreMid; - led.animatedMustangRainbow(10, 10); + mLed.animatedMustangRainbow(10, 10); break; default: autonCommand = centerEngage; - led.animatedRainbow(false, 10, 10); + mLed.animatedRainbow(false, 10, 10); } return autonCommand; @@ -173,12 +162,12 @@ public MustangCommand getAutonomousCommand() { @Override public void autonomousInit() { - arm.setStateToStarting(); + mArm.setStateToStarting(); } @Override public void teleopInit() { - arm.clearSetpoint(); + mArm.clearSetpoint(); } @Override @@ -186,7 +175,7 @@ public void testInit() {} @Override public void disabled() { - SmartDashboard.putBoolean(matchStarted, false); + SmartDashboard.putBoolean(kMatchStartedString, false); } @Override @@ -194,35 +183,35 @@ public void disabledPeriodic() { // int selectedPath = (int) // (SmartDashboard.getEntry(autonChooser).getInteger(-1)); - arm.getShoulder().sendAngleToDashboard(); - arm.getElbow().sendAngleToDashboard(); - arm.getWrist().sendAngleToDashboard(); + mArm.getShoulder().sendAngleToDashboard(); + mArm.getElbow().sendAngleToDashboard(); + mArm.getWrist().sendAngleToDashboard(); - int selectedPath = (int) SmartDashboard.getNumber(autonChooser, 0); + int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0); switch (selectedPath) { case 0: - led.blinkhsv(LEDColor.LIGHT_BLUE); + mLed.blinkhsv(LEDColor.LIGHT_BLUE); break; case 1: - led.blinkhsv(LEDColor.SEXY_YELLOW); + mLed.blinkhsv(LEDColor.SEXY_YELLOW); break; case 2: - led.blinkhsv(LEDColor.SEXY_PURPLE); + mLed.blinkhsv(LEDColor.SEXY_PURPLE); break; case 3: - led.blinkhsv(LEDColor.GREEN); + mLed.blinkhsv(LEDColor.GREEN); break; case 4: - led.animatedRainbow(false, 10, 10); + mLed.animatedRainbow(false, 10, 10); break; case 5: - led.blinkhsv(LEDColor.PINK); + mLed.blinkhsv(LEDColor.PINK); break; case 6: - led.animatedMustangRainbow(10, 10); + mLed.animatedMustangRainbow(10, 10); break; default: - led.animatedRainbow(false, 10, 10); + mLed.animatedRainbow(false, 10, 10); } } @@ -264,7 +253,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 cbc431a2..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,7 @@ 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 +55,7 @@ public Map getHealthRequirements() { public void initialize() { this.startPose = driveBase.getPose(); PathPlannerTrajectory traj = PathPlanner.generatePath( - frc.team670.robot.constants.RobotConstants.DriveBase.kAutoPathConstraints, + RobotConstants.DriveBase.kAutoPathConstraints, calcStartPoint(endPose), calcEndPoint(startPose)); driveBase.getPoseEstimator().addTrajectory(traj); 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 c6e1db31..d6d68c58 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 @@ -15,6 +15,7 @@ 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; @@ -47,7 +48,7 @@ public void initialize() { PathPoint[] fullPathPoints = getPathPointsFromNodes(fullPath); PathPlannerTrajectory trajectory = PathPlanner.generatePath( - frc.team670.robot.constants.RobotConstants.DriveBase.kAutoPathConstraints, + RobotConstants.DriveBase.kAutoPathConstraints, Arrays.asList(fullPathPoints)); driveBase.getPoseEstimator().addTrajectory(trajectory); pathDrivingCommand = driveBase.getFollowTrajectoryCommand(trajectory); 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 fd6f4e69..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,17 +25,8 @@ public AutonCalibration(DriveBase driveBase, String pathName) { this.pathName = pathName; List trajectoryGroup = PathPlanner.loadPathGroup(pathName, 2.5, 2); - HashMap eventMap = new HashMap<>(); - - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = - new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, - driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, - eventMap, true, new Subsystem[] {driveBase}); - - CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(new HashMap<>()).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 64402620..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 @@ -45,15 +45,7 @@ public CenterEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { 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.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, 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 ab04de7c..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,26 +3,19 @@ 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; @@ -37,16 +30,10 @@ 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.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, driveBase::setModuleStates, null, - true, new Subsystem[] {driveBase}); - + + CommandBase fullAuto = driveBase.getAutoBuilderFromEvents(null).fullAuto(trajectoryGroup); addCommands(new SequentialCommandGroup(new MoveToTarget(arm, ArmState.SCORE_MID), - new ClawInstantEject(claw), new MoveToTarget(arm, ArmState.STOWED), - autoBuilder.fullAuto(trajectoryGroup), + 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 db1bdcd5..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 @@ -48,15 +48,8 @@ public CenterIntake(DriveBase driveBase, Claw claw, Arm arm, String pathName) { 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.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, 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 d4564fe0..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 @@ -48,15 +48,7 @@ public ConeCube(DriveBase driveBase, Claw claw, Arm arm, String pathName) { // after testing. eventMap.put("moveToStowed", new MoveToTarget(arm, ArmState.STOWED)); - SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); - - SwerveAutoBuilder autoBuilder = - new SwerveAutoBuilder(driveBase::getPose, driveBase::resetOdometry, - driveBaseKinematics, RobotConstants.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, 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 0cdcc99b..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,20 +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.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, 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 297fa3b4..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 @@ -51,16 +51,7 @@ public ConeCubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) // 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.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, 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/CubeEngage.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java index 7f670d0d..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 @@ -58,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 100678ff..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,8 @@ 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.DriveBase.kAutonTranslationPID, - RobotConstants.DriveBase.kAutonThetaPID, 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)); 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 c2f49631..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; 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 a7e26584..e69ae55c 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 @@ -8,9 +8,11 @@ import java.net.NetworkInterface; import java.net.SocketException; import java.util.Enumeration; +import java.util.HashMap; import java.util.Map; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.auto.PIDConstants; +import com.pathplanner.lib.auto.SwerveAutoBuilder; import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation3d; @@ -18,6 +20,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team670.mustanglib.constants.RobotConstantsBase; import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; @@ -44,14 +47,14 @@ public final class RobotConstants extends RobotConstantsBase { * 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). + * 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"; @@ -162,8 +165,6 @@ public static final class DriveBase { public static final PIDController thetaController = new PIDController(0.2, 0, 0); public static final PathConstraints kAutoPathConstraints = new PathConstraints( kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); - - // public static final SwerveAutoBuilder autoBuilder = } 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 507a1e45..e3fcd222 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 @@ -5,7 +5,7 @@ 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 com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.wpilibj.DriverStation; @@ -27,13 +27,13 @@ public enum Status { private int ejectCounter = 0; private boolean isFull = false; private double ejectingSpeed = - frc.team670.robot.constants.RobotConstants.Arm.Claw.kEjectingSpeed; + RobotConstants.Arm.Claw.kEjectingSpeed; private LED led; public Claw(LED led) { motor = SparkMAXFactory.buildSparkMAX( - frc.team670.robot.constants.RobotConstants.Arm.Claw.kMotorID, + RobotConstants.Arm.Claw.kMotorID, SparkMAXFactory.defaultConfig, Motor_Type.NEO); status = Status.IDLE; this.led = led; @@ -56,7 +56,7 @@ public boolean isFull() { } public void startEjecting() { - this.startEjecting(frc.team670.robot.constants.RobotConstants.Arm.Claw.kEjectingSpeed); + this.startEjecting(RobotConstants.Arm.Claw.kEjectingSpeed); } /** @@ -111,14 +111,14 @@ public void mustangPeriodic() { switch (status) { case IDLE: - motor.set(frc.team670.robot.constants.RobotConstants.Arm.Claw.kIdleSpeed); + motor.set(RobotConstants.Arm.Claw.kIdleSpeed); break; case INTAKING: - motor.set(frc.team670.robot.constants.RobotConstants.Arm.Claw.kRollingSpeed); + motor.set(RobotConstants.Arm.Claw.kRollingSpeed); if (motor - .getOutputCurrent() > frc.team670.robot.constants.RobotConstants.Arm.Claw.kCurrentMax) { + .getOutputCurrent() > RobotConstants.Arm.Claw.kCurrentMax) { currentSpikeCounter++; - if (currentSpikeCounter > frc.team670.robot.constants.RobotConstants.Arm.Claw.kCurrentSpikeIterations) { + if (currentSpikeCounter > RobotConstants.Arm.Claw.kCurrentSpikeIterations) { isFull = true; if (DriverStation.isTeleopEnabled()) { led.solidhsv(LEDColor.GREEN); @@ -137,7 +137,7 @@ public void mustangPeriodic() { case EJECTING: motor.set(ejectingSpeed); ejectCounter++; - if (ejectCounter > frc.team670.robot.constants.RobotConstants.Arm.Claw.kEjectIterations) { + if (ejectCounter > RobotConstants.Arm.Claw.kEjectIterations) { ejectCounter = 0; isFull = false; if (DriverStation.isTeleopEnabled()) { 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 1182b100..6889c4c7 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 @@ -16,17 +16,21 @@ import frc.team670.mustanglib.swervelib.SwerveModule; import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand; import frc.team670.mustanglib.utils.MustangController; +import frc.team670.robot.constants.OI; import frc.team670.robot.constants.RobotConstants; public class DriveBase extends SwerveDrive { - - + private static DriveBase mInstance; private MustangCommand defaultCommand; private MustangController mController; - public DriveBase(MustangController mustangController) { + public static DriveBase getInstance() { + return mInstance == null ? new DriveBase() : mInstance; + } + + public DriveBase() { super(RobotConstants.DriveBase.kConfig); - this.mController = mustangController; + this.mController = OI.getDriverController(); } /** diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index e432c542..9d2e8372 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -7,6 +7,7 @@ import java.util.PriorityQueue; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; +import frc.team670.robot.constants.RobotConstants; /** * Represents the whole Arm system, containing multiple joints. Models the arm as a state machine. @@ -21,10 +22,6 @@ public class Arm extends MustangSubsystemBase { private boolean initializedState; private VoltageCalculator voltageCalculator; - private double elbowOffset; - private double shoulderOffset; - private double wristOffset; - private boolean hasSetShoulderTarget = true; private boolean hasSetElbowTarget = true; private boolean hasSetWristTarget = true; @@ -91,9 +88,9 @@ public Arm() { this.targetState = ArmState.STOWED; this.initializedState = false; this.voltageCalculator = new VoltageCalculator( - frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kSegment, - frc.team670.robot.constants.RobotConstants.Arm.Elbow.kSegment, - frc.team670.robot.constants.RobotConstants.Arm.Wrist.kWristSegment); + RobotConstants.Arm.Shoulder.kSegment, + RobotConstants.Arm.Elbow.kSegment, + RobotConstants.Arm.Wrist.kWristSegment); init(); } @@ -176,8 +173,8 @@ public void resetPositionFromAbsolute() { */ 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; 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 e0c64da0..66c21e53 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,14 +1,11 @@ package frc.team670.robot.subsystems.arm; -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.SparkMAXLite; import frc.team670.robot.constants.RobotConstants; /** @@ -39,25 +36,8 @@ public class Elbow extends SparkMaxRotatingSubsystem { public Elbow() { super(RobotConstants.Arm.Elbow.kConfig); absEncoder = new DutyCycleEncoder( - frc.team670.robot.constants.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; + RobotConstants.Arm.Elbow.kAbsoluteEncoderID); + super.getmRotator().setInverted(true); } /** @@ -66,20 +46,20 @@ public double getSetpoint() { */ 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 - - (frc.team670.robot.constants.RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffsetRadians + - (RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffsetRadians - 0.5)) - + 1) * frc.team670.robot.constants.RobotConstants.Arm.Elbow.kGearRatio) - % frc.team670.robot.constants.RobotConstants.Arm.Elbow.kGearRatio; + + 1) * RobotConstants.Arm.Elbow.kGearRatio) + % RobotConstants.Arm.Elbow.kGearRatio; if (calculatedRelativePosition == 0.0 || Math.abs(360 * ((previousPositionRot - relativePosition) - / this.ROTATOR_GEAR_RATIO)) < 10.0) { + / kConfig.kRotatorGearRatio())) < 10.0) { clearSetpoint(); - REVLibError error = rotator_encoder.setPosition(relativePosition); + REVLibError error = mEncoder.setPosition(relativePosition); SmartDashboard.putNumber("Elbow absEncoder position when reset", absEncoderPosition); SmartDashboard.putNumber("Elbow relEncoder position when reset", relativePosition); @@ -97,7 +77,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()); @@ -134,8 +114,8 @@ public void resetPositionFromAbsolute() { // @Override // public boolean hasReachedTargetPosition() { - // return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, - // frc.team670.robot.constants.RobotConstants.Arm.Elbow.kAllowedErrorDegrees)); + // return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, + // RobotConstants.Arm.Elbow.kAllowedErrorDegrees)); // } @Override @@ -146,8 +126,8 @@ public void setSystemTargetAngleInDegrees(double targetAngle) { private void setOffset(double offset) { if (Math.abs( - offset) > frc.team670.robot.constants.RobotConstants.Arm.Elbow.kMaxOverrideDegreees) { - this.offset = frc.team670.robot.constants.RobotConstants.Arm.Elbow.kMaxOverrideDegreees + offset) > RobotConstants.Arm.Elbow.kMaxOverrideDegreees) { + this.offset = RobotConstants.Arm.Elbow.kMaxOverrideDegreees * this.offset / Math.abs(this.offset); } else { this.offset = offset; @@ -167,13 +147,13 @@ 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(current, super.getRotator().getOutputCurrent()); + SmartDashboard.putNumber(setpointRot, mSetpoint); + SmartDashboard.putNumber(current, super.getmRotator().getOutputCurrent()); } @Override @@ -199,13 +179,13 @@ public void mustangPeriodic() { hasSetAbsolutePosition = true; } } else if (!relativePositionIsSet) { - double position = super.rotator_encoder.getPosition(); + 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 bdccd68b..eacdc1a3 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 @@ -37,13 +37,13 @@ public class Shoulder extends SparkMaxRotatingSubsystem { public Shoulder() { super(RobotConstants.Arm.Shoulder.kConfig); - super.getRotator().setInverted(true); + super.getmRotator().setInverted(true); follower = SparkMAXFactory.setPermanentFollower( - frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kFollowerMotorID, rotator, + RobotConstants.Arm.Shoulder.kFollowerMotorID, mRotator, true); follower.setIdleMode(IdleMode.kBrake); absEncoder = new DutyCycleEncoder( - frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kAbsoluteEncoderID); + RobotConstants.Arm.Shoulder.kAbsoluteEncoderID); } /** @@ -54,18 +54,6 @@ 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; @@ -79,9 +67,9 @@ public void setSystemTargetAngleInDegrees(double targetAngle) { private void setOffset(double offset) { if (Math.abs( - offset) > frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kMaxOverrideDegrees) { + offset) > RobotConstants.Arm.Shoulder.kMaxOverrideDegrees) { this.offset = - frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kMaxOverrideDegrees + RobotConstants.Arm.Shoulder.kMaxOverrideDegrees * this.offset / Math.abs(this.offset); } else { this.offset = offset; @@ -101,7 +89,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); @@ -132,11 +120,11 @@ 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); } @@ -146,19 +134,19 @@ public void debugSubsystem() { */ 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 - - (frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffsetRadians + - (RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffsetRadians - 0.5)) - + 1) * frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kGearRatio) - % frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kGearRatio; + + 1) * RobotConstants.Arm.Shoulder.kGearRatio) + % RobotConstants.Arm.Shoulder.kGearRatio; if (calculatedRelativePosition == 0.0 || Math.abs(360 - * ((previousPositionRot - relativePosition) / this.ROTATOR_GEAR_RATIO)) < 5.0) { + * ((previousPositionRot - relativePosition) / kConfig.kRotatorGearRatio())) < 5.0) { clearSetpoint(); - REVLibError error = rotator_encoder.setPosition(relativePosition); + REVLibError error = mEncoder.setPosition(relativePosition); SmartDashboard.putNumber("Shoulder absEncoder position when reset", absEncoderPosition); SmartDashboard.putNumber("Shoulder relEncoder position when reset", @@ -170,15 +158,10 @@ 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, - frc.team670.robot.constants.RobotConstants.Arm.Shoulder.kAllowedErrorDegrees)); + return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, + RobotConstants.Arm.Shoulder.kAllowedErrorDegrees)); } @Override @@ -204,14 +187,14 @@ public void mustangPeriodic() { hasSetAbsolutePosition = true; } } else if (!relativePositionIsSet) { - double position = super.rotator_encoder.getPosition(); + 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 77012315..f3082dfc 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,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.robot.constants.RobotConstants; -import frc.team670.mustanglib.utils.motorcontroller.SparkMAXLite; /** * Represents the wrist joint. Uses only one motor @@ -39,45 +33,34 @@ public class Wrist extends SparkMaxRotatingSubsystem { public Wrist() { super(RobotConstants.Arm.Wrist.kConfig); absEncoder = new DutyCycleEncoder( - frc.team670.robot.constants.RobotConstants.Arm.Wrist.kAbsoluteEncoderID); - super.getRotator().setInverted(false); - + RobotConstants.Arm.Wrist.kAbsoluteEncoderID); + super.getmRotator().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() */ - 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 - - (frc.team670.robot.constants.RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffsetRadians + - (RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffsetRadians - 0.5)) - + 1) * frc.team670.robot.constants.RobotConstants.Arm.Wrist.kGearRatio) - % frc.team670.robot.constants.RobotConstants.Arm.Wrist.kGearRatio; + + 1) * RobotConstants.Arm.Wrist.kGearRatio) + % RobotConstants.Arm.Wrist.kGearRatio; if (calculatedRelativePosition == 0.0 || Math.abs(360 * ((previousPositionRot - relativePosition) - / this.ROTATOR_GEAR_RATIO)) < 20.0) { + / kConfig.kRotatorGearRatio())) < 20.0) { clearSetpoint(); - REVLibError error = rotator_encoder.setPosition(relativePosition); + REVLibError error = mEncoder.setPosition(relativePosition); SmartDashboard.putNumber("Wrist absEncoder position when reset", absEncoderPosition); SmartDashboard.putNumber("Wrist relEncoder position when reset", relativePosition); @@ -95,7 +78,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("Wrist error! Rotator Error is " + rotatorError.toString()); @@ -133,18 +116,18 @@ public void resetPositionFromAbsolute() { @Override public boolean hasReachedTargetPosition() { - return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, - frc.team670.robot.constants.RobotConstants.Arm.Wrist.kAllowedErrorDegrees)); + return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, + RobotConstants.Arm.Wrist.kAllowedErrorDegrees)); } @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); } @@ -171,10 +154,10 @@ 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); } } } From af508a7d5f877e36abcf4e56a7336c1f7c8e4aee Mon Sep 17 00:00:00 2001 From: Ethan Date: Sat, 8 Apr 2023 14:28:00 -0700 Subject: [PATCH 06/33] cleaned swerve code and removed unreachable logic --- .../java/frc/team670/robot/constants/OI.java | 237 +++++++++--------- .../team670/robot/subsystems/DriveBase.java | 42 +--- 2 files changed, 124 insertions(+), 155 deletions(-) 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 3d150fb3..787306ed 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 @@ -27,118 +27,127 @@ 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)); - - } + + // 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(new XboxSwerveDrive(driveBase, driverController, + RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, + RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond)); + + 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)); + + } } 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 6889c4c7..9c83af3e 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 @@ -21,8 +21,6 @@ public class DriveBase extends SwerveDrive { private static DriveBase mInstance; - private MustangCommand defaultCommand; - private MustangController mController; public static DriveBase getInstance() { return mInstance == null ? new DriveBase() : mInstance; @@ -30,28 +28,6 @@ public static DriveBase getInstance() { public DriveBase() { super(RobotConstants.DriveBase.kConfig); - this.mController = OI.getDriverController(); - } - - /** - * Makes the DriveBase's default command initialize teleop - */ - public void initDefaultCommand() { // 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, - RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, - RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond); - MustangScheduler.getInstance().setDefaultCommand(this, defaultCommand); - } - - public void cancelDefaultCommand() { - MustangScheduler.getInstance().cancel(defaultCommand); - } - - public MustangCommand getDefaultMustangCommand() { - return defaultCommand; } public void mustangPeriodic() { @@ -59,11 +35,6 @@ public void mustangPeriodic() { SmartDashboard.putNumber("pitch", getPitch()); } - // @Override - // public void periodic() { - // return; - // } - @Override public HealthState checkHealth() { for (SwerveModule curr : getModules()) { @@ -78,17 +49,6 @@ public HealthState checkHealth() { } @Override - public void debugSubsystem() { - - } - - public MustangPPSwerveControllerCommand getFollowTrajectoryCommand(PathPlannerTrajectory traj) { - setmSwerveControllerCommand(new MustangPPSwerveControllerCommand(traj, this::getPose, - getSwerveKinematics(), RobotConstants.DriveBase.xController, - RobotConstants.DriveBase.yController, RobotConstants.DriveBase.thetaController, - this::setModuleStates, new Subsystem[] {this})); - return getSwerveControllerCommand(); - - } + public void debugSubsystem() {} } From edee5e1d6e687c68d2281e5646e572b78989eece Mon Sep 17 00:00:00 2001 From: Ethan Date: Mon, 17 Apr 2023 17:44:46 -0700 Subject: [PATCH 07/33] dynamic current limit override --- .../src/main/java/frc/team670/mustanglib | 2 +- .../robot/constants/RobotConstants.java | 22 +++++++++++-------- .../team670/robot/subsystems/DriveBase.java | 8 ------- 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index cb2239dd..2063bd24 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit cb2239dd8e2902c6880040c12b3657f497d2d98c +Subproject commit 2063bd244ba670453d559fdd5915a33d934eea5b 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 e69ae55c..1ce4a166 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 @@ -25,6 +25,7 @@ import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper.GearRatio; +import frc.team670.mustanglib.swervelib.Mk4ModuleConfiguration; import frc.team670.mustanglib.swervelib.ModuleConfiguration; import frc.team670.mustanglib.swervelib.SdsModuleConfigurations; import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; @@ -131,6 +132,8 @@ public static final class DriveBase { public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI * 8; 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 * * * @@ -146,15 +149,16 @@ public static final class DriveBase { / Math.hypot(kTrackWidthMeters / 2.0, kWheelBaseMeters / 2.0); public static final SwerveDrive.Config kConfig = new SwerveDrive.Config(kTrackWidthMeters, - kWheelBaseMeters, kMaxVelocityMetersPerSecond, kMaxVoltage, kNAVXPort, - kSwerveModuleGearRatio, kFrontLeftModuleDriveMotorID, kFrontLeftModuleSteerMotorID, - kFrontLeftModuleSteerEncoderID, kFrontLeftModuleSteerOffsetRadians, - kFrontRightModuleDriveMotorID, kFrontRightModuleSteerMotorID, - kFrontRightModuleSteerEncoderID, kFrontRightModuleSteerOffsetRadians, - kBackLeftModuleDriveMotorID, kBackLeftModuleSteerMotorID, - kBackLeftModuleSteerEncoderID, kBackLeftModuleSteerOffsetRadians, - kBackRightModuleDriveMotorID, kBackRightModuleSteerMotorID, - kBackRightModuleSteerEncoderID, kBackRightModuleSteerOffsetRadians); + 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); 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 9c83af3e..8b001fca 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,19 +4,11 @@ package frc.team670.robot.subsystems; -import com.pathplanner.lib.PathPlannerTrajectory; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.team670.mustanglib.commands.MustangCommand; -import frc.team670.mustanglib.commands.MustangScheduler; -import frc.team670.mustanglib.commands.drive.teleop.XboxSwerveDrive; import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; import frc.team670.mustanglib.swervelib.SwerveModule; -import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand; -import frc.team670.mustanglib.utils.MustangController; -import frc.team670.robot.constants.OI; import frc.team670.robot.constants.RobotConstants; public class DriveBase extends SwerveDrive { From 22eb1f6213809871c1e36f126b69fc9da47d4b01 Mon Sep 17 00:00:00 2001 From: Ethan Date: Mon, 17 Apr 2023 17:56:31 -0700 Subject: [PATCH 08/33] move pathfinder to mustanglib --- .../src/main/java/frc/team670/mustanglib | 2 +- .../drivebase/PathFindMoveToPose.java | 4 +- .../robot/constants/FieldConstants.java | 4 +- .../java/frc/team670/robot/constants/OI.java | 7 - .../team670/robot/pathfinder/Obstacle.java | 1079 ----------------- .../pathfinder/ObstacleAvoidanceAStarMap.java | 125 -- .../team670/robot/pathfinder/PoseEdge.java | 39 - .../team670/robot/pathfinder/PoseNode.java | 65 - 8 files changed, 5 insertions(+), 1320 deletions(-) delete mode 100644 2023-Robot/src/main/java/frc/team670/robot/pathfinder/Obstacle.java delete mode 100644 2023-Robot/src/main/java/frc/team670/robot/pathfinder/ObstacleAvoidanceAStarMap.java delete mode 100644 2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseEdge.java delete mode 100644 2023-Robot/src/main/java/frc/team670/robot/pathfinder/PoseNode.java diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 2063bd24..90d9763d 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 2063bd244ba670453d559fdd5915a33d934eea5b +Subproject commit 90d9763dd0a9a142493d699834ffb983ecf75ec1 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 d6d68c58..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 { 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 2eba37c3..f0b8dd01 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,8 +15,8 @@ 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 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 787306ed..645ddf98 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 @@ -109,11 +109,6 @@ public void configureButtonBindings(MustangSubsystemBase... subsystemBases) { 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 @@ -121,8 +116,6 @@ public void configureButtonBindings(MustangSubsystemBase... subsystemBases) { 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)); 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; - } -} From 014837935d2e0cf91ab5f70ce7d4ac0940ec044e Mon Sep 17 00:00:00 2001 From: Ethan Date: Mon, 17 Apr 2023 21:11:07 -0700 Subject: [PATCH 09/33] fixed --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- 2023-Robot/src/main/java/frc/team670/robot/constants/OI.java | 1 - .../src/main/java/frc/team670/robot/subsystems/DriveBase.java | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 90d9763d..23bda538 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 90d9763dd0a9a142493d699834ffb983ecf75ec1 +Subproject commit 23bda53870905d5a7986fdc912adfb61d4b73cb3 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 645ddf98..cdb37738 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 @@ -10,7 +10,6 @@ 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; 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 8b001fca..84e80c45 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 @@ -14,7 +14,7 @@ public class DriveBase extends SwerveDrive { private static DriveBase mInstance; - public static DriveBase getInstance() { + public static synchronized DriveBase getInstance() { return mInstance == null ? new DriveBase() : mInstance; } From ef003330eca6f925e51d07996871faff6682fe05 Mon Sep 17 00:00:00 2001 From: Ethan Date: Thu, 20 Apr 2023 10:48:59 -0700 Subject: [PATCH 10/33] mass refactor, integrate mustanglib removed files --- .../src/main/java/frc/team670/mustanglib | 2 +- .../frc/team670/robot/RobotContainer.java | 95 +-- .../robot/commands/leds/SetColorPurple.java | 2 +- .../robot/commands/leds/SetColorYellow.java | 2 +- .../robot/constants/FieldConstants.java | 746 +++++++++--------- .../java/frc/team670/robot/constants/OI.java | 31 +- .../robot/constants/RobotConstants.java | 181 ++--- .../frc/team670/robot/subsystems/Claw.java | 11 +- .../frc/team670/robot/subsystems/LED.java | 54 +- .../frc/team670/robot/subsystems/Vision.java | 16 +- .../frc/team670/robot/subsystems/arm/Arm.java | 6 + 11 files changed, 552 insertions(+), 594 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 23bda538..b7cf2bca 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 23bda53870905d5a7986fdc912adfb61d4b73cb3 +Subproject commit b7cf2bca81dcbfb8ca0d1876ac2cfcfd7aae19c8 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 f01da9b9..30e2d005 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -11,13 +11,10 @@ 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.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.MustangController; import frc.team670.robot.commands.pathplanner.CenterEngageSequential; import frc.team670.robot.commands.pathplanner.CenterIntake; @@ -27,7 +24,6 @@ import frc.team670.robot.commands.pathplanner.ScoreEngage; import frc.team670.robot.commands.pathplanner.ScoreMid; 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.LED; @@ -35,24 +31,21 @@ import frc.team670.robot.subsystems.arm.Arm; /** - * RobotContainer is where we put the high-level code for the robot. It contains subsystems, OI + * RobotContainer is where we put the high-level code for the robot. It contains + * subsystems, OI * devices, etc, and has required methods (autonomousInit, periodic, etc) */ public class RobotContainer extends RobotContainerBase { - - private final PowerDistribution pd = new PowerDistribution(1, ModuleType.kCTRE); - - private final Vision mVision = new Vision(pd); - private final DriveBase mDriveBase = new DriveBase(); - private final LED mLed = new LED(RobotConstants.kLEDPort, 0, 61); - private final Arm mArm = new Arm(); - private final Claw mClaw = new Claw(mLed); + 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 kOi = new OI(); private Notifier updateArbitraryFeedForward; private final String kMatchStartedString = "match-started"; @@ -62,7 +55,7 @@ public RobotContainer() { super(); addSubsystem(mDriveBase, mVision, mArm, mArm.getShoulder(), mArm.getElbow(), mArm.getWrist(), mClaw, mLed); - kOi.configureButtonBindings(mDriveBase, mVision, mArm, mClaw, mLed); + OI.configureButtonBindings(); for (MustangSubsystemBase subsystem : getSubsystems()) { subsystem.setDebugSubsystem(true); @@ -84,11 +77,10 @@ public void robotInit() { mDriveBase.initVision(mVision); SmartDashboard.putNumber(kAutonChooserString, 0); - updateArbitraryFeedForward = new Notifier(new Runnable() { - public void run() { - mArm.updateArbitraryFeedForward(); - } - }); + updateArbitraryFeedForward = new Notifier( + () -> { + mArm.updateArbitraryFeedForward(); + }); updateArbitraryFeedForward.startPeriodic(0.01); } @@ -100,64 +92,37 @@ public void run() { */ @Override public MustangCommand getAutonomousCommand() { - // return new NonPidAutoLevel(driveBase, true); - // return new ConeCube(driveBase, claw, arm, "StationScoreShort"); - SmartDashboard.putBoolean(kMatchStartedString, true); - // SmartDashboard.putNumber(autonChooser, 0); int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0); MustangCommand autonCommand; switch (selectedPath) { case 0: autonCommand = cableScore; - mLed.solidhsv(LEDColor.LIGHT_BLUE); break; case 1: autonCommand = stationScore; - mLed.solidhsv(LEDColor.SEXY_YELLOW); break; case 2: autonCommand = cableEngage; - mLed.solidhsv(LEDColor.SEXY_PURPLE); break; case 3: autonCommand = stationEngage; - mLed.solidhsv(LEDColor.GREEN); break; case 4: autonCommand = centerEngage; - mLed.animatedRainbow(false, 10, 10); break; case 5: autonCommand = centerIntake; - mLed.solidhsv(LEDColor.PINK); break; case 6: autonCommand = scoreMid; - mLed.animatedMustangRainbow(10, 10); break; default: autonCommand = centerEngage; - mLed.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 @@ -171,7 +136,8 @@ public void teleopInit() { } @Override - public void testInit() {} + public void testInit() { + } @Override public void disabled() { @@ -180,40 +146,12 @@ public void disabled() { @Override public void disabledPeriodic() { - // int selectedPath = (int) - // (SmartDashboard.getEntry(autonChooser).getInteger(-1)); - mArm.getShoulder().sendAngleToDashboard(); mArm.getElbow().sendAngleToDashboard(); mArm.getWrist().sendAngleToDashboard(); int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0); - switch (selectedPath) { - case 0: - mLed.blinkhsv(LEDColor.LIGHT_BLUE); - break; - case 1: - mLed.blinkhsv(LEDColor.SEXY_YELLOW); - break; - case 2: - mLed.blinkhsv(LEDColor.SEXY_PURPLE); - break; - case 3: - mLed.blinkhsv(LEDColor.GREEN); - break; - case 4: - mLed.animatedRainbow(false, 10, 10); - break; - case 5: - mLed.blinkhsv(LEDColor.PINK); - break; - case 6: - mLed.animatedMustangRainbow(10, 10); - break; - default: - mLed.animatedRainbow(false, 10, 10); - - } + mLed.updateAutonPathColor(selectedPath); } @Override @@ -237,7 +175,6 @@ public void teleopPeriodic() { parkBeforeDisable(); } - public MustangController getOperatorController() { return OI.getOperatorController(); } 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/constants/FieldConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/FieldConstants.java index f0b8dd01..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 @@ -23,394 +23,380 @@ * 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.kClearance, - 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.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))}; + } + + // 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)); + } } - - // 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 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 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); + } + + /** + * 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 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 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; } + } - /** - * 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 Translation2d allianceOrientedAllianceFlip(Translation2d t) { + if (DriverStation.getAlliance() == Alliance.Red) { + return new Translation2d(fieldLength - t.getX(), fieldWidth - t.getY()); + } else { + return t; } + } - 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 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 cdb37738..4eaceb53 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 @@ -5,8 +5,6 @@ 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; @@ -25,35 +23,22 @@ import frc.team670.robot.subsystems.arm.Arm; import frc.team670.robot.subsystems.arm.ArmState; -public class OI extends OIBase { +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(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); @@ -95,13 +80,12 @@ 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]; + 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, RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond)); @@ -140,6 +124,5 @@ public void configureButtonBindings(MustangSubsystemBase... subsystemBases) { 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 1ce4a166..2c8e11c3 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 @@ -21,7 +21,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.team670.mustanglib.constants.RobotConstantsBase; import frc.team670.mustanglib.subsystems.SparkMaxRotatingSubsystem; import frc.team670.mustanglib.subsystems.drivebase.SwerveDrive; import frc.team670.mustanglib.swervelib.Mk4iSwerveModuleHelper.GearRatio; @@ -32,29 +31,46 @@ import frc.team670.robot.subsystems.arm.ArmSegment; /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared * globally (i.e. public static). Do not put anything functional in this class. * - * It is advised to statically import this class (or one of its inner classes) wherever the + * It is advised to statically import this class (or one of its inner classes) + * wherever the * constants are needed, to reduce verbosity. */ -public final class RobotConstants extends RobotConstantsBase { +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 + * 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). */ @@ -90,38 +106,37 @@ public static final class DriveBase { 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 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 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 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 double kFrontRightModuleSteerOffsetRadians = robotSpecificConstants + .get("kFrontRightModuleSteerOffsetRadians"); 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 static final double kBackLeftModuleSteerOffsetRadians = robotSpecificConstants + .get("kBackLeftModuleSteerOffsetRadians"); 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 double kBackRightModuleSteerOffsetRadians = robotSpecificConstants + .get("kBackRightModuleSteerOffsetRadians"); public final static SerialPort.Port kNAVXPort = SerialPort.Port.kMXP; public static final double kPitchOffset = 2; @@ -144,7 +159,6 @@ public static final class DriveBase { 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); @@ -171,10 +185,9 @@ public static final class DriveBase { kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); } - // vision public static final class Vision { - public static final String[] kVisionCameraIDs = {"Arducam_B", "Arducam_C"}; + public static final String[] kVisionCameraIDs = { "Arducam_B", "Arducam_C" }; public static final Transform3d[] kCameraOffsets = { // Cam B - RIGHT new Transform3d( @@ -185,7 +198,7 @@ public static final class Vision { new Transform3d( new Translation3d(Units.inchesToMeters(0.56), Units.inchesToMeters(5.25), Units.inchesToMeters(19 + 5)), - new Rotation3d(0, 0, Units.degreesToRadians(45)))}; + new Rotation3d(0, 0, Units.degreesToRadians(45))) }; public static final double kLockedOnErrorX = 0.3; public static final double xLockedOnErrorY = 0.3; @@ -198,11 +211,10 @@ 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 kAbsoluteEncoderVerticalOffsetRadians = - RobotConstants.robotSpecificConstants - .get("kShoulderAbsoluteEncoderVerticalOffsetRadians"); - public static final int kGearRatio = - (RobotConstants.robotSpecificConstants.get("kShoulderGearRatio")).intValue(); + public static final double kAbsoluteEncoderVerticalOffsetRadians = RobotConstants.robotSpecificConstants + .get("kShoulderAbsoluteEncoderVerticalOffsetRadians"); + 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; @@ -210,9 +222,8 @@ public static final class Shoulder { public static final double kMassLb = 8.7; public static final double kMassDistribution = 0.356; public static final double kAllowedErrorDegrees = 0.2; - public static final ArmSegment kSegment = - new ArmSegment(kLengthInches, kMassLb, kMassDistribution, - frc.team670.robot.subsystems.arm.Shoulder.SHOULDER_ARBITRARY_FF); + 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; @@ -222,34 +233,30 @@ public static final class Shoulder { 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 = new float[] { (float) Units.degreesToRotations(kSoftLimitMax), + (float) Units.degreesToRotations(kSoftLimitMin) }; 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, kAllowedErrorDegrees, - kSoftLimits, kContinuousCurrent, kPeakCurrent); + public static final SparkMaxRotatingSubsystem.Config kConfig = new SparkMaxRotatingSubsystem.Config( + kLeaderMotorID, 0, kMotorType, kIdleMode, + kGearRatio, kP, kI, kD, kFF, kIz, kMaxOutput, kMinOutput, + kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorDegrees, + kSoftLimits, kContinuousCurrent, kPeakCurrent); } public static final class Elbow { - public static final double kAbsoluteEncoderVerticalOffsetRadians = - RobotConstants.robotSpecificConstants - .get("kElbowAbsoluteEncoderVerticalOffsetRadians"); - public static final double kGearRatio = - RobotConstants.robotSpecificConstants.get("kElbowGearRatio"); + public static final double kAbsoluteEncoderVerticalOffsetRadians = RobotConstants.robotSpecificConstants + .get("kElbowAbsoluteEncoderVerticalOffsetRadians"); + 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; @@ -257,8 +264,7 @@ public static final class Elbow { 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 ArmSegment kSegment = new ArmSegment(kLengthInches, kMassLb, kMassDistribution, 0.5); public static final int kMotorID = 4; public static final IdleMode kIdleMode = IdleMode.kBrake; @@ -270,39 +276,36 @@ public static final class Elbow { 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 = new float[] { (float) Units.degreesToRotations(kSoftLimitMax), + (float) Units.degreesToRotations(kSoftLimitMin) }; 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 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 kAbsoluteEncoderVerticalOffsetRadians = - RobotConstants.robotSpecificConstants - .get("kWristAbsoluteEncoderVerticalOffsetRadians"); - public static final double kGearRatio = - (RobotConstants.robotSpecificConstants.get("kWristGearRatio")).intValue(); + public static final double kAbsoluteEncoderVerticalOffsetRadians = RobotConstants.robotSpecificConstants + .get("kWristAbsoluteEncoderVerticalOffsetRadians"); + 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 ArmSegment kWristSegment = - new ArmSegment(Claw.kLengthInches, Claw.kMassLB, Claw.kMassDistribution, 0); + 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; @@ -314,7 +317,6 @@ public static final class Wrist { 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; @@ -326,11 +328,11 @@ public static final class Wrist { 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, kAllowedErrorDegrees, - kSoftLimits, kContinuousCurrent, kPeakCurrent); + 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); } @@ -349,10 +351,11 @@ public static final class Claw { } } - public static final double kConeMassLb = 1.4; - public static final int kLEDPort = 0; - - + 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 @@ -390,6 +393,4 @@ public static String getMACAddress() { return ""; } - - } 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 e3fcd222..04dda486 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 @@ -31,16 +31,21 @@ public enum Status { private LED led; - public Claw(LED led) { + private static Claw mInstance; + + public static synchronized Claw getInstance() { + return mInstance == null ? new Claw() : 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() { 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..9301e97b 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() { + return mInstance == null ? new LED() : 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 Date: Thu, 20 Apr 2023 22:10:03 -0700 Subject: [PATCH 11/33] integrate iron panther vision std scaling --- .../src/main/java/frc/team670/mustanglib | 2 +- .../robot/constants/RobotConstants.java | 32 ++++++++++++++++--- .../frc/team670/robot/subsystems/Claw.java | 2 +- .../frc/team670/robot/subsystems/LED.java | 1 - .../frc/team670/robot/subsystems/Vision.java | 5 +-- 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index b7cf2bca..f5e170d5 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit b7cf2bca81dcbfb8ca0d1876ac2cfcfd7aae19c8 +Subproject commit f5e170d55bb2a0e708bd02fb2fa9a4074d1a93ea 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 2c8e11c3..6461c992 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,26 +5,31 @@ 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.HashMap; +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.pathplanner.lib.auto.SwerveAutoBuilder; 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 edu.wpi.first.wpilibj2.command.Subsystem; 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.Mk4ModuleConfiguration; import frc.team670.mustanglib.swervelib.ModuleConfiguration; import frc.team670.mustanglib.swervelib.SdsModuleConfigurations; import frc.team670.mustanglib.utils.motorcontroller.MotorConfig; @@ -203,6 +208,25 @@ public static final class Vision { 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 { 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 04dda486..d792594a 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,8 +1,8 @@ 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.RobotConstants; 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 9301e97b..73525909 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,7 +1,6 @@ package frc.team670.robot.subsystems; import frc.team670.mustanglib.subsystems.LEDSubsystem; -import frc.team670.mustanglib.utils.LEDColor; import frc.team670.robot.constants.RobotConstants; public class LED extends LEDSubsystem { diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java index 3bdd16f4..1928766b 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java @@ -16,10 +16,7 @@ public static synchronized Vision getInstance() { } public Vision() { - super(FieldConstants.getFieldLayout(FieldConstants.aprilTags), - new PhotonCamera[] {new PhotonCamera(RobotConstants.Vision.kVisionCameraIDs[0]), - new PhotonCamera(RobotConstants.Vision.kVisionCameraIDs[1])}, - RobotConstants.Vision.kCameraOffsets); + super(RobotConstants.Vision.kConfig); setName("Vision"); } From ac68db0b2316895e44682e115e5ed56e500fecc6 Mon Sep 17 00:00:00 2001 From: DevTeamHRT Date: Wed, 3 May 2023 17:39:23 -0700 Subject: [PATCH 12/33] bug fixes --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- .../src/main/java/frc/team670/robot/subsystems/Claw.java | 6 +++++- .../main/java/frc/team670/robot/subsystems/DriveBase.java | 5 ++++- .../src/main/java/frc/team670/robot/subsystems/LED.java | 4 +++- .../src/main/java/frc/team670/robot/subsystems/Vision.java | 5 ++++- .../main/java/frc/team670/robot/subsystems/arm/Arm.java | 7 +++++-- 6 files changed, 22 insertions(+), 7 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index f5e170d5..16200a44 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit f5e170d55bb2a0e708bd02fb2fa9a4074d1a93ea +Subproject commit 16200a44546d13c4ebdc03ac0ea61192855cc52f 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 d792594a..c2d6f61c 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 @@ -34,7 +34,11 @@ public enum Status { private static Claw mInstance; public static synchronized Claw getInstance() { - return mInstance == null ? new Claw() : mInstance; + if(mInstance==null){ + mInstance=new Claw(); + } + + return mInstance; } public Claw() { 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 84e80c45..18b38280 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 @@ -15,7 +15,10 @@ public class DriveBase extends SwerveDrive { private static DriveBase mInstance; public static synchronized DriveBase getInstance() { - return mInstance == null ? new DriveBase() : mInstance; + if(mInstance==null){ + mInstance=new DriveBase(); + } + return mInstance; } public DriveBase() { 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 73525909..c70a3f1a 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 @@ -9,7 +9,9 @@ public class LED extends LEDSubsystem { private int prevPath = -1; public static synchronized LED getInstance() { - return mInstance == null ? new LED() : mInstance; + if(mInstance==null) + mInstance=new LED(); + return mInstance; } public LED() { diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java index 1928766b..779f7346 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java @@ -12,7 +12,10 @@ public class Vision extends VisionSubsystemBase { private static Vision mInstance; public static synchronized Vision getInstance() { - return mInstance == null ? new Vision() : mInstance; + if(mInstance==null){ + mInstance=new Vision(); + } + return mInstance; } public Vision() { diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 361a8c3a..2c986021 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -83,8 +83,11 @@ public class Arm extends MustangSubsystemBase { private static Arm mInstance; - public static Arm getInstance() { - return mInstance == null ? new Arm() : mInstance; + public static synchronized Arm getInstance() { + if(mInstance==null){ + mInstance=new Arm(); + } + return mInstance; } public Arm() { From 7e058d4367f3d1355f12d99f0fa1d46146f7cb1c Mon Sep 17 00:00:00 2001 From: Ethan Date: Fri, 5 May 2023 12:32:50 -0700 Subject: [PATCH 13/33] formatting lol --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- .../src/main/java/frc/team670/robot/subsystems/Claw.java | 5 +---- .../main/java/frc/team670/robot/subsystems/DriveBase.java | 4 +--- .../src/main/java/frc/team670/robot/subsystems/LED.java | 3 +-- .../src/main/java/frc/team670/robot/subsystems/Vision.java | 6 +----- .../src/main/java/frc/team670/robot/subsystems/arm/Arm.java | 4 +--- 6 files changed, 6 insertions(+), 18 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 16200a44..9ad31712 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 16200a44546d13c4ebdc03ac0ea61192855cc52f +Subproject commit 9ad3171232c353cabd71b113310b532798d51665 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 c2d6f61c..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 @@ -34,10 +34,7 @@ public enum Status { private static Claw mInstance; public static synchronized Claw getInstance() { - if(mInstance==null){ - mInstance=new Claw(); - } - + mInstance = mInstance == null ? new Claw() : mInstance; return mInstance; } 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 18b38280..b92fc0a5 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 @@ -15,9 +15,7 @@ public class DriveBase extends SwerveDrive { private static DriveBase mInstance; public static synchronized DriveBase getInstance() { - if(mInstance==null){ - mInstance=new DriveBase(); - } + mInstance = mInstance == null ? new DriveBase() : mInstance; return mInstance; } 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 c70a3f1a..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 @@ -9,8 +9,7 @@ public class LED extends LEDSubsystem { private int prevPath = -1; public static synchronized LED getInstance() { - if(mInstance==null) - mInstance=new LED(); + mInstance = mInstance == null ? new LED() : mInstance; return mInstance; } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java index 779f7346..26bc5a2d 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Vision.java @@ -1,8 +1,6 @@ package frc.team670.robot.subsystems; -import org.photonvision.PhotonCamera; import frc.team670.mustanglib.subsystems.VisionSubsystemBase; -import frc.team670.robot.constants.FieldConstants; import frc.team670.robot.constants.RobotConstants; /** @@ -12,9 +10,7 @@ public class Vision extends VisionSubsystemBase { private static Vision mInstance; public static synchronized Vision getInstance() { - if(mInstance==null){ - mInstance=new Vision(); - } + mInstance = mInstance == null ? new Vision() : mInstance; return mInstance; } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 2c986021..3ce6f32b 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -84,9 +84,7 @@ public class Arm extends MustangSubsystemBase { private static Arm mInstance; public static synchronized Arm getInstance() { - if(mInstance==null){ - mInstance=new Arm(); - } + mInstance = mInstance == null ? new Arm() : mInstance; return mInstance; } From 1d3fe11be4d88c0f487023cf32fadaaa68963c79 Mon Sep 17 00:00:00 2001 From: DevTeamHRT Date: Fri, 5 May 2023 16:54:54 -0700 Subject: [PATCH 14/33] wrist fixes --- .../src/main/java/frc/team670/mustanglib | 2 +- .../robot/constants/RobotConstants.java | 24 +++++++++---------- .../team670/robot/subsystems/arm/Elbow.java | 3 ++- .../robot/subsystems/arm/Shoulder.java | 2 +- .../team670/robot/subsystems/arm/Wrist.java | 2 +- 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 16200a44..fdd270e1 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 16200a44546d13c4ebdc03ac0ea61192855cc52f +Subproject commit fdd270e1733cad1bb8c892ddbd6d17458fb22d2e 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 6461c992..42e91ab3 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 @@ -87,9 +87,9 @@ public final class RobotConstants { entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(299.807)), entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(137.499)), entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(318.604)), - entry("kShoulderAbsoluteEncoderVerticalOffsetRadians", 0.484233), // 0.47777 - entry("kElbowAbsoluteEncoderVerticalOffsetRadians", 0.004892), - entry("kWristAbsoluteEncoderVerticalOffsetRadians", 0.337308), + entry("kShoulderAbsoluteEncoderVerticalOffset", 0.484233), // 0.47777 + entry("kElbowAbsoluteEncoderVerticalOffset", 0.004892), + entry("kWristAbsoluteEncoderVerticalOffset", 0.177702),//234.126606 entry("kShoulderGearRatio", 96.0), entry("kElbowGearRatio", 70.833333333333), entry("kSwerveModuleConfig", 2.0), entry("kWristGearRatio", 125.0))), entry(kSkipperAddress, @@ -98,9 +98,9 @@ public final class RobotConstants { entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(233.29)), entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(225.77)), entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(112.53)), - entry("kShoulderAbsoluteEncoderVerticalOffsetRadians", 0.895), - entry("kElbowAbsoluteEncoderVerticalOffsetRadians", 0.588), - entry("kWristAbsoluteEncoderVerticalOffsetRadians", 0.918), + 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); @@ -235,8 +235,8 @@ 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 kAbsoluteEncoderVerticalOffsetRadians = RobotConstants.robotSpecificConstants - .get("kShoulderAbsoluteEncoderVerticalOffsetRadians"); + 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; @@ -278,8 +278,8 @@ public static final class Shoulder { } public static final class Elbow { - public static final double kAbsoluteEncoderVerticalOffsetRadians = RobotConstants.robotSpecificConstants - .get("kElbowAbsoluteEncoderVerticalOffsetRadians"); + 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; @@ -322,8 +322,8 @@ public static final class Elbow { public static final class Wrist { // Wrist - public static final double kAbsoluteEncoderVerticalOffsetRadians = RobotConstants.robotSpecificConstants - .get("kWristAbsoluteEncoderVerticalOffsetRadians"); + 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; 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 66c21e53..c3aa6f9d 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 @@ -50,7 +50,7 @@ private void setEncoderPositionFromAbsolute() { if (absEncoderPosition != 0.0) { double relativePosition = ((-1 * (absEncoderPosition - - (RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffsetRadians + - (RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffset - 0.5)) + 1) * RobotConstants.Arm.Elbow.kGearRatio) % RobotConstants.Arm.Elbow.kGearRatio; @@ -154,6 +154,7 @@ public void debugSubsystem() { SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); SmartDashboard.putNumber(setpointRot, mSetpoint); SmartDashboard.putNumber(current, super.getmRotator().getOutputCurrent()); + SmartDashboard.putNumber("Elbow motor power: ",super.mRotator.get() ); } @Override 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 eacdc1a3..a66cde0a 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 @@ -138,7 +138,7 @@ private void setEncoderPositionFromAbsolute() { if (absEncoderPosition != 0.0) { double relativePosition = ((-1 * (absEncoderPosition - - (RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffsetRadians + - (RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffset - 0.5)) + 1) * RobotConstants.Arm.Shoulder.kGearRatio) % RobotConstants.Arm.Shoulder.kGearRatio; 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 f3082dfc..15103a52 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 @@ -50,7 +50,7 @@ private void setEncoderPositionFromAbsolute() { if (absEncoderPosition != 0.0) { double relativePosition = ((1 * (absEncoderPosition - - (RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffsetRadians + - (RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffset - 0.5)) + 1) * RobotConstants.Arm.Wrist.kGearRatio) % RobotConstants.Arm.Wrist.kGearRatio; From ffcb277dbc98840dda3132a1f868efe999e1882c Mon Sep 17 00:00:00 2001 From: Ethan Date: Mon, 8 May 2023 14:24:59 -0700 Subject: [PATCH 15/33] constants --- .../src/main/java/frc/team670/mustanglib | 2 +- .../main/java/frc/team670/robot/Robot.java | 1 - .../robot/constants/RobotConstants.java | 2 +- .../team670/robot/subsystems/arm/Elbow.java | 22 ++++++-------- .../robot/subsystems/arm/Shoulder.java | 29 +++++++++---------- .../team670/robot/subsystems/arm/Wrist.java | 13 ++++----- 6 files changed, 29 insertions(+), 40 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index fdd270e1..9ad31712 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit fdd270e1733cad1bb8c892ddbd6d17458fb22d2e +Subproject commit 9ad3171232c353cabd71b113310b532798d51665 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/constants/RobotConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java index 42e91ab3..3db4f9d4 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 @@ -89,7 +89,7 @@ public final class RobotConstants { entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(318.604)), entry("kShoulderAbsoluteEncoderVerticalOffset", 0.484233), // 0.47777 entry("kElbowAbsoluteEncoderVerticalOffset", 0.004892), - entry("kWristAbsoluteEncoderVerticalOffset", 0.177702),//234.126606 + entry("kWristAbsoluteEncoderVerticalOffset", 0.337308), // 0.177702 entry("kShoulderGearRatio", 96.0), entry("kElbowGearRatio", 70.833333333333), entry("kSwerveModuleConfig", 2.0), entry("kWristGearRatio", 125.0))), entry(kSkipperAddress, 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 c3aa6f9d..14ff08d8 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 @@ -35,9 +35,8 @@ public class Elbow extends SparkMaxRotatingSubsystem { public Elbow() { super(RobotConstants.Arm.Elbow.kConfig); - absEncoder = new DutyCycleEncoder( - RobotConstants.Arm.Elbow.kAbsoluteEncoderID); - super.getmRotator().setInverted(true); + absEncoder = new DutyCycleEncoder(RobotConstants.Arm.Elbow.kAbsoluteEncoderID); + super.getRotator().setInverted(true); } /** @@ -50,10 +49,8 @@ private void setEncoderPositionFromAbsolute() { if (absEncoderPosition != 0.0) { double relativePosition = ((-1 * (absEncoderPosition - - (RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffset - - 0.5)) - + 1) * RobotConstants.Arm.Elbow.kGearRatio) - % RobotConstants.Arm.Elbow.kGearRatio; + - (RobotConstants.Arm.Elbow.kAbsoluteEncoderVerticalOffset - 0.5)) + 2) + * RobotConstants.Arm.Elbow.kGearRatio) % RobotConstants.Arm.Elbow.kGearRatio; if (calculatedRelativePosition == 0.0 || Math.abs(360 * ((previousPositionRot - relativePosition) @@ -125,10 +122,9 @@ public void setSystemTargetAngleInDegrees(double targetAngle) { } private void setOffset(double offset) { - if (Math.abs( - offset) > RobotConstants.Arm.Elbow.kMaxOverrideDegreees) { - this.offset = RobotConstants.Arm.Elbow.kMaxOverrideDegreees - * 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; } @@ -153,8 +149,8 @@ public void debugSubsystem() { SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); SmartDashboard.putNumber(setpointRot, mSetpoint); - SmartDashboard.putNumber(current, super.getmRotator().getOutputCurrent()); - SmartDashboard.putNumber("Elbow motor power: ",super.mRotator.get() ); + SmartDashboard.putNumber(current, super.getRotator().getOutputCurrent()); + SmartDashboard.putNumber("Elbow motor power: ", super.mRotator.get()); } @Override 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 a66cde0a..98611d5e 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 @@ -37,13 +37,11 @@ public class Shoulder extends SparkMaxRotatingSubsystem { public Shoulder() { super(RobotConstants.Arm.Shoulder.kConfig); - super.getmRotator().setInverted(true); - follower = SparkMAXFactory.setPermanentFollower( - RobotConstants.Arm.Shoulder.kFollowerMotorID, mRotator, - true); + super.getRotator().setInverted(true); + follower = SparkMAXFactory + .setPermanentFollower(RobotConstants.Arm.Shoulder.kFollowerMotorID, mRotator, true); follower.setIdleMode(IdleMode.kBrake); - absEncoder = new DutyCycleEncoder( - RobotConstants.Arm.Shoulder.kAbsoluteEncoderID); + absEncoder = new DutyCycleEncoder(RobotConstants.Arm.Shoulder.kAbsoluteEncoderID); } /** @@ -66,11 +64,9 @@ public void setSystemTargetAngleInDegrees(double targetAngle) { } private void setOffset(double offset) { - if (Math.abs( - offset) > RobotConstants.Arm.Shoulder.kMaxOverrideDegrees) { - this.offset = - RobotConstants.Arm.Shoulder.kMaxOverrideDegrees - * 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; } @@ -137,14 +133,15 @@ private void setEncoderPositionFromAbsolute() { double previousPositionRot = super.mEncoder.getPosition(); if (absEncoderPosition != 0.0) { - double relativePosition = ((-1 * (absEncoderPosition - - (RobotConstants.Arm.Shoulder.kAbsoluteEncoderVerticalOffset - - 0.5)) + double relativePosition = ((-1 + * (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) / kConfig.kRotatorGearRatio())) < 5.0) { + if (calculatedRelativePosition == 0.0 + || Math.abs(360 * ((previousPositionRot - relativePosition) + / kConfig.kRotatorGearRatio())) < 5.0) { clearSetpoint(); REVLibError error = mEncoder.setPosition(relativePosition); SmartDashboard.putNumber("Shoulder absEncoder position when reset", 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 15103a52..d7f30028 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 @@ -32,14 +32,13 @@ public class Wrist extends SparkMaxRotatingSubsystem { public Wrist() { super(RobotConstants.Arm.Wrist.kConfig); - absEncoder = new DutyCycleEncoder( - RobotConstants.Arm.Wrist.kAbsoluteEncoderID); - super.getmRotator().setInverted(false); + absEncoder = new DutyCycleEncoder(RobotConstants.Arm.Wrist.kAbsoluteEncoderID); + super.getRotator().setInverted(false); } - + /** * PRIVATE method to set position from absolute. DO NOT USE DIRECTLY. Instead, use * resetPositionFromAbsolute() @@ -50,10 +49,8 @@ private void setEncoderPositionFromAbsolute() { if (absEncoderPosition != 0.0) { double relativePosition = ((1 * (absEncoderPosition - - (RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffset - - 0.5)) - + 1) * RobotConstants.Arm.Wrist.kGearRatio) - % RobotConstants.Arm.Wrist.kGearRatio; + - (RobotConstants.Arm.Wrist.kAbsoluteEncoderVerticalOffset - 0.5)) + 1) + * RobotConstants.Arm.Wrist.kGearRatio) % RobotConstants.Arm.Wrist.kGearRatio; if (calculatedRelativePosition == 0.0 || Math.abs(360 * ((previousPositionRot - relativePosition) From 2d0b0255ee8c0c333b7e7c78b9f7ccb63e510c0b Mon Sep 17 00:00:00 2001 From: Ethan Date: Tue, 9 May 2023 09:50:08 -0700 Subject: [PATCH 16/33] reverted rotating subsystem deviation --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- .../main/java/frc/team670/robot/constants/RobotConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 9ad31712..66813cd3 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 9ad3171232c353cabd71b113310b532798d51665 +Subproject commit 66813cd302e09562f7142667d6e53815019a0c6b 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 3db4f9d4..e4e6d233 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 @@ -284,7 +284,7 @@ public static final class Elbow { 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 kAllowedErrorDegrees = 0.75;; public static final double kLengthInches = 35; public static final double kMassLb = 5.5; public static final double kMassDistribution = 0.686; From f259464588d3a9f66fd8389a22356cbfaa70b8a5 Mon Sep 17 00:00:00 2001 From: Armaan Gomes <91099806+arghunter@users.noreply.github.com> Date: Sat, 17 Jun 2023 12:03:16 -0700 Subject: [PATCH 17/33] Extra debugging lines --- .../main/java/frc/team670/robot/subsystems/arm/Arm.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 3ce6f32b..9eb99547 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -283,6 +283,13 @@ public static ArmState[] getValidPath(ArmState start, ArmState finish) { @Override public void debugSubsystem() { SmartDashboard.putString(targetPositionKey, getTargetState().toString()); + SmartDashboard.putString("Arm Target State", this.targetState.toString()); + SmartDashboard.putBoolean(" Arm hasInitializeState",initializedState); + SmartDashboard.putBoolean(" Arm hasSetShoulderTarget", hasSetShoulderTarget); + SmartDashboard.putBoolean(" Arm hasSetElbowTarget", hasSetElbowTarget); + SmartDashboard.putBoolean(" Arm hasSetWristTarget", hasSetWristTarget); + SmartDashboard.putString("Arm Time Delays", Arrays.toString(currentTimeDelays)); + // SmartDashboard.putNumber("Elbow offset", elbowOffset); // SmartDashboard.putNumber("Shoulder offset", shoulderOffset); // SmartDashboard.putNumber("Wrist offset", wristOffset); From 68105cc1107f0cf45bc050024a51a80b726e2614 Mon Sep 17 00:00:00 2001 From: Kedar Haldar <72819555+Anomalous-0@users.noreply.github.com> Date: Sat, 17 Jun 2023 12:54:42 -0700 Subject: [PATCH 18/33] More debug statements --- .../java/frc/team670/robot/subsystems/arm/Arm.java | 3 +++ .../frc/team670/robot/subsystems/arm/Elbow.java | 7 ++++++- .../frc/team670/robot/subsystems/arm/Shoulder.java | 14 ++++++++++++++ .../frc/team670/robot/subsystems/arm/Wrist.java | 5 ++++- 4 files changed, 27 insertions(+), 2 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 9eb99547..4e5d4077 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -280,6 +280,9 @@ public static ArmState[] getValidPath(ArmState start, ArmState finish) { } + + + @Override public void debugSubsystem() { SmartDashboard.putString(targetPositionKey, getTargetState().toString()); 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 14ff08d8..eb2747f8 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 @@ -144,7 +144,12 @@ public void addOffset(double offset) { @Override public void debugSubsystem() { double relativePosition = super.mEncoder.getPosition(); - + SmartDashboard.putBoolean("Elbow has set absolute position", hasSetAbsolutePosition); + SmartDashboard.putNumber("Elbow crevious reading", previousReading); + SmartDashboard.putNumber("Elbow calculated relative position", calculatedRelativePosition); + SmartDashboard.putBoolean("Elbow relative position is set", relativePositionIsSet); + SmartDashboard.putNumber("Elbow offset", offset); + SmartDashboard.putNumber("Elbow orgTargetAngle", orgTargetAngle); SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); 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 98611d5e..4420e0be 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 @@ -114,13 +114,27 @@ public void resetPositionFromAbsolute() { setEncoderPositionFromAbsolute(); } + /* + * double previousReading = 0.0; + double calculatedRelativePosition = 0.0; + boolean relativePositionIsSet = false; + private double offset = 0; + private double orgTargetAngle = 0; + */ @Override public void debugSubsystem() { double relativePosition = super.mEncoder.getPosition(); + SmartDashboard.putBoolean("Shoulder has set absolute position", hasSetAbsolutePosition); + SmartDashboard.putNumber("Shoulder previous reading", previousReading); + SmartDashboard.putNumber("Shoulder calculated relative position", calculatedRelativePosition); + SmartDashboard.putBoolean("Shoulder relative position is set", relativePositionIsSet); + SmartDashboard.putNumber("Shoulder offset", offset); + SmartDashboard.putNumber("Shoulder orgTargetAngle", orgTargetAngle); SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); SmartDashboard.putNumber(setpointRot, mSetpoint); + SmartDashboard.putNumber("Shoulder motor power: ", super.mRotator.get()); } 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 d7f30028..9197f986 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 @@ -120,7 +120,10 @@ public boolean hasReachedTargetPosition() { @Override public void debugSubsystem() { double relativePosition = super.mEncoder.getPosition(); - + SmartDashboard.putBoolean("Wrist has set absolute position", hasSetAbsolutePosition); + SmartDashboard.putNumber("Wrist previous reading", previousReading); + SmartDashboard.putNumber("Wrist calculated relative position", calculatedRelativePosition); + SmartDashboard.putBoolean("Wrist relative position is set", relativePositionIsSet); SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); From 352e37722031f8d239e96df70932b6e42aa923a1 Mon Sep 17 00:00:00 2001 From: Armaan Gomes <91099806+arghunter@users.noreply.github.com> Date: Sat, 17 Jun 2023 13:25:27 -0700 Subject: [PATCH 19/33] Update RobotConstants.java --- .../main/java/frc/team670/robot/constants/RobotConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e4e6d233..55dee471 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 @@ -89,7 +89,7 @@ public final class RobotConstants { entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(318.604)), entry("kShoulderAbsoluteEncoderVerticalOffset", 0.484233), // 0.47777 entry("kElbowAbsoluteEncoderVerticalOffset", 0.004892), - entry("kWristAbsoluteEncoderVerticalOffset", 0.337308), // 0.177702 + entry("kWristAbsoluteEncoderVerticalOffset", 0.174231), // 0.177702 entry("kShoulderGearRatio", 96.0), entry("kElbowGearRatio", 70.833333333333), entry("kSwerveModuleConfig", 2.0), entry("kWristGearRatio", 125.0))), entry(kSkipperAddress, From 441a1490a998697a27917d5309bc89d82df903f3 Mon Sep 17 00:00:00 2001 From: Armaan Gomes <91099806+arghunter@users.noreply.github.com> Date: Sat, 17 Jun 2023 14:48:04 -0700 Subject: [PATCH 20/33] Revert "More debug statements" This reverts commit 68105cc1107f0cf45bc050024a51a80b726e2614. --- .../java/frc/team670/robot/subsystems/arm/Arm.java | 3 --- .../frc/team670/robot/subsystems/arm/Elbow.java | 7 +------ .../frc/team670/robot/subsystems/arm/Shoulder.java | 14 -------------- .../frc/team670/robot/subsystems/arm/Wrist.java | 5 +---- 4 files changed, 2 insertions(+), 27 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 4e5d4077..9eb99547 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -280,9 +280,6 @@ public static ArmState[] getValidPath(ArmState start, ArmState finish) { } - - - @Override public void debugSubsystem() { SmartDashboard.putString(targetPositionKey, getTargetState().toString()); 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 eb2747f8..14ff08d8 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 @@ -144,12 +144,7 @@ public void addOffset(double offset) { @Override public void debugSubsystem() { double relativePosition = super.mEncoder.getPosition(); - SmartDashboard.putBoolean("Elbow has set absolute position", hasSetAbsolutePosition); - SmartDashboard.putNumber("Elbow crevious reading", previousReading); - SmartDashboard.putNumber("Elbow calculated relative position", calculatedRelativePosition); - SmartDashboard.putBoolean("Elbow relative position is set", relativePositionIsSet); - SmartDashboard.putNumber("Elbow offset", offset); - SmartDashboard.putNumber("Elbow orgTargetAngle", orgTargetAngle); + SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); 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 4420e0be..98611d5e 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 @@ -114,27 +114,13 @@ public void resetPositionFromAbsolute() { setEncoderPositionFromAbsolute(); } - /* - * double previousReading = 0.0; - double calculatedRelativePosition = 0.0; - boolean relativePositionIsSet = false; - private double offset = 0; - private double orgTargetAngle = 0; - */ @Override public void debugSubsystem() { double relativePosition = super.mEncoder.getPosition(); - SmartDashboard.putBoolean("Shoulder has set absolute position", hasSetAbsolutePosition); - SmartDashboard.putNumber("Shoulder previous reading", previousReading); - SmartDashboard.putNumber("Shoulder calculated relative position", calculatedRelativePosition); - SmartDashboard.putBoolean("Shoulder relative position is set", relativePositionIsSet); - SmartDashboard.putNumber("Shoulder offset", offset); - SmartDashboard.putNumber("Shoulder orgTargetAngle", orgTargetAngle); SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); SmartDashboard.putNumber(setpointRot, mSetpoint); - SmartDashboard.putNumber("Shoulder motor power: ", super.mRotator.get()); } 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 9197f986..d7f30028 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 @@ -120,10 +120,7 @@ public boolean hasReachedTargetPosition() { @Override public void debugSubsystem() { double relativePosition = super.mEncoder.getPosition(); - SmartDashboard.putBoolean("Wrist has set absolute position", hasSetAbsolutePosition); - SmartDashboard.putNumber("Wrist previous reading", previousReading); - SmartDashboard.putNumber("Wrist calculated relative position", calculatedRelativePosition); - SmartDashboard.putBoolean("Wrist relative position is set", relativePositionIsSet); + SmartDashboard.putNumber(positionDeg, getCurrentAngleInDegrees()); SmartDashboard.putNumber(positionRot, relativePosition); SmartDashboard.putNumber(absEncoderPos, absEncoder.getAbsolutePosition()); From 6585d4a6322f7bc59f37728305036bc178a947fe Mon Sep 17 00:00:00 2001 From: Armaan Gomes <91099806+arghunter@users.noreply.github.com> Date: Sat, 17 Jun 2023 14:48:10 -0700 Subject: [PATCH 21/33] Revert "Extra debugging lines" This reverts commit f259464588d3a9f66fd8389a22356cbfaa70b8a5. --- .../main/java/frc/team670/robot/subsystems/arm/Arm.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index 9eb99547..3ce6f32b 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -283,13 +283,6 @@ public static ArmState[] getValidPath(ArmState start, ArmState finish) { @Override public void debugSubsystem() { SmartDashboard.putString(targetPositionKey, getTargetState().toString()); - SmartDashboard.putString("Arm Target State", this.targetState.toString()); - SmartDashboard.putBoolean(" Arm hasInitializeState",initializedState); - SmartDashboard.putBoolean(" Arm hasSetShoulderTarget", hasSetShoulderTarget); - SmartDashboard.putBoolean(" Arm hasSetElbowTarget", hasSetElbowTarget); - SmartDashboard.putBoolean(" Arm hasSetWristTarget", hasSetWristTarget); - SmartDashboard.putString("Arm Time Delays", Arrays.toString(currentTimeDelays)); - // SmartDashboard.putNumber("Elbow offset", elbowOffset); // SmartDashboard.putNumber("Shoulder offset", shoulderOffset); // SmartDashboard.putNumber("Wrist offset", wristOffset); From 5b5807b107602a726d374d673316ef954dd7c1cc Mon Sep 17 00:00:00 2001 From: Armaan Gomes <91099806+arghunter@users.noreply.github.com> Date: Sat, 17 Jun 2023 15:22:41 -0700 Subject: [PATCH 22/33] minor constant fixes --- 2023-Robot/.Glass/glass.json | 4 ++-- .../frc/team670/robot/constants/RobotConstants.java | 12 ++++++++---- 2 files changed, 10 insertions(+), 6 deletions(-) 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/src/main/java/frc/team670/robot/constants/RobotConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java index 55dee471..8556ee39 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 @@ -245,7 +245,9 @@ public static final class Shoulder { 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 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); @@ -272,7 +274,7 @@ public static final class Shoulder { public static final SparkMaxRotatingSubsystem.Config kConfig = new SparkMaxRotatingSubsystem.Config( kLeaderMotorID, 0, kMotorType, kIdleMode, kGearRatio, kP, kI, kD, kFF, kIz, kMaxOutput, kMinOutput, - kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorDegrees, + kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorRotations, kSoftLimits, kContinuousCurrent, kPeakCurrent); } @@ -284,7 +286,8 @@ public static final class Elbow { 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 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; @@ -328,6 +331,7 @@ public static final class Wrist { .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); @@ -355,7 +359,7 @@ public static final class Wrist { 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, + kMaxRotatorRPM, kMinRotatorRPM, kMaxAcceleration, kAllowedErrorRotations, kSoftLimits, kContinuousCurrent, kPeakCurrent); } From 8134925c6bc8a2b064a6bb94adea9c079d662731 Mon Sep 17 00:00:00 2001 From: arghunter Date: Mon, 19 Jun 2023 17:22:34 -0700 Subject: [PATCH 23/33] disabled manual controls --- 2023-Robot/src/main/java/frc/team670/mustanglib | 2 +- .../src/main/java/frc/team670/robot/subsystems/arm/Elbow.java | 4 ++-- .../main/java/frc/team670/robot/subsystems/arm/Shoulder.java | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index 66813cd3..bbd88266 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit 66813cd302e09562f7142667d6e53815019a0c6b +Subproject commit bbd8826628a92d7bbba55e12f69bef337901376f 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 14ff08d8..73003d3e 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 @@ -118,7 +118,7 @@ public void resetPositionFromAbsolute() { @Override public void setSystemTargetAngleInDegrees(double targetAngle) { orgTargetAngle = targetAngle; - super.setSystemTargetAngleInDegrees(orgTargetAngle + offset); + super.setSystemTargetAngleInDegrees(targetAngle); } private void setOffset(double offset) { @@ -128,7 +128,7 @@ private void setOffset(double offset) { } else { this.offset = offset; } - setSystemTargetAngleInDegrees(orgTargetAngle); + // setSystemTargetAngleInDegrees(orgTargetAngle); } 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 98611d5e..4b130d2d 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 @@ -60,7 +60,7 @@ public boolean getTimeout() { @Override public void setSystemTargetAngleInDegrees(double targetAngle) { orgTargetAngle = targetAngle; - super.setSystemTargetAngleInDegrees(orgTargetAngle + offset); + super.setSystemTargetAngleInDegrees(targetAngle); } private void setOffset(double offset) { @@ -70,7 +70,7 @@ private void setOffset(double offset) { } else { this.offset = offset; } - setSystemTargetAngleInDegrees(orgTargetAngle); + // setSystemTargetAngleInDegrees(orgTargetAngle); } From e15a5ea4b66a179a74d48d756cbd34356383bc8e Mon Sep 17 00:00:00 2001 From: arghunter Date: Mon, 19 Jun 2023 17:53:21 -0700 Subject: [PATCH 24/33] minor encapsulation --- .../java/frc/team670/robot/subsystems/arm/Shoulder.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 4b130d2d..e28ce480 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 @@ -21,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; From 823cd89b9a62227b939766398a78c557af404cc5 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Thu, 22 Jun 2023 18:27:56 -0700 Subject: [PATCH 25/33] set softlimits to null --- .../team670/robot/constants/RobotConstants.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) 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 8556ee39..ff89e80f 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 @@ -82,12 +82,14 @@ public final class RobotConstants { 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(); - private static Map robotSpecificConstants = Map.ofEntries(entry(kSunTzuAddress, - Map.ofEntries(entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(90.895)), + + private 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("kShoulderAbsoluteEncoderVerticalOffset", 0.484233), // 0.47777 entry("kElbowAbsoluteEncoderVerticalOffset", 0.004892), entry("kWristAbsoluteEncoderVerticalOffset", 0.174231), // 0.177702 entry("kShoulderGearRatio", 96.0), entry("kElbowGearRatio", 70.833333333333), @@ -263,8 +265,8 @@ public static final class Shoulder { 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 = 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; @@ -307,8 +309,8 @@ public static final class Elbow { 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 = 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; From 5611558efc743131cb79e9741d98511f54b8aff2 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Thu, 22 Jun 2023 18:45:08 -0700 Subject: [PATCH 26/33] changed allowed error from degrees to rotations --- .../java/frc/team670/robot/constants/RobotConstants.java | 2 +- .../java/frc/team670/robot/subsystems/arm/Elbow.java | 9 +++++---- .../java/frc/team670/robot/subsystems/arm/Shoulder.java | 2 +- .../java/frc/team670/robot/subsystems/arm/Wrist.java | 2 +- 4 files changed, 8 insertions(+), 7 deletions(-) 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 ff89e80f..7619a0de 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 @@ -333,7 +333,7 @@ public static final class Wrist { .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 double kAllowedErrorRotations = kGearRatio* kAllowedErrorDegrees/360; public static final ArmSegment kWristSegment = new ArmSegment(Claw.kLengthInches, Claw.kMassLB, Claw.kMassDistribution, 0); 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 73003d3e..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 @@ -6,6 +6,7 @@ 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.robot.constants.RobotConstants; /** @@ -110,10 +111,10 @@ public void resetPositionFromAbsolute() { } // @Override - // public boolean hasReachedTargetPosition() { - // return (MathUtils.doublesEqual(rotator_encoder.getPosition(), setpoint, - // RobotConstants.Arm.Elbow.kAllowedErrorDegrees)); - // } + public boolean hasReachedTargetPosition() { + return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, + RobotConstants.Arm.Elbow.kAllowedErrorRotations)); + } @Override public void setSystemTargetAngleInDegrees(double targetAngle) { 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 e28ce480..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 @@ -158,7 +158,7 @@ private void setEncoderPositionFromAbsolute() { @Override public boolean hasReachedTargetPosition() { return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, - RobotConstants.Arm.Shoulder.kAllowedErrorDegrees)); + RobotConstants.Arm.Shoulder.kAllowedErrorRotations)); } @Override 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 d7f30028..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 @@ -114,7 +114,7 @@ public void resetPositionFromAbsolute() { @Override public boolean hasReachedTargetPosition() { return (MathUtils.doublesEqual(mEncoder.getPosition(), mSetpoint, - RobotConstants.Arm.Wrist.kAllowedErrorDegrees)); + RobotConstants.Arm.Wrist.kAllowedErrorRotations)); } @Override From b70bec6e322d133c178fdca63e697057a9478986 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Sat, 24 Jun 2023 15:24:38 -0700 Subject: [PATCH 27/33] rezero on teleop init --- 2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) 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 30e2d005..2344c8d9 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -16,6 +16,7 @@ import frc.team670.mustanglib.commands.MustangCommand; import frc.team670.mustanglib.subsystems.MustangSubsystemBase; import frc.team670.mustanglib.utils.MustangController; +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; @@ -133,6 +134,7 @@ public void autonomousInit() { @Override public void teleopInit() { mArm.clearSetpoint(); + new ResetArmFromAbsolute(mArm); } @Override From b2d00dce76f2f965949a98feb99b884ed6e7f369 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Sat, 24 Jun 2023 15:54:42 -0700 Subject: [PATCH 28/33] made it call mustangScheduler instead of command scheduler --- .../java/frc/team670/robot/constants/OI.java | 4 +--- .../frc/team670/robot/subsystems/DriveBase.java | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 4 deletions(-) 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 4eaceb53..3c9c6fe1 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 @@ -86,9 +86,7 @@ public static void configureButtonBindings() { Claw claw = Claw.getInstance(); LED led = LED.getInstance(); - driveBase.initDefaultCommand(new XboxSwerveDrive(driveBase, driverController, - RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, - RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond)); + driveBase.initDefaultCommand(); zeroGyroDriver.onTrue(new SetSwerveForwardDirection(driveBase)); zeroArm.onTrue(new ResetArmFromAbsolute(arm)); 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 b92fc0a5..e3e86aa5 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 @@ -7,13 +7,18 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; 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.subsystems.drivebase.SwerveDrive; import frc.team670.mustanglib.swervelib.SwerveModule; +import frc.team670.mustanglib.utils.MustangController; import frc.team670.robot.constants.RobotConstants; public class DriveBase extends SwerveDrive { private static DriveBase mInstance; - + private MustangCommand defaultCommand; + private MustangController mController; public static synchronized DriveBase getInstance() { mInstance = mInstance == null ? new DriveBase() : mInstance; return mInstance; @@ -23,6 +28,15 @@ public DriveBase() { super(RobotConstants.DriveBase.kConfig); } + + public void initDefaultCommand() { // 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, RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond); + MustangScheduler.getInstance().setDefaultCommand(this, defaultCommand); + } + public void mustangPeriodic() { super.mustangPeriodic(); SmartDashboard.putNumber("pitch", getPitch()); From a884c4e87e1e98e102d82ffe09871fad34318396 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Thu, 6 Jul 2023 23:55:37 -0700 Subject: [PATCH 29/33] fixed controller error --- 2023-Robot/src/main/java/frc/team670/robot/constants/OI.java | 2 +- .../src/main/java/frc/team670/robot/subsystems/DriveBase.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 3c9c6fe1..f79f02d4 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 @@ -86,7 +86,7 @@ public static void configureButtonBindings() { Claw claw = Claw.getInstance(); LED led = LED.getInstance(); - driveBase.initDefaultCommand(); + driveBase.initDefaultCommand(driverController); zeroGyroDriver.onTrue(new SetSwerveForwardDirection(driveBase)); zeroArm.onTrue(new ResetArmFromAbsolute(arm)); 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 e3e86aa5..48ee0778 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 @@ -29,7 +29,7 @@ public DriveBase() { } - public void initDefaultCommand() { // TODO: switch to super class's init default command + public void initDefaultCommand(MustangController mController) { // 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); From 9fadeff0e449002e07b10b516c09fe29b4305028 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Fri, 7 Jul 2023 10:03:55 -0700 Subject: [PATCH 30/33] added drivebase as param for init default command --- 2023-Robot/src/main/java/frc/team670/robot/constants/OI.java | 2 +- .../src/main/java/frc/team670/robot/subsystems/DriveBase.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 f79f02d4..77f5d966 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 @@ -86,7 +86,7 @@ public static void configureButtonBindings() { Claw claw = Claw.getInstance(); LED led = LED.getInstance(); - driveBase.initDefaultCommand(driverController); + driveBase.initDefaultCommand(driveBase, driverController); zeroGyroDriver.onTrue(new SetSwerveForwardDirection(driveBase)); zeroArm.onTrue(new ResetArmFromAbsolute(arm)); 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 48ee0778..4b1416e6 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 @@ -29,11 +29,11 @@ public DriveBase() { } - public void initDefaultCommand(MustangController mController) { // 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, RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond); + defaultCommand = new XboxSwerveDrive(driveBase, mController, RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond); MustangScheduler.getInstance().setDefaultCommand(this, defaultCommand); } From 0ddaf02909769de240433f40522b6b524efcfe19 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Sat, 8 Jul 2023 12:52:13 -0700 Subject: [PATCH 31/33] fixed controller issue --- 2023-Robot/src/main/java/frc/team670/robot/constants/OI.java | 3 ++- .../src/main/java/frc/team670/robot/subsystems/DriveBase.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) 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 77f5d966..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,6 +3,7 @@ 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.utils.MustangController; @@ -86,7 +87,7 @@ public static void configureButtonBindings() { Claw claw = Claw.getInstance(); LED led = LED.getInstance(); - driveBase.initDefaultCommand(driveBase, driverController); + driveBase.initDefaultCommand(new XboxSwerveDrive(driveBase, driverController)); zeroGyroDriver.onTrue(new SetSwerveForwardDirection(driveBase)); zeroArm.onTrue(new ResetArmFromAbsolute(arm)); 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 4b1416e6..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 @@ -33,7 +33,7 @@ public void initDefaultCommand(DriveBase driveBase, MustangController controller // defaultCommand = new XboxSwerveDrive(this, mController, // MAX_VELOCITY_METERS_PER_SECOND, // MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND); - defaultCommand = new XboxSwerveDrive(driveBase, mController, RobotConstants.DriveBase.kMaxVelocityMetersPerSecond, RobotConstants.DriveBase.kMaxAngularVelocityRadiansPerSecond); + defaultCommand = new XboxSwerveDrive(driveBase, mController); MustangScheduler.getInstance().setDefaultCommand(this, defaultCommand); } From 1c62f24a836eb388503c3d5f4724b173092d3945 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Thu, 13 Jul 2023 21:41:04 -0700 Subject: [PATCH 32/33] updated 670_bench wheel offsets --- .../main/java/frc/team670/robot/RobotContainer.java | 10 ++++++++-- .../frc/team670/robot/constants/RobotConstants.java | 13 ++++++------- 2 files changed, 14 insertions(+), 9 deletions(-) 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 2344c8d9..a704f9b6 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.team670.robot.commands.pathplanner.ScoreEngage; import frc.team670.robot.commands.pathplanner.ScoreMid; 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.LED; @@ -54,10 +55,15 @@ public class RobotContainer extends RobotContainerBase { public RobotContainer() { super(); - addSubsystem(mDriveBase, mVision, mArm, mArm.getShoulder(), mArm.getElbow(), mArm.getWrist(), - mClaw, mLed); + 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); } 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 7619a0de..07e9eb24 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 @@ -83,7 +83,7 @@ public final class RobotConstants { public static final String kSkipperAddress = "00:80:2F:33:D0:46"; public static final String kRobotAddress = getMACAddress(); - private static Map robotSpecificConstants = Map.ofEntries( + public static Map robotSpecificConstants = Map.ofEntries( entry(kSunTzuAddress, Map.ofEntries( entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(90.895)), entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(299.807)), @@ -94,12 +94,11 @@ public final class RobotConstants { 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(82.694)), - entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(233.29)), - entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(225.77)), - entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(112.53)), + entry(kSkipperAddress,Map.ofEntries( + entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(84.023437)), + entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(232.734379)), + entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(133.681651)), + entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(202.939455)), entry("kShoulderAbsoluteEncoderVerticalOffset", 0.895), entry("kElbowAbsoluteEncoderVerticalOffset", 0.588), entry("kWristAbsoluteEncoderVerticalOffset", 0.918), From c0e021159ec17378110fd2d70ad2578b9daf1e60 Mon Sep 17 00:00:00 2001 From: Aditi Kattuparambil <112968168+KattuparambilAditi@users.noreply.github.com> Date: Sat, 15 Jul 2023 13:01:43 -0700 Subject: [PATCH 33/33] updated zeros --- .../team670/robot/constants/RobotConstants.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) 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 07e9eb24..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 @@ -97,8 +97,8 @@ public final class RobotConstants { entry(kSkipperAddress,Map.ofEntries( entry("kBackRightModuleSteerOffsetRadians", -Math.toRadians(84.023437)), entry("kBackLeftModuleSteerOffsetRadians", -Math.toRadians(232.734379)), - entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(133.681651)), - entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(202.939455)), + entry("kFrontRightModuleSteerOffsetRadians", -Math.toRadians(22.939455)), + entry("kFrontLeftModuleSteerOffsetRadians", -Math.toRadians(134.281651)), entry("kShoulderAbsoluteEncoderVerticalOffset", 0.895), entry("kElbowAbsoluteEncoderVerticalOffset", 0.588), entry("kWristAbsoluteEncoderVerticalOffset", 0.918), @@ -132,17 +132,19 @@ public static final class DriveBase { 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 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 final static SerialPort.Port kNAVXPort = SerialPort.Port.kMXP; public static final double kPitchOffset = 2;