More changes

src/main/java/org/team1540/elmo/Constants.java

@@ -43,7 +43,7 @@ public final class Constants {
 
     // APRILTAGS
     public static final int TOWER_APRILTAG_ID = 15;
-    public static final String CAMERA_NAME = "photonvision";
+    public static final String CAMERA_NAME = "Microsoft_LifeCam_HD-3000";
 
     // SELF MEASUREMENTS
     public static final double CAMERA_HEIGHT_METERS = 2;

src/main/java/org/team1540/elmo/RobotContainer.java

@@ -66,30 +66,28 @@ private void configureButtonBindings()
 
 
         //////////////////// SUCTION /////////////////////
+        ///// Eject /////
         // left side
-        new JoystickButton(driver,XboxController.Button.kX.value).and(neitherBumper).whenActive(
+        new JoystickButton(driver,XboxController.Button.kX.value).and(eitherBumper).whenActive(
                 new EjectInnerCommand(innerEjector)
         );
         // right side
-        new JoystickButton(driver,XboxController.Button.kB.value).and(neitherBumper).whenActive(
+        new JoystickButton(driver,XboxController.Button.kB.value).and(eitherBumper).whenActive(
                 new EjectOuterCommand(outerEjector)
         );
         // upper
-        new JoystickButton(driver,XboxController.Button.kY.value).and(neitherBumper).whenActive(
+        new JoystickButton(driver,XboxController.Button.kY.value).and(eitherBumper).whenActive(
                 new EjectUpperCommand(upperEjector)
         );
-        new JoystickButton(driver,XboxController.Button.kY.value).and(neitherBumper).whileActiveOnce(
-                new DriveToAprilTagPIDCommand(Constants.TOWER_APRILTAG_ID,0.1,driveTrain,camera)
-        );
 
         // toggle individual suction when either bumper and X:inner / B:outer / Y:upper are pressed
-        new JoystickButton(driver,XboxController.Button.kX.value).and(eitherBumper).toggleWhenActive(
+        new JoystickButton(driver,XboxController.Button.kX.value).and(neitherBumper).toggleWhenActive(
             new ResetEjectorAndWaitForeverCommand(innerEjector,true)
         );
-        new JoystickButton(driver,XboxController.Button.kB.value).and(eitherBumper).toggleWhenActive(
+        new JoystickButton(driver,XboxController.Button.kB.value).and(neitherBumper).toggleWhenActive(
             new ResetEjectorAndWaitForeverCommand(outerEjector,true)
         );
-        new JoystickButton(driver,XboxController.Button.kY.value).and(eitherBumper).toggleWhenActive(
+        new JoystickButton(driver,XboxController.Button.kY.value).and(neitherBumper).toggleWhenActive(
             new ResetEjectorAndWaitForeverCommand(upperEjector,true)
         );
 
@@ -111,8 +109,17 @@ private void configureButtonBindings()
         );
 
 //        driveTrain.setDefaultCommand(new TankDrive(driveTrain, driver));
+//        driveTrain.setDefaultCommand(new BobaTankDrive(driveTrain, driver));
          driveTrain.setDefaultCommand(new ArcadeDrive(driveTrain, driver));
 
+//        new JoystickButton(driver,XboxController.Button.kA.value).whileActiveOnce(
+//                new DriveToAprilTagPIDCommand(Constants.TOWER_APRILTAG_ID,0.1,driveTrain,camera)
+//        );
+        new JoystickButton(driver,XboxController.Button.kA.value).toggleWhenPressed(
+                new SegmentedAuto(driveTrain,upperEjector,outerEjector,innerEjector,camera)
+        );
+
+
 
         // Add button to command mappings here.
         // See https://docs.wpilib.org/en/stable/docs/software/commandbased/binding-commands-to-triggers.html
@@ -127,6 +134,6 @@ private void configureButtonBindings()
     public Command getAutonomousCommand()
     {
         // An ExampleCommand will run in autonomous
-        return new Auto(driveTrain,upperEjector,outerEjector,innerEjector,camera);
+        return new SegmentedAuto(driveTrain,upperEjector,outerEjector,innerEjector,camera);
     }
 }

src/main/java/org/team1540/elmo/subsystems/drivetrain/ArcadeDrive.java

@@ -25,7 +25,7 @@ public void initialize() {
 
     @Override
     public void execute() {
-        double spinPercent = -driver.getLeftX();
+        double spinPercent = -driver.getLeftX()/1.8;
         double forwardPercent = driver.getLeftY();
 
         // all shit handled in drivetrain functions

src/main/java/org/team1540/elmo/subsystems/drivetrain/Auto.java

@@ -16,17 +16,12 @@ public Auto(DriveTrain driveTrain, Ejector.UpperEjector upperEjector, Ejector.Ou
                 // drive forward until can see april tag
                 deadline(
                         new WaitUntilCanSeeAprilTagCommand(Constants.TOWER_APRILTAG_ID,camera),
-                        new DriveForwardRampedForeverCommand(0.8,driveTrain)
+                        new DriveForwardRampedForeverCommand(-0.8,driveTrain)
                 ),
                 // drive fast (w/ ramping) pointing to tag until close to april tag
                 deadline(
-                        new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,2,camera),
-                        new DriveToAprilTagCommand(Constants.TOWER_APRILTAG_ID,0.7,1,driveTrain,camera)
-                ),
-                // drive slow (w/ ramping) pointing to tag until very close to april tag
-                deadline(
-                        new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,0.1,camera),
-                        new DriveToAprilTagCommand(Constants.TOWER_APRILTAG_ID,0.1,1,driveTrain,camera)
+                        new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,0.4,camera),
+                        new DriveToAprilTagPIDCommand(Constants.TOWER_APRILTAG_ID,0.1,driveTrain,camera)
                 ),
                 // drive forward slowly for last few seconds
                 deadline(
@@ -35,14 +30,12 @@ public Auto(DriveTrain driveTrain, Ejector.UpperEjector upperEjector, Ejector.Ou
                 ),
                 // stop drivetrain (right now no ramping, possibly add ramping?)
                 new InstantCommand(driveTrain::stop,driveTrain),
-                // eject all tubes!
+                // eject all the tubes!
                 parallel(
                         new EjectUpperCommand(upperEjector),
                         new EjectInnerCommand(innerEjector),
                         new EjectOuterCommand(outerEjector)
                 ),
-                // wait for tubes to fully eject
-                new WaitCommand(2),
                 // back up from the tower for a few seconds to give better position for driver to take over
                 deadline(
                         new DriveForwardRampedForeverCommand(-0.3,driveTrain),

src/main/java/org/team1540/elmo/subsystems/drivetrain/BobaTankDrive.java

@@ -0,0 +1,58 @@
+package org.team1540.elmo.subsystems.drivetrain;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.team1540.elmo.utils.SignedSlewRateLimiter;
+
+/**
+ * Standard drive, left stick controls left side of drivetrain, right stick controls right side.
+ */
+public class BobaTankDrive extends CommandBase {
+
+    private final DriveTrain drivetrain;
+    private final XboxController controller;
+    private final double deadzone = 0.15;
+
+    private final SignedSlewRateLimiter speedRateLimiter = new SignedSlewRateLimiter(2,2);
+    private final SignedSlewRateLimiter spinRateLimiter = new SignedSlewRateLimiter(3,3);
+
+    public BobaTankDrive(DriveTrain drivetrain, XboxController controller) {
+        this.drivetrain = drivetrain;
+        this.controller = controller;
+        addRequirements(drivetrain);
+    }
+
+    @Override
+    public void execute() {
+        double left = Math.abs(controller.getLeftY()) > deadzone ? controller.getLeftY() : 0;
+        double right = Math.abs(controller.getRightY()) > deadzone ? controller.getRightY() : 0;
+//        double forward = Math.abs(controller.getRightTriggerAxis()) > deadzone ? Math.pow(controller.getRightTriggerAxis(), 3) : 0;
+//        double backward = Math.abs(controller.getLeftTriggerAxis()) > deadzone ? Math.pow(controller.getLeftTriggerAxis(), 3) : 0;
+//        double multiplier = 0.5 - ((0.3 * elevator.getRotations())/ElevatorConstants.ELEVATOR_ROTS_TO_TOP);
+//        speedRateLimiter.setAccelerationLimit(2-(elevatorPercentHeight*1)); // at bottom = 2-0%*1, at top: 2-100%*1
+
+        double speed = (left + right) / 2;
+        double spin = left - speed; // this changes sign lol
+        ///
+        speed = Math.pow(speed,2) * Math.signum(speed);
+
+        spin = Math.pow(spin,2) * Math.signum(spin);
+        spin = spin/2;
+        ///
+        double rateLimitedSpeed = speedRateLimiter.calculate(speed);
+        double rateLimitedSpin = spinRateLimiter.calculate(spin);
+
+//        double rateLimitedLeft = speed + spin;
+//        double rateLimitedRight = speed - spin;
+        double rateLimitedLeft = rateLimitedSpeed + rateLimitedSpin;
+        double rateLimitedRight = rateLimitedSpeed - rateLimitedSpin;
+
+//        drivetrain.setPercent(multiplier*(-1*left + forward - backward), multiplier*(-1*right + forward - backward));
+        drivetrain.setPercent(rateLimitedLeft,rateLimitedRight);
+    }
+
+    @Override
+    public void end(boolean isInterrupted) {
+        drivetrain.stop();
+    }
+}

src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagPIDCommand.java

@@ -46,9 +46,16 @@ public void execute() {
         );
         double pitch = target.getPitch();
 
+        System.out.println("DRIVE PID COMMAND");
+            System.out.print("Distance");
+            System.out.println(distance);
+
         double speed = -pid.calculate(distance);
         double spin = pitch/180;
 
+            System.out.print("speed");
+            System.out.println(speed);
+
         driveTrain.setSpeedAndSpin(speed,spin);
     }
 }

src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveTrain.java

@@ -86,7 +86,7 @@ public void setPercent(double leftPercent, double rightPercent) {
     }
 
     ///////////////// Differential Drive Mode Ramping /////////////////
-    private final double defaultAccLimit = 5;
+    private final double defaultAccLimit = 4;
     private final double defaultDecLimit = 3;
     private double leftRampedSetpoint = 0;
     private double rightRampedSetpoint = 0;

src/main/java/org/team1540/elmo/subsystems/drivetrain/SegmentedAuto.java

@@ -0,0 +1,54 @@
+package org.team1540.elmo.subsystems.drivetrain;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import org.team1540.elmo.Constants;
+import org.team1540.elmo.subsystems.ejectors.EjectInnerCommand;
+import org.team1540.elmo.subsystems.ejectors.EjectOuterCommand;
+import org.team1540.elmo.subsystems.ejectors.EjectUpperCommand;
+import org.team1540.elmo.subsystems.ejectors.Ejector;
+import org.team1540.elmo.utils.ChickenPhotonCamera;
+
+public class SegmentedAuto extends SequentialCommandGroup {
+    public SegmentedAuto(DriveTrain driveTrain, Ejector.UpperEjector upperEjector, Ejector.OuterEjector outerEjector, Ejector.InnerEjector innerEjector, ChickenPhotonCamera camera) {
+        super(
+                // drive forward until can see april tag
+                deadline(
+                        new WaitUntilCanSeeAprilTagCommand(Constants.TOWER_APRILTAG_ID,camera),
+                        new DriveForwardRampedForeverCommand(-0.8,driveTrain)
+                ),
+                // drive fast (w/ ramping) pointing to tag until close to april tag
+                deadline(
+                        new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,2,camera),
+                        new DriveToAprilTagCommand(Constants.TOWER_APRILTAG_ID,0.7,1,driveTrain,camera)
+                ),
+                // drive slow (w/ ramping) pointing to tag until very close to april tag
+                deadline(
+                        new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,0.1,camera),
+                        new DriveToAprilTagCommand(Constants.TOWER_APRILTAG_ID,0.1,1,driveTrain,camera)
+                ),
+                // drive forward slowly for last few seconds
+                deadline(
+                        new WaitCommand(1),
+                        new DriveForwardRampedForeverCommand(0.1,driveTrain)
+                ),
+                // stop drivetrain (right now no ramping, possibly add ramping?)
+                new InstantCommand(driveTrain::stop,driveTrain),
+                // eject all tubes!
+                parallel(
+                        new EjectUpperCommand(upperEjector),
+                        new EjectInnerCommand(innerEjector),
+                        new EjectOuterCommand(outerEjector)
+                ),
+                // wait for tubes to fully eject
+                new WaitCommand(2),
+                // back up from the tower for a few seconds to give better position for driver to take over
+                deadline(
+                        new DriveForwardRampedForeverCommand(-0.3,driveTrain),
+                        new WaitCommand(1.5)
+                ),
+                new InstantCommand(driveTrain::stopRamped,driveTrain)
+        );
+    }
+}

src/main/java/org/team1540/elmo/subsystems/drivetrain/TankDrive.java

@@ -63,12 +63,13 @@ public void execute() {
         // double leftOutputPercent = totalOutputPercent * leftPercent / totalPercent; // proportion of left to total
         // double rightOutputPercent = totalOutputPercent * rightPercent / totalPercent; // proportion of 
 
-        double leftOutPercent = leftRateLimiter.calculate(leftPercent);
-        double rightOutPercent = rightRateLimiter.calculate(rightPercent);
+//        double leftOutPercent = leftRateLimiter.calculate(leftPercent);
+//        double rightOutPercent = rightRateLimiter.calculate(rightPercent);
+//        driveTrain.setPercentRamped(leftOutPercent,rightOutPercent);
+//        SmartDashboard.putNumber(getName()+"/leftPercent", leftOutPercent);
+//        SmartDashboard.putNumber(getName()+"/rightPercent", rightOutPercent);
 
-        driveTrain.setPercent(leftOutPercent,rightOutPercent);
+        driveTrain.setPercentRamped(leftPercent,rightPercent);
 
-        SmartDashboard.putNumber(getName()+"/leftPercent", leftOutPercent);
-        SmartDashboard.putNumber(getName()+"/rightPercent", rightOutPercent);
     }
 }

src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilAprilTagIsWithinXMetersCommand.java

@@ -14,7 +14,14 @@ public WaitUntilAprilTagIsWithinXMetersCommand(int apriltagId, double meters, Ch
                 Constants.CAMERA_HEIGHT_METERS,
                 Constants.TARGET_HEIGHT_METERS,
                 Constants.CAMERA_PITCH_RADIANS,
-                camera.getTargetById(apriltagId).getPitch()
-        ) < meters);
+                camera.getTargetById(apriltagId).getPitch()) < meters
+        );
+
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        System.out.println("ENDED!");
+        super.end(interrupted);
     }
 }

src/main/java/org/team1540/elmo/utils/ChickenPhotonCamera.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.networktables.NetworkTableInstance;
 import org.photonvision.PhotonCamera;
+import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
@@ -11,11 +12,18 @@ public class ChickenPhotonCamera extends PhotonCamera {
 
     public ChickenPhotonCamera(String cameraName) {
         super(cameraName);
+        this.setLED(VisionLEDMode.kBlink);
+        this.setDriverMode(true);
     }
 
     public PhotonTrackedTarget getTargetById(int id) {
+//        this.takeInputSnapshot();
         PhotonPipelineResult result = getLatestResult();
+        var allTags = result.getTargets();
+        System.out.println(allTags.toString());
+
         Optional<PhotonTrackedTarget> target = result.getTargets().stream().filter((targ)->targ.getFiducialId() == id).findFirst();
         return target.orElseGet(()->null);
+//        return getLatestResult().getBestTarget(); // testing
     }
 }