Auto when lined up with apriltag at start

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

@@ -40,4 +40,16 @@ public final class Constants {
     public static final int DEPLOY_INNER_CHANNEL = 6;
     // COMPRESSOR
     public static final int COMPRESSOR = 0;
+
+    // INPUT
+    public static final int NAVX_PORT = 0;
+
+    // APRILTAGS
+    public static final int TOWER_APRILTAG_ID = -1;
+    public static final String CAMERA_NAME = "";
+
+    // SELF MEASUREMENTS
+    public static final double CAMERA_HEIGHT_METERS = 2;
+    public static final double CAMERA_PITCH_RADIANS = 0; // from the horizontal plane >0=up
+    public static final double TARGET_HEIGHT_METERS = 2;
 }

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

@@ -11,17 +11,15 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
-import java.util.concurrent.atomic.AtomicBoolean;
-
-import org.team1540.elmo.subsystems.drivetrain.ArcadeDrive;
+import org.team1540.elmo.subsystems.drivetrain.Auto;
+import org.team1540.elmo.subsystems.drivetrain.DriveForwardAndTurnCommand;
 import org.team1540.elmo.subsystems.drivetrain.DriveTrain;
 import org.team1540.elmo.subsystems.drivetrain.TankDrive;
 import org.team1540.elmo.subsystems.ejectors.*;
+import org.team1540.elmo.utils.ChickenPhotonCamera;
 
 
 /**
@@ -40,6 +38,7 @@ public class RobotContainer
 
     // Misc Components
     private final Compressor compressor = new Compressor(Constants.COMPRESSOR, PneumaticsModuleType.CTREPCM);
+    private final ChickenPhotonCamera camera = new ChickenPhotonCamera(Constants.CAMERA_NAME);
 
     // Control Devices
     private XboxController driver = new XboxController(Constants.DRIVER_CONTROLLER_PORT);
@@ -68,6 +67,8 @@ private void configureButtonBindings()
         Trigger eitherBumper = new JoystickButton(driver,XboxController.Button.kLeftBumper.value).or(new JoystickButton(driver,XboxController.Button.kRightBumper.value));
         Trigger neitherBumper = eitherBumper.negate();
 
+
+        //////////////////// SUCTION /////////////////////
         // left side
         new JoystickButton(driver,XboxController.Button.kX.value).and(neitherBumper).whenActive(
                 new EjectInnerCommand(innerEjector)
@@ -92,9 +93,27 @@ private void configureButtonBindings()
             new ResetEjectorAndWaitForeverCommand(upperEjector,true)
         );
 
+        //////////////////// AUTO DRIVE /////////////////////
+        Trigger bothBumpers = new JoystickButton(driver,XboxController.Button.kLeftBumper.value).and(new JoystickButton(driver,XboxController.Button.kRightBumper.value));
+        Trigger leftJoy = new JoystickButton(driver,XboxController.Button.kLeftStick.value);
+        Trigger rightJoy = new JoystickButton(driver,XboxController.Button.kRightStick.value);
+        Trigger bothJoys = leftJoy.and(rightJoy);
+        Trigger eitherJoy = leftJoy.or(rightJoy);
+        // reset gyro
+        bothBumpers.whenActive(
+            new InstantCommand(driveTrain::resetGyro)
+        );
+        rightJoy.and(bothJoys.negate()).whileActiveContinuous(
+            new DriveForwardAndTurnCommand(45, .5, .3, driveTrain)
+        );
+        leftJoy.and(bothJoys.negate()).whileActiveContinuous(
+            new DriveForwardAndTurnCommand(0, .5, .3, driveTrain)
+        );
+
         driveTrain.setDefaultCommand(new TankDrive(driveTrain, driver));
         // driveTrain.setDefaultCommand(new ArcadeDrive(driveTrain, driver));
 
+
         // Add button to command mappings here.
         // See https://docs.wpilib.org/en/stable/docs/software/commandbased/binding-commands-to-triggers.html
     }
@@ -108,6 +127,6 @@ private void configureButtonBindings()
     public Command getAutonomousCommand()
     {
         // An ExampleCommand will run in autonomous
-        return null;
+        return new Auto(driveTrain,upperEjector,outerEjector,innerEjector,camera);
     }
 }

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

@@ -20,19 +20,8 @@ public ArcadeDrive(DriveTrain driveTrain, XboxController driver) {
     public void execute() {
         double spinPercent = driver.getRightX();
         double forwardPercent = driver.getLeftY();
-
 
-        double leftPercent = -spinPercent + forwardPercent;
-        double rightPercent = spinPercent + forwardPercent;
-
-        // Any calculated output is greater than 1, scale both coefficients down so that the max is 1
-        double maxScalarPercent = Math.max(Math.abs(leftPercent),Math.abs(rightPercent));
-        if(maxScalarPercent > 1) {
-            double coefficient = 1 / maxScalarPercent;
-            leftPercent *= coefficient;
-            rightPercent *= coefficient;
-        }
-
-        driveTrain.setPercent(leftPercent, rightPercent);
+        // all shit handled in drivetrain functions
+        driveTrain.setSpeedAndSpinRamped(forwardPercent, spinPercent);
     }
 }

src/main/java/org/team1540/elmo/subsystems/drivetrain/Auto.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 Auto extends SequentialCommandGroup {
+    public Auto(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/DriveForwardAndTurnCommand.java

@@ -0,0 +1,31 @@
+package org.team1540.elmo.subsystems.drivetrain;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+public class DriveForwardAndTurnCommand extends PIDCommand {
+    public DriveForwardAndTurnCommand(double setPoint, double rotationSpeed, double driveSpeed, DriveTrain driveTrain) {
+        super(new PIDController(rotationSpeed, rotationSpeed/7, 0),
+            ()->driveTrain.getAngle()%360,
+            ()->setPoint,
+            (double output)->{ // output from -180 to 180
+                // put output to smartdashboard
+                SmartDashboard.putNumber("DriveForwardAndTurn/PID-Output", output);
+
+                // void useOutput(double output) {
+                output=output/180;
+                double leftOutput = driveSpeed;
+                double rightOutput = driveSpeed;
+                if(output > 0) {
+                    rightOutput -= output;
+                } else {
+                    leftOutput -= output;
+                }
+                driveTrain.setPercent(leftOutput, rightOutput);
+                // }
+            },
+            driveTrain);
+            this.m_controller.enableContinuousInput(0, 360);
+    }
+}

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

@@ -0,0 +1,19 @@
+package org.team1540.elmo.subsystems.drivetrain;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveForwardRampedForeverCommand extends CommandBase {
+    private DriveTrain driveTrain;
+    private double motorPercent;
+
+    public DriveForwardRampedForeverCommand(double motorPercent, DriveTrain driveTrain) {
+        addRequirements(driveTrain);
+        this.driveTrain = driveTrain;
+        this.motorPercent = motorPercent;
+    }
+
+    @Override
+    public void execute() {
+        driveTrain.setPercentRamped(motorPercent,motorPercent);
+    }
+}

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

@@ -0,0 +1,34 @@
+package org.team1540.elmo.subsystems.drivetrain;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.team1540.elmo.utils.ChickenPhotonCamera;
+
+public class DriveToAprilTagCommand extends CommandBase {
+
+    private int tagId;
+    private double driveSpeedPercent;
+    private double turnSpeedMaxPercent;
+    private DriveTrain driveTrain;
+    private ChickenPhotonCamera camera;
+
+    public DriveToAprilTagCommand(int tagId, double driveSpeedPercent, double turnSpeedFactor, DriveTrain driveTrain, ChickenPhotonCamera camera) {
+        this.tagId = tagId;
+        this.driveSpeedPercent = driveSpeedPercent;
+        this.turnSpeedMaxPercent = turnSpeedFactor;
+        this.driveTrain = driveTrain;
+        this.camera = camera;
+    }
+
+    @Override
+    public void execute() {
+        PhotonTrackedTarget target = camera.getTargetById(tagId);
+        if(target==null) return;
+
+        final double yaw = target.getYaw(); // in degrees
+        double turnSpeedPercent = yaw/180*turnSpeedMaxPercent;
+
+        // this function handles ramping and shit
+        driveTrain.setSpeedAndSpinRamped(driveSpeedPercent, turnSpeedPercent);
+    }
+}

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

@@ -0,0 +1,38 @@
+package org.team1540.elmo.subsystems.drivetrain;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.team1540.elmo.utils.ChickenPhotonCamera;
+
+public class DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand extends CommandBase {
+
+    private int tagId;
+    private double driveSpeedPercent;
+    private double turnSpeedMaxPercent;
+    private DriveTrain driveTrain;
+    private ChickenPhotonCamera camera;
+
+    public DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand(int tagId, double driveSpeedPercent, double turnSpeedMaxPercent, DriveTrain driveTrain, ChickenPhotonCamera camera) {
+        this.tagId = tagId;
+        this.driveSpeedPercent = driveSpeedPercent;
+        this.turnSpeedMaxPercent = turnSpeedMaxPercent;
+        this.driveTrain = driveTrain;
+        this.camera = camera;
+    }
+
+    @Override
+    public void execute() {
+        PhotonTrackedTarget target = camera.getTargetById(tagId);
+        if(target==null) return;
+
+        final double yaw = target.getYaw(); // in degrees
+        double turnSpeedPercent = yaw/180*turnSpeedMaxPercent;
+
+        double leftOutput = driveSpeedPercent;
+        double rightOutput = driveSpeedPercent;
+        leftOutput += turnSpeedPercent/2d;
+        rightOutput -= turnSpeedPercent/2d;
+
+        driveTrain.setPercent(leftOutput, rightOutput);
+    }
+}