Vision testing

.pathplanner/settings.json

@@ -0,0 +1,14 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [
+    "New Folder"
+  ],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Gains Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Gains Path.path

@@ -0,0 +1,81 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.053233435210415,
+        "y": 7.1495765710980645
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.890081170868021,
+        "y": 7.389779736530214
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.066039257723953,
+        "y": 6.344189487002038
+      },
+      "prevControl": {
+        "x": 5.066039257723953,
+        "y": 7.344189487002038
+      },
+      "nextControl": {
+        "x": 7.066039257723953,
+        "y": 5.344189487002038
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.461668000788668,
+        "y": 3.1650299445177192
+      },
+      "prevControl": {
+        "x": 6.857296743853384,
+        "y": 4.450823359478042
+      },
+      "nextControl": {
+        "x": 6.237127153147944,
+        "y": 2.4352721896853664
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.15401830115192,
+        "y": 0.9325534657953997
+      },
+      "prevControl": {
+        "x": 5.61389212279285,
+        "y": 0.9466830637619968
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/java/frc/robot/RobocketsController.java

@@ -80,39 +80,39 @@ public void teleopPeriodic() {
             }
         }
         // Intake
-        if (map.intake != null) {
-            map.intake.rotate(deadzone(getRightX(), 0.1));
-
-            if (getLeftBumperPressed()) {
-                map.intake.intake();
-            }
-            else if (getRightBumperPressed()) {
-                map.intake.outtake();
-            }
-            }
+        // if (map.intake != null) {
+        //     map.intake.rotate(deadzone(getRightX(), 0.1));
+
+        //     if (getLeftBumperPressed()) {
+        //         map.intake.intake();
+        //     }
+        //     else if (getRightBumperPressed()) {
+        //         map.intake.outtake();
+        //     }
+        //     }
         // Shooter
-        if (map.shooter != null) {
-            if (getAButtonPressed()) {
-                //CommandScheduler.getInstance().schedule(new Shoot(SmartDashboard.getNumber("Shooter Speed", 0.5)));
-                map.shooter.setSpeed(SmartDashboard.getNumber("Shooter In Speed", 0.5));
-            }
-            if (getBButtonPressed()) {
-                //CommandScheduler.getInstance().schedule(new Shoot(-SmartDashboard.getNumber("Shooter Speed", 0.5)));
-                map.shooter.setSpeed(-SmartDashboard.getNumber("Shooter Out Speed", 0.5));
-            }
-            if (getYButtonPressed()) {
-                map.shooter.setIntakeSpeed(SmartDashboard.getNumber("Shooter Intake Speed", 0.5));
-            }
-            if (getXButtonPressed()) {
-                map.shooter.setIntakeSpeed(-SmartDashboard.getNumber("Shooter Outtake Speed", 0.5));
-            }
-            if (getAButtonReleased() || getBButtonReleased()) {
-                map.shooter.setSpeed(0);
-            }
-            if (getXButtonReleased() || getYButtonReleased()) {
-                map.shooter.setIntakeSpeed(0);
-            }
-        }
+        // if (map.shooter != null) {
+        //     if (getAButtonPressed()) {
+        //         //CommandScheduler.getInstance().schedule(new Shoot(SmartDashboard.getNumber("Shooter Speed", 0.5)));
+        //         map.shooter.setSpeed(SmartDashboard.getNumber("Shooter In Speed", 0.5));
+        //     }
+        //     if (getBButtonPressed()) {
+        //         //CommandScheduler.getInstance().schedule(new Shoot(-SmartDashboard.getNumber("Shooter Speed", 0.5)));
+        //         map.shooter.setSpeed(-SmartDashboard.getNumber("Shooter Out Speed", 0.5));
+        //     }
+        //     if (getYButtonPressed()) {
+        //         map.shooter.setIntakeSpeed(SmartDashboard.getNumber("Shooter Intake Speed", 0.5));
+        //     }
+        //     if (getXButtonPressed()) {
+        //         map.shooter.setIntakeSpeed(-SmartDashboard.getNumber("Shooter Outtake Speed", 0.5));
+        //     }
+        //     if (getAButtonReleased() || getBButtonReleased()) {
+        //         map.shooter.setSpeed(0);
+        //     }
+        //     if (getXButtonReleased() || getYButtonReleased()) {
+        //         map.shooter.setIntakeSpeed(0);
+        //     }
+        // }
         // Vision
         if (map.vision != null) {
             if(getAButtonPressed()){
@@ -149,8 +149,8 @@ else if (getRightBumperPressed()) {
 
 
         // West Coast
-        if (map.westcoast != null) {
-            map.westcoast.arcadeDrive(getLeftY(), getRightX());
-        }
+        // if (map.westcoast != null) {
+        //     map.westcoast.arcadeDrive(getLeftY(), getRightX());
+        // }
     }
 }

src/main/java/frc/robot/RobocketsShuffleboard.java

@@ -9,6 +9,7 @@ public static void teleopInit() {
         addNumber("Shooter Out Speed", 0.5);
         addNumber("Shooter Intake Speed", 0.5);
         addNumber("Shooter Outtake Speed", 0.5);
+
     }
 
 

src/main/java/frc/robot/Robot.java

@@ -23,15 +23,15 @@ public class Robot extends TimedRobot {
   // This is also the file where all the subsystems reside
   private static RobotMap map = new RobotMap();
   public static RobotMap getMap() { return map; }
-  public static RobocketsController controller = new RobocketsController(Constants.CONTROLLER_PORT, map);
+  // public static RobocketsController controller = new RobocketsController(Constants.CONTROLLER_PORT, map);
 
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
   @Override
   public void robotInit() {
-    AutoConstruct.sendAutoOptionsToSmartDashboard();
+    // AutoConstruct.sendAutoOptionsToSmartDashboard();
   }
 
   /**
@@ -56,14 +56,15 @@ public void robotPeriodic() {}
    */
   @Override
   public void autonomousInit() {
-    // CommandScheduler.getInstance().schedule(new SwerveGoCartesianF(map.swerve, new Translation2d(0, 6)));
     AutoConstruct.scheduleSelectedCommand(map);
   }
 
   /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
-    CommandScheduler.getInstance().run();
+
+
+    //CommandScheduler.getInstance().run();
 
     //Gian: I'm not so sure why we would ever use this if all the auto code is done in the commandscheduler
 
@@ -88,15 +89,9 @@ public void teleopInit() {
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {
-    controller.teleopPeriodic();
-    if (map.vision != null) {
-      var result = map.vision.getLatestResult();
-
-      if(result.hasTargets()){
-        System.out.println(result.getBestTarget());
-      }
-    }
+    // controller.teleopPeriodic();
     // Run any commands
+    map.vision.getPoseFromTag();
     CommandScheduler.getInstance().run();
   }
 

src/main/java/frc/robot/RobotMap.java

@@ -7,7 +7,7 @@
 import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
 import frc.robot.subsystems.swerve.SwerveModuleNeo;
 import frc.robot.subsystems.vision.VisionSubsystem;
-import frc.robot.subsystems.westcoast.WestCoastSubsystem;
+// import frc.robot.subsystems.westcoast.WestCoastSubsystem;
 
 // Contains all physical items on the robot (motors, encoders, LEDs, etc)
 public class RobotMap
@@ -32,8 +32,8 @@ public class RobotMap
     public RobotMap() 
     {
         // intake = new IntakeSubsystem();
+        vision = new VisionSubsystem();
         swerve = new SwerveDriveSubsystem(new Translation2d(-0.31115, 0.31115), new Translation2d(0.31115, 0.31115), new Translation2d(-0.31115, -0.31115), new Translation2d(0.31115, -0.31115));    // All translations are the swerve module positions relative to the center of the bot
-        // vision = new VisionSubsystem();
         //shooter = new ShooterSubsystem();
         // leds=new LedSubsystem();
 

src/main/java/frc/robot/field/Field.java

@@ -12,19 +12,19 @@ public class Field {
     //-------
     // Speaker
     //-------
-    public static final Speaker SPEAKER = new Speaker(SPEAKER_TOP_LEFT, SPEAKER_BOTTOM_RIGHT);
-    public static final Subwoofer SUBWOOFER = new Subwoofer(SUBWOOFER_TOP_LEFT, SUBWOOFER_BOTTOM_RIGHT);
+    // public static final Speaker SPEAKER = new Speaker(SPEAKER_TOP_LEFT, SPEAKER_BOTTOM_RIGHT);
+    // public static final Subwoofer SUBWOOFER = new Subwoofer(SUBWOOFER_TOP_LEFT, SUBWOOFER_BOTTOM_RIGHT);
 
     //-------
     // Stage
     //-------
-    public static final Stage STAGE =
-        new Stage(
-            LEFT_STAGE_POST_TOP_LEFT, TOP_STAGE_POST_CENTER, BOTTOM_STAGE_POST_CENTER,
-            RIGHT_TRAP_POINT1, RIGHT_TRAP_POINT2,
-            TOP_TRAP_POINT1, TOP_TRAP_POINT2,
-            BOTTOM_TRAP_POINT1, BOTTOM_TRAP_POINT2
-        );
+    // public static final Stage STAGE =
+    //     new Stage(
+    //         LEFT_STAGE_POST_TOP_LEFT, TOP_STAGE_POST_CENTER, BOTTOM_STAGE_POST_CENTER,
+    //         RIGHT_TRAP_POINT1, RIGHT_TRAP_POINT2,
+    //         TOP_TRAP_POINT1, TOP_TRAP_POINT2,
+    //         BOTTOM_TRAP_POINT1, BOTTOM_TRAP_POINT2
+    //     );
 
     //-------
     // Notes

src/main/java/frc/robot/field/TagPositions.java

@@ -20,4 +20,4 @@ public static AprilTagFieldLayout getAprilTagFieldLayout(){
     public static Optional<Pose3d> getTagPose3d(int ID){
         return getAprilTagFieldLayout().getTagPose(ID);
     }
-}
+}

src/main/java/frc/robot/subsystems/shooter/Shoot.java

@@ -18,7 +18,7 @@ public void initialize() {}
 
     @Override
     public void execute() {
-        Robot.getMap().shooter.setSpeed(speed);
+        // Robot.getMap().shooter.setSpeed(speed);
     }
 
     @Override
@@ -30,6 +30,6 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        Robot.getMap().shooter.setSpeed(0);
+        // Robot.getMap().shooter.setSpeed(0);
     }
 }