buildspace changes

Author: AyushSagar16

src/main/deploy/pathplanner/paths/left_intake.path

@@ -32,12 +32,12 @@
     },
     {
       "anchor": {
-        "x": 2.622507500654099,
-        "y": 6.913091720920284
+        "x": 2.560783467431597,
+        "y": 6.954241076401952
       },
       "prevControl": {
-        "x": 2.272737979059918,
-        "y": 6.727919621252776
+        "x": 2.2110139458374154,
+        "y": 6.769068976734444
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/left_shoot.path

@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -48,5 +48,5 @@
     "rotation": 40.92725638742595,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/middle_intake.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.66,
-        "y": 5.67
+        "x": 2.64,
+        "y": 5.51401363454356
       },
       "prevControl": {
-        "x": 1.9028194056388115,
-        "y": 5.651342508139562
+        "x": 1.1931420754069582,
+        "y": 5.378601946827955
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/middle_left_intake.path

@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.2,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

src/main/deploy/pathplanner/paths/middle_middle_intake.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.8427978760719429,
-        "y": 6.635333571419023
+        "x": 1.3160154641111295,
+        "y": 5.51401363454356
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.871909586805377,
-        "y": 6.635333571419023
+        "x": 1.3451271748445637,
+        "y": 5.51401363454356
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.5710708063020142,
-        "y": 6.995390431883621
+        "x": 2.4476227398570094,
+        "y": 5.51401363454356
       },
       "prevControl": {
-        "x": 2.5596865489604834,
-        "y": 6.995390431883621
+        "x": 2.4362384825154786,
+        "y": 5.51401363454356
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 58.49573328079566,
+    "rotation": 0.0,
     "velocity": 0
   },
   "useDefaultConstraints": false

src/main/java/org/ghrobotics/frc2024/Robot.java

@@ -21,8 +21,11 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 /**
@@ -53,9 +56,11 @@ public class Robot extends TimedRobot {
   // Controller
   private final CommandXboxController driver_controller_ = new CommandXboxController(0);
   private final CommandXboxController operator_controller_ = new CommandXboxController(1);
+  // private final CommandPS4Controller operator_controller_ = new CommandPS4Controller(1);
+
 
   // Superstructure
-  private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_, climber_);
+  private final Superstructure superstructure_ = new Superstructure(arm_, intake_, shooter_, feeder_, robot_state_, climber_, drive_);
   private final AutoSelector auto_selector_= new AutoSelector(drive_, robot_state_, superstructure_, arm_, intake_, shooter_, feeder_);
 
   // Telemetry
@@ -72,6 +77,8 @@ public void robotInit() {
 
     // Just to test the blue subwoofer distance
     // robot_state_.reset(limelight_.getEstimatedVisionRobotPose());
+
+    // drive_.leftRightSteerEcoder();
   }
 
   @Override
@@ -84,13 +91,15 @@ public void robotPeriodic() {
       robot_state_.update();
 
     SmartDashboard.putBoolean("Auto", isAuto);
-    SmartDashboard.putNumber("Vision x", limelight_.getBotPose2d().getX());
-    SmartDashboard.putNumber("Vision y", limelight_.getBotPose2d().getY());
-    SmartDashboard.putNumber("Vision Degrees", limelight_.getBotPose2d().getRotation().getDegrees());
+    // SmartDashboard.putNumber("Vision x", limelight_.getBotPose2d().getX());
+    // SmartDashboard.putNumber("Vision y", limelight_.getBotPose2d().getY());
+    // SmartDashboard.putNumber("Vision Degrees", limelight_.getBotPose2d().getRotation().getDegrees());
 
     SmartDashboard.putData(auto_selector_.getRoutineChooser());
     SmartDashboard.putData(auto_selector_.getPositionChooser());
 
+    SmartDashboard.putNumber("Vision X", LimelightHelpers.getTX("limelight"));
+
     // if(!Robot.isSimulation())
     //   field_.setRobotPose(robot_state_.getPosition());
   }
@@ -150,31 +159,34 @@ private void setupTeleopControls() {
 
     // Driver Control
     //  * RT:  Spin Shooter
-    driver_controller_.rightTrigger().whileTrue(superstructure_.setShooterPercent(0.85));
+    driver_controller_.rightTrigger().whileTrue(superstructure_.setShooterPercent(0.95));
 
     driver_controller_.rightBumper().whileTrue(superstructure_.setShooterPercent(-0.6));
 
     // driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.55));
 
     // driver_controller_.pov(270).whileTrue(superstructure_.setShooter(85));
 
-    driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(0.55));
+    driver_controller_.leftTrigger().whileTrue(new ParallelCommandGroup(
+      superstructure_.setIntake(0.55),
+      superstructure_.align())
+    );
 
     driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(-0.25));
 
     driver_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.056));
 
     driver_controller_.a().whileTrue(superstructure_.setFeeder(0.85));
 
-    driver_controller_.b().whileTrue(superstructure_.setFeeder(-0.75));
+    driver_controller_.b().whileTrue(superstructure_.setFeeder(-0.35));
 
     driver_controller_.y().onTrue(new InstantCommand(() -> drive_.resetGyro()));
 
     // driver_controller_.pov(90).whileTrue(superstructure_.setShooter(90));
 
-    driver_controller_.pov(90).whileTrue(superstructure_.setArmPercent(0.15));
+    driver_controller_.pov(90).whileTrue(superstructure_.setPosition(Superstructure.Position.SUBWOOFER));
 
-    driver_controller_.pov(270).whileTrue(superstructure_.setArmPercent(-0.15));
+    driver_controller_.pov(270).whileTrue(superstructure_.setPosition(Superstructure.Position.GROUND_INTAKE));
 
 
     // Operator Control

src/main/java/org/ghrobotics/frc2024/Superstructure.java

@@ -1,7 +1,11 @@
 package org.ghrobotics.frc2024;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -14,6 +18,7 @@
 import org.ghrobotics.frc2024.commands.ArmPID;
 import org.ghrobotics.frc2024.subsystems.Arm;
 import org.ghrobotics.frc2024.subsystems.Climber;
+import org.ghrobotics.frc2024.subsystems.Drive;
 import org.ghrobotics.frc2024.subsystems.Feeder;
 import org.ghrobotics.frc2024.subsystems.Intake;
 import org.ghrobotics.frc2024.subsystems.Shooter;
@@ -26,6 +31,7 @@ public class Superstructure {
   private final Feeder feeder_;
   private final Climber climber_;
   private final RobotState robot_state_;
+  private final Drive drive_;
   // private final ShootingPosition shootingPosition_ = new ShootingPosition();
 
 
@@ -36,14 +42,19 @@ public class Superstructure {
   public double armShootingAngle;
   public double shootingDistance;
 
+  public double y_velocity;
+
+  private final PIDController pid_ = new PIDController(0.2, 0, 0);
+
   // Constructor
-  public Superstructure(Arm arm, Intake intake, Shooter shooter, Feeder feeder, RobotState robot_state, Climber climber) {
+  public Superstructure(Arm arm, Intake intake, Shooter shooter, Feeder feeder, RobotState robot_state, Climber climber, Drive drive) {
     arm_ = arm;
     intake_ = intake;
     shooter_ = shooter;
     feeder_ = feeder;
     climber_ = climber;
     robot_state_ = robot_state;
+    drive_ = drive;
   }
 
   public void periodic() {
@@ -57,10 +68,22 @@ public void periodic() {
 
     SmartDashboard.putString("Superstructure State", state);
 
+    y_velocity = MathUtil.clamp(pid_.calculate(LimelightHelpers.getTX("limelight"), -9.8), -0.5, 0.5);
+    SmartDashboard.putNumber("Vision Error", pid_.getPositionError());
+
     // Checks output current to see if note has intaked or not (current > 35 means intaked)
     if (intake_.getRightOutputCurrent() > 45) {
       LimelightHelpers.setLEDMode_ForceOn("limelight");
     }
+
+    if(IO.align == true) {
+      drive_.setSpeeds(new ChassisSpeeds(0, y_velocity, 0), Drive.OutputType.OPEN_LOOP);
+    }
+    else {
+      drive_.setSpeeds(drive_.getSpeeds());
+    }
+
+    
   }
 
   // Position Setter
@@ -135,6 +158,33 @@ public Command autoArm(double angle_deg) {
     );
   }
 
+  public Command align() {
+    return new StartEndCommand(
+      () -> IO.align = true,
+      () -> IO.align= false, 
+      drive_
+    );
+  }
+
+  public Command autoAlign() {
+    // pid_.setSetpoint(-9.8);
+
+    
+
+    return new FunctionalCommand(
+      () -> SmartDashboard.putNumber("Started", 0.1), 
+      () -> drive_.setSpeeds(new ChassisSpeeds(0, y_velocity, 0), Drive.OutputType.OPEN_LOOP), 
+      (interrupted) -> drive_.setSpeeds(new ChassisSpeeds(0, 0, 0), Drive.OutputType.OPEN_LOOP),
+      () -> (pid_.getPositionError() < 0.5), 
+      drive_
+      );
+  }
+
+  public void alignNote() {
+    
+    drive_.setSpeeds(new ChassisSpeeds(0, y_velocity, 0), Drive.OutputType.OPEN_LOOP);
+  }
+
   /**
    * Feeder Setter
    * @param percent
@@ -188,6 +238,10 @@ public String getState() {
     return state;
   }
 
+  public static class IO {
+    public static boolean align;
+  }
+
   public enum Position {
     STOW(55, "STOW"),
     SUBWOOFER(16.2, "SUBWOOFER"),