Merge pull request #8 from Ocebots/subsystems

drive code

Author: Yaypixel

.idea/modules.xml

@@ -0,0 +1,2 @@
+#This file is generated by updateDaemonJvm
+toolchainVersion=17

gradle/gradle-daemon-jvm.properties

@@ -4,17 +4,26 @@
 
 package frc.robot;
 
+import static frc.robot.config.VisionConfig.photonPoseEstimatorForward;
+import static frc.robot.config.VisionConfig.result;
+
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.config.TunerConstants;
+import frc.robot.config.VisionConfig;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
 
 @Logged
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
+  private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
 
   private final RobotContainer m_robotContainer;
 
@@ -28,6 +37,19 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    Optional<EstimatedRobotPose> visionEst =
+        VisionConfig.photonPoseEstimatorForward.estimateCoprocMultiTagPose(result);
+    if (visionEst.isEmpty()) {
+      visionEst = VisionConfig.photonPoseEstimatorForward.estimateLowestAmbiguityPose(result);
+    } else {
+      drivetrain.addVisionMeasurement(
+          photonPoseEstimatorForward
+              .estimateAverageBestTargetsPose(result)
+              .get()
+              .estimatedPose
+              .toPose2d(),
+          visionEst.get().timestampSeconds);
+    }
   }
 
   @Override

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

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.*;
 import frc.robot.config.CANMappings;
+import frc.robot.config.HopperConfig;
 import frc.robot.config.TunerConstants;
 import frc.robot.subsystems.*;
 
@@ -24,18 +25,26 @@ public class RobotContainer {
   private Climb climb = new Climb();
   private Intake intake = new Intake();
   private Kicker kicker = new Kicker();
-  private CommandXboxController controller = new CommandXboxController(0);
+  private CommandXboxController controller = new CommandXboxController(1);
   private SwerveDriveState driveState = new SwerveDriveState();
+  public static boolean isExtended = false;
 
   private Command shootGroup =
       Commands.parallel(
               new KickerCommand(kicker, KickerCommand.Position.INTAKE),
-              new HopperCommand(hopper, HopperCommand.Position.SHOOT_RETRACT),
+              Commands.runEnd(
+                  () -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION),
+                  () -> hopper.stop(),
+                  hopper).withDeadline(Commands.waitSeconds(2)),
               new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE))
           .finallyDo(
               interrupt ->
                   CommandScheduler.getInstance()
-                      .schedule(new HopperCommand(hopper, HopperCommand.Position.SHOOT_EXTEND)));
+                      .schedule(
+                          Commands.runEnd(
+                              () -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION),
+                              () -> hopper.stop(),
+                              hopper).withDeadline(Commands.waitSeconds(2))));
 
   public RobotContainer() {
     configureBindings();
@@ -63,12 +72,22 @@ private void configureBindings() {
         .x()
         .and(() -> !shootGroup.isScheduled())
         .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.INTAKE));
+
+    controller
+            .povRight()
+            .and(() -> !shootGroup.isScheduled())
+            .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE));
     // Right Stick Down = Extend/Retract Hopper
     controller
         .rightStick()
         .and(() -> !shootGroup.isScheduled())
-        .onTrue(new HopperCommand(hopper, HopperCommand.Position.EXTEND_RETRACT));
-    // Right Trigger = Climb Retract
+        .onTrue(
+            Commands.runEnd(
+                    () -> hopper.move(Hopper.getRotation(isExtended)), () -> hopper.stop(), hopper)
+                .withDeadline(Commands.waitSeconds(2))
+                .andThen(Commands.runOnce(() -> isExtended = !isExtended)));
+
+    //    // Right Trigger = Climb Retract
     controller
         .rightTrigger()
         .whileTrue(new ClimbCommand(climb, ClimbCommand.Position.DIRECTIONAL_CLIMB));
@@ -92,6 +111,7 @@ private void configureBindings() {
         .a()
         .toggleOnTrue(
             new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain));
+    controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/RobotContainer.java

@@ -70,24 +70,21 @@ public void execute() {
 
       // Default driving mode
       case TELEOP:
-        subsystem.applyRequest(
-            () ->
-                drive
-                    .withVelocityX(
-                        -leftY.getAsDouble() * MaxSpeed) // Drive forward with negative Y (forward)
-                    .withVelocityY(-leftX.getAsDouble() * MaxSpeed) // Drive left with negative X
-                    .withRotationalRate(
-                        -rightX.getAsDouble()
-                            * MaxAngularRate)); // Drive counterclockwise with negative X
-        //        System.out.println("Drivetrain: Teleop");
+        subsystem.setControl(
+            drive
+                .withVelocityX(
+                    -leftY.getAsDouble() * MaxSpeed) // Drive forward with negative Y (forward)
+                .withVelocityY(-leftX.getAsDouble() * MaxSpeed) // Drive left with negative X
+                .withRotationalRate(
+                    -rightX.getAsDouble()
+                        * MaxAngularRate)); // Drive counterclockwise with negative X
         break;
 
       // (Needs check) Mode for when shots are from a still position
       case STILL_SHOT:
         // make X with wheels, set wheels to brake mode
         applyRequest.ModuleStates = states;
         subsystem.setControl(applyRequest);
-        subsystem.setControl(brakeRequest);
         System.out.println("Drivetrain: Still Shot Configuration");
         break;
 

src/main/java/frc/robot/commands/DrivetrainCommand.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
 import frc.robot.config.HopperConfig;
 import frc.robot.subsystems.Hopper;
 
@@ -15,11 +16,12 @@ public static enum Position {
 
   private Hopper subsystem;
   private HopperCommand.Position pose;
-  private static boolean isExtended = false;
+  private boolean isExtended;
 
-  public HopperCommand(Hopper subsystem, HopperCommand.Position pose) {
+  public HopperCommand(Hopper subsystem, HopperCommand.Position pose, boolean isExtended) {
     this.pose = pose;
     this.subsystem = subsystem;
+    this.isExtended = isExtended;
 
     addRequirements(subsystem);
   }
@@ -31,13 +33,15 @@ public void initialize() {
       // (Needs check) If hopper already extended, retract, and if hopper retracted, extend
       case EXTEND_RETRACT:
         if (isExtended) {
-          //          System.out.println("Hopper: Retracting");
+          System.out.println("Hopper: Retracting");
           subsystem.move(HopperConfig.HOPPER_RETRACT_ROTATION);
-          isExtended = false;
+          RobotContainer.isExtended = false;
+          System.out.println(RobotContainer.isExtended);
         } else {
-          //          System.out.println("Hopper: Extending");
+          System.out.println("Hopper: Extending");
           subsystem.move(HopperConfig.HOPPER_EXTEND_ROTATION);
-          isExtended = true;
+          RobotContainer.isExtended = true;
+          System.out.println(RobotContainer.isExtended);
         }
         break;
 

src/main/java/frc/robot/commands/HopperCommand.java

@@ -22,17 +22,17 @@ public class FlywheelConfig {
   // Set
   public static final double FLYWHEEL_LEFT_MAX_CRUISE_VELOCITY = 3000;
   public static final double FLYWHEEL_LEFT_TARGET_ACCELERATION = 500;
-  public static final double FLYWHEEL_LEFT_P = 10;
+  public static final double FLYWHEEL_LEFT_P = 0.5;
   public static final double FLYWHEEL_LEFT_I = 0;
   public static final double FLYWHEEL_LEFT_D = 0;
-  public static final double FLYWHEEL_LEFT_S = 0;
-  public static final double FLYWHEEL_LEFT_V = 0;
+  public static final double FLYWHEEL_LEFT_S = 0.18;
+  public static final double FLYWHEEL_LEFT_V = 0.115;
   public static final double FLYWHEEL_LEFT_A = 0;
 
   // Set
   public static final double FLYWHEEL_OUTTAKE_SPEED = 0;
-  public static final double FLYWHEEL_HUB_SHOT_SPEED = 0;
+  public static final double FLYWHEEL_HUB_SHOT_SPEED = 40;
   public static final double FLYWHEEL_TOWER_SHOT_SPEED = 0;
-  public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 0;
+  public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40;
   public static final double FLYWHEEL_TOLERANCE = 5;
 }

src/main/java/frc/robot/config/FlywheelConfig.java

@@ -8,17 +8,20 @@ public class HopperConfig {
   // Set
   public static final double HOPPER_MAX_CRUISE_VELOCITY = 3000;
   public static final double HOPPER_TARGET_ACCELERATION = 500;
-  public static final double HOPPER_P = 0;
+  public static final double HOPPER_P = 1.5;
   public static final double HOPPER_I = 0;
   public static final double HOPPER_D = 0;
+  public static final double HOPPER_S = 0;
+  public static final double HOPPER_V = 0;
+  public static final double HOPPER_A = 0;
 
-  public static final double SLOW_HOPPER_P = 0;
+  public static final double SLOW_HOPPER_P = 1;
   public static final double SLOW_HOPPER_I = 0;
   public static final double SLOW_HOPPER_D = 0;
 
-  public static final double HOPPER_TOLERANCE = 0.01;
+  public static final double HOPPER_TOLERANCE = 0.2;
 
   // Set
-  public static final double HOPPER_EXTEND_ROTATION = 0.0;
-  public static final double HOPPER_RETRACT_ROTATION = 0.0;
+  public static final double HOPPER_EXTEND_ROTATION = 4.1;
+  public static final double HOPPER_RETRACT_ROTATION = 0.1;
 }

src/main/java/frc/robot/config/HopperConfig.java

@@ -6,7 +6,7 @@ public class IntakeConfig {
   public static final double INTAKE_GEAR_RATIO = 1;
 
   // Set
-  public static final double INTAKE_INTAKE_SPEED = 0;
-  public static final double INTAKE_OUTTAKE_SPEED = 0;
+  public static final double INTAKE_INTAKE_SPEED = 4;
+  public static final double INTAKE_OUTTAKE_SPEED = 4;
   public static final double INTAKE_SLOW_SPEED = 0;
 }

src/main/java/frc/robot/config/IntakeConfig.java

@@ -16,8 +16,8 @@ public class KickerConfig {
   public static final double KICKER_V = 0;
   public static final double KICKER_A = 0;
 
-  public static final double KICKER_INTAKE_SPEED = 0;
-  public static final double KICKER_OUTTAKE_SPEED = 0;
+  public static final double KICKER_INTAKE_SPEED = 11;
+  public static final double KICKER_OUTTAKE_SPEED = 11;
 
   public static final double KICKER_FRONT_INTAKE_SPEED = 0;
   public static final double KICKER_FRONT_OUTTAKE_SPEED = 0;