rev3 elbow not working DONT MERGE

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": [

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"
 

2023-Robot/src/main/java/frc/team670/robot/Robot.java

@@ -19,6 +19,5 @@
 public class Robot extends RobotBase {
 	public Robot() {
 		super(new RobotContainer());
-
 	}
 }

2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java

@@ -7,38 +7,25 @@
 
 package frc.team670.robot;
 
-import java.lang.reflect.Field;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.VideoMode.PixelFormat;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Notifier;
-import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.team670.mustanglib.RobotContainerBase;
 import frc.team670.mustanglib.commands.MustangCommand;
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase;
-import frc.team670.mustanglib.utils.LEDColor;
-import frc.team670.mustanglib.utils.Logger;
 import frc.team670.mustanglib.utils.MustangController;
-import frc.team670.robot.commands.drivebase.NonPidAutoLevel;
-import frc.team670.robot.commands.drivebase.SwerveDriveParkCommand;
-import frc.team670.robot.commands.pathplanner.AutonCalibration;
-import frc.team670.robot.commands.pathplanner.CenterEngage;
+import frc.team670.robot.commands.arm.ResetArmFromAbsolute;
 import frc.team670.robot.commands.pathplanner.CenterEngageSequential;
 import frc.team670.robot.commands.pathplanner.CenterIntake;
 import frc.team670.robot.commands.pathplanner.ConeCube;
 import frc.team670.robot.commands.pathplanner.ConeCubeCube;
-import frc.team670.robot.commands.pathplanner.ConeCubeEngage;
 import frc.team670.robot.commands.pathplanner.CubeEngage;
 import frc.team670.robot.commands.pathplanner.ScoreEngage;
 import frc.team670.robot.commands.pathplanner.ScoreMid;
-import frc.team670.robot.constants.FieldConstants;
 import frc.team670.robot.constants.OI;
-import frc.team670.robot.constants.RobotMap;
+import frc.team670.robot.constants.RobotConstants;
 import frc.team670.robot.subsystems.Claw;
 import frc.team670.robot.subsystems.DriveBase;
 import frc.team670.robot.subsystems.LED;
@@ -52,55 +39,55 @@
  */
 
 public class RobotContainer extends RobotContainerBase {
-
-    private final PowerDistribution pd = new PowerDistribution(1, ModuleType.kCTRE);
-
-    private final Vision vision = new Vision(pd);
-    private final DriveBase driveBase = new DriveBase(getDriverController());
-    private final Arm arm = new Arm();
-    private final LED led = new LED(RobotMap.LED_PORT, 0, 61);
-    private final Claw claw = new Claw(led);
+    private final Vision mVision = Vision.getInstance();
+    private final DriveBase mDriveBase = DriveBase.getInstance();
+    private final LED mLed = LED.getInstance();
+    private final Arm mArm = Arm.getInstance();
+    private final Claw mClaw = Claw.getInstance();
 
     private MustangCommand cableScore, cableEngage, stationScore, stationEngage, centerEngage,
             centerIntake, scoreMid;
 
-    private static OI oi = new OI();
     private Notifier updateArbitraryFeedForward;
 
-    private final String matchStarted = "match-started";
-    private final String autonChooser = "auton-chooser";
+    private final String kMatchStartedString = "match-started";
+    private final String kAutonChooserString = "auton-chooser";
 
     public RobotContainer() {
         super();
-        addSubsystem(driveBase, vision, arm, arm.getShoulder(), arm.getElbow(), arm.getWrist(),
-                claw, led);
-        oi.configureButtonBindings(driveBase, vision, arm, claw, led);
+        addSubsystem(mDriveBase);
+        OI.configureButtonBindings();
+
+        // SmartDashboard.putNumber("fr", RobotConstants.robotSpecificConstants
+        // .get("kFrontLeftModuleSteerOffsetRadians"));
+        // SmartDashboard.putNumber("fl", RobotConstants.robotSpecificConstants.get(
+        // "kFrontLeftModuleSteerOffsetRadians"));
+
 
         for (MustangSubsystemBase subsystem : getSubsystems()) {
             subsystem.setDebugSubsystem(true);
         }
 
-        cableScore = new ConeCube(driveBase, claw, arm, "CableScoreShort");
-        stationScore = new ConeCubeCube(driveBase, claw, arm, "Station3Piece");
-        cableEngage = new CubeEngage(driveBase, claw, arm, "CableEngage");
-        stationEngage = new ScoreEngage(driveBase, claw, arm, "StationScoreEngage3");
-        centerEngage = new CenterEngageSequential(driveBase, claw, arm);
-        centerIntake = new CenterIntake(driveBase, claw, arm, "CenterIntake");
-        scoreMid = new ScoreMid(driveBase, claw, arm);
+        cableScore = new ConeCube(mDriveBase, mClaw, mArm, "CableScoreShort");
+        stationScore = new ConeCubeCube(mDriveBase, mClaw, mArm, "Station3Piece");
+        cableEngage = new CubeEngage(mDriveBase, mClaw, mArm, "CableEngage");
+        stationEngage = new ScoreEngage(mDriveBase, mClaw, mArm, "StationScoreEngage3");
+        centerEngage = new CenterEngageSequential(mDriveBase, mClaw, mArm);
+        centerIntake = new CenterIntake(mDriveBase, mClaw, mArm, "CenterIntake");
+        scoreMid = new ScoreMid(mDriveBase, mClaw, mArm);
 
     }
 
     @Override
     public void robotInit() {
         CameraServer.startAutomaticCapture().setVideoMode(PixelFormat.kYUYV, 160, 120, 30);
 
-        driveBase.initVision(vision);
-        SmartDashboard.putNumber(autonChooser, 0);
-        updateArbitraryFeedForward = new Notifier(new Runnable() {
-            public void run() {
-                arm.updateArbitraryFeedForward();
-            }
-        });
+        mDriveBase.initVision(mVision);
+        SmartDashboard.putNumber(kAutonChooserString, 0);
+        updateArbitraryFeedForward = new Notifier(
+                () -> {
+                    mArm.updateArbitraryFeedForward();
+                });
 
         updateArbitraryFeedForward.startPeriodic(0.01);
     }
@@ -112,74 +99,48 @@ public void run() {
      */
     @Override
     public MustangCommand getAutonomousCommand() {
-        // return new NonPidAutoLevel(driveBase, true);
-        // return new ConeCube(driveBase, claw, arm, "StationScoreShort");
+        SmartDashboard.putBoolean(kMatchStartedString, true);
 
-        SmartDashboard.putBoolean(matchStarted, true);
-
-        // SmartDashboard.putNumber(autonChooser, 0);
-        int selectedPath = (int) SmartDashboard.getNumber(autonChooser, 0);
+        int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0);
         MustangCommand autonCommand;
         switch (selectedPath) {
             case 0:
                 autonCommand = cableScore;
-                led.solidhsv(LEDColor.LIGHT_BLUE);
                 break;
             case 1:
                 autonCommand = stationScore;
-                led.solidhsv(LEDColor.SEXY_YELLOW);
                 break;
             case 2:
                 autonCommand = cableEngage;
-                led.solidhsv(LEDColor.SEXY_PURPLE);
                 break;
             case 3:
                 autonCommand = stationEngage;
-                led.solidhsv(LEDColor.GREEN);
                 break;
             case 4:
                 autonCommand = centerEngage;
-                led.animatedRainbow(false, 10, 10);
                 break;
             case 5:
                 autonCommand = centerIntake;
-                led.solidhsv(LEDColor.PINK);
                 break;
             case 6:
                 autonCommand = scoreMid;
-                led.animatedMustangRainbow(10, 10);
                 break;
             default:
                 autonCommand = centerEngage;
-                led.animatedRainbow(false, 10, 10);
-
         }
+        mLed.updateAutonPathColor(selectedPath);
         return autonCommand;
-
-        // LEAVE COMMENTED
-        // greturn new ConeCube(driveBase, claw, arm, "CableScore");
-        //return new AutonCalibration(driveBase, "Straight180"); // TODO: use curve
-        // path after
-        // straight
-        // path
-        // return new ConeCubeCube(driveBase, claw, arm, "Station3Piece");
-        // return new ConeCubeEngage(driveBase, claw, arm, "StationScoreEngage2");
-        // return new NonPidAutoLevel(driveBase, true);
-        //return new CenterEngage(driveBase, claw, arm, "CenterEngage");
-
-        // return new ConeCube(driveBase, claw, arm, "CableScore");
-        // return new ConeCube(driveBase, claw, arm, "RightConeCube");
-        // return new NonPidAutoLevel(driveBase, true);
     }
 
     @Override
     public void autonomousInit() {
-        arm.setStateToStarting();
+        mArm.setStateToStarting();
     }
 
     @Override
     public void teleopInit() {
-        arm.clearSetpoint();
+        mArm.clearSetpoint();
+        new ResetArmFromAbsolute(mArm);
     }
 
     @Override
@@ -188,45 +149,17 @@ public void testInit() {
 
     @Override
     public void disabled() {
-        SmartDashboard.putBoolean(matchStarted, false);
+        SmartDashboard.putBoolean(kMatchStartedString, false);
     }
 
     @Override
     public void disabledPeriodic() {
-        // int selectedPath = (int)
-        // (SmartDashboard.getEntry(autonChooser).getInteger(-1));
+        mArm.getShoulder().sendAngleToDashboard();
+        mArm.getElbow().sendAngleToDashboard();
+        mArm.getWrist().sendAngleToDashboard();
 
-        arm.getShoulder().sendAngleToDashboard();
-        arm.getElbow().sendAngleToDashboard();
-        arm.getWrist().sendAngleToDashboard();
-
-        int selectedPath = (int) SmartDashboard.getNumber(autonChooser, 0);
-        switch (selectedPath) {
-            case 0:
-                led.blinkhsv(LEDColor.LIGHT_BLUE);
-                break;
-            case 1:
-                led.blinkhsv(LEDColor.SEXY_YELLOW);
-                break;
-            case 2:
-                led.blinkhsv(LEDColor.SEXY_PURPLE);
-                break;
-            case 3:
-                led.blinkhsv(LEDColor.GREEN);
-                break;
-            case 4:
-                led.animatedRainbow(false, 10, 10);
-                break;
-            case 5:
-                led.blinkhsv(LEDColor.PINK);
-                break;
-            case 6:
-                led.animatedMustangRainbow(10, 10);
-                break;
-            default:
-                led.animatedRainbow(false, 10, 10);
-
-        }
+        int selectedPath = (int) SmartDashboard.getNumber(kAutonChooserString, 0);
+        mLed.updateAutonPathColor(selectedPath);
     }
 
     @Override
@@ -250,7 +183,6 @@ public void teleopPeriodic() {
         parkBeforeDisable();
     }
 
-
     public MustangController getOperatorController() {
         return OI.getOperatorController();
     }
@@ -266,7 +198,7 @@ public MustangController getBackupController() {
     private void parkBeforeDisable() {
         double cTime = DriverStation.getMatchTime();
         if (cTime <= 0.1 && cTime != -1) {
-            driveBase.park();
+            mDriveBase.park();
         }
     }
 

2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java

@@ -2,22 +2,18 @@
 
 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;
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase;
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState;
 import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand;
 import frc.team670.robot.commands.vision.IsLockedOn;
-import frc.team670.robot.constants.FieldConstants;
 import frc.team670.robot.constants.RobotConstants;
 import frc.team670.robot.subsystems.DriveBase;
 
@@ -58,7 +54,8 @@ public Map<MustangSubsystemBase, HealthState> getHealthRequirements() {
     @Override
     public void initialize() {
         this.startPose = driveBase.getPose();
-        PathPlannerTrajectory traj = PathPlanner.generatePath(RobotConstants.kAutoPathConstraints,
+        PathPlannerTrajectory traj = PathPlanner.generatePath(
+                RobotConstants.DriveBase.kAutoPathConstraints,
                 calcStartPoint(endPose), calcEndPoint(startPose));
         driveBase.getPoseEstimator().addTrajectory(traj);
 

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)));

2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/PathFindMoveToPose.java

@@ -11,13 +11,13 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.team670.mustanglib.commands.MustangCommand;
+import frc.team670.mustanglib.pathfinder.ObstacleAvoidanceAStarMap;
+import frc.team670.mustanglib.pathfinder.PoseNode;
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase;
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState;
 import frc.team670.mustanglib.swervelib.pathplanner.MustangPPSwerveControllerCommand;
 import frc.team670.robot.constants.FieldConstants;
 import frc.team670.robot.constants.RobotConstants;
-import frc.team670.robot.pathfinder.ObstacleAvoidanceAStarMap;
-import frc.team670.robot.pathfinder.PoseNode;
 import frc.team670.robot.subsystems.DriveBase;
 
 public class PathFindMoveToPose extends CommandBase implements MustangCommand {
@@ -47,8 +47,9 @@ public void initialize() {
 		// points in the path.
 		PathPoint[] fullPathPoints = getPathPointsFromNodes(fullPath);
 
-		PathPlannerTrajectory trajectory = PathPlanner
-				.generatePath(RobotConstants.kAutoPathConstraints, Arrays.asList(fullPathPoints));
+		PathPlannerTrajectory trajectory = PathPlanner.generatePath(
+				RobotConstants.DriveBase.kAutoPathConstraints,
+				Arrays.asList(fullPathPoints));
 		driveBase.getPoseEstimator().addTrajectory(trajectory);
 		pathDrivingCommand = driveBase.getFollowTrajectoryCommand(trajectory);
 		pathDrivingCommand.schedule();