Auto dev test0324

src/main/java/frc/robot/Constants.java

@@ -182,6 +182,11 @@ public static class AlgaeSubsystemConstants {
   public static class ElevatorSubsystemConstants {
     public static final int ELEVATOR_LEFT_MOTOR_ID = 11;
     public static final int ELEVATOR_RIGHT_MOTOR_ID = 12;
+
+    public static final double kSVolts = 0.11356;
+    public static final double kGVolts = 0.29175;
+    public static final double kVVoltSecondPerRad = 1.5928;
+    public static final double kAVoltSecondSquaredPerRad = 0.030171;
   }
 
   public static class ClimberSubsystemConstants {
@@ -230,19 +235,19 @@ public static class VisionConstants {
     public static final ArrayList<Integer> blueHPTags = new ArrayList<>(Arrays.asList(13, 12));
     public static ArrayList<Integer> HPTags = new ArrayList<>();
 
-    // Standard deviations below are from Team Spectrum 3847’s X-Ray robot
+    // Standard deviations below are from Team Spectrum 3847s X-Ray robot
 
     /**
      * Standard deviations of model states. Increase these numbers to trust your model's state
-     * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians,
+     * estimates less. This matrix is in the form [x, y, theta], with units in meters and radians,
      * then meters.
      */
     public static final Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1, 0.1, 10);
 
     /**
      * Standard deviations of the vision measurements. Increase these numbers to trust global
-     * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in
-     * meters and radians.
+     * measurements from vision less. This matrix is in the form [x, y, theta], with units in meters
+     * and radians.
      */
     public static final Matrix<N3, N1> measurementStdDevs = VecBuilder.fill(5, 5, 500);
 

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

@@ -150,6 +150,7 @@ public void disabledInit() {
     RobotContainer.elevatorSubsystem.stop();
     RobotContainer.coralHoldAngleSubsystem.stopTilt();
     // RobotContainer.climberSubsystem.stop();
+
     // centerModules.ignoringDisable(true);
   }
 
@@ -217,6 +218,10 @@ public void autonomousPeriodic() {
       // have flex create points to follow
       flexAutoSubsystem.CreatePath(constraints, m_CoralStationChooser.getSelected());
     }
+    // Will constantly get the position of the elevator,
+    // so that when we go into tele, we have an offset to
+    // adjust to
+    RobotContainer.getStart();
   }
 
   @Override
@@ -234,6 +239,11 @@ public void teleopInit() {
     } else {
       CommandScheduler.getInstance().cancelAll();
     }
+    // This is for testing- if we already have an offset,
+    // this will do nothing
+    if (!RobotContainer.gottenStart()) {
+      RobotContainer.getStart();
+    }
   }
 
   /** This function is called periodically during operator control. */

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

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -28,6 +29,8 @@
 import frc.robot.commands.AlgaeIntakeCommand;
 import frc.robot.commands.AlgaeShootCommand;
 import frc.robot.commands.AlignWithNearest;
+import frc.robot.commands.ArmLevel2;
+import frc.robot.commands.ArmLevel3;
 import frc.robot.commands.CoralIntakeCommand;
 import frc.robot.commands.CoralShootCommand;
 import frc.robot.commands.ElevatorDown;
@@ -45,7 +48,6 @@
 import frc.robot.commands.algaeStopIntake;
 import frc.robot.commands.stopCoralIntake;
 import frc.robot.subsystems.AlgaeSubsystem;
-// import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.CoralHoldAngleSubsystem;
 import frc.robot.subsystems.CoralHoldSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
@@ -56,6 +58,10 @@
 import java.util.Optional;
 import swervelib.SwerveInputStream;
 
+// import frc.robot.subsystems.KeyboardInput;
+
+// import java.util.Scanner;
+
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -66,8 +72,11 @@
 public class RobotContainer {
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
+
   public static final CommandXboxController driverXbox = new CommandXboxController(0);
   final CommandGenericHID genericHID = new CommandGenericHID(1);
+  final CommandXboxController coralController = new CommandXboxController(1);
+  // final CommandGenericHID genericHID = new CommandGenericHID(1);
   // The robot's subsystems and commands are defined here...
   public static final SwerveSubsystem drivebase =
       new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo"));
@@ -79,6 +88,8 @@ public class RobotContainer {
   public static PathConstraints Pathconstraints;
   public static FlexAutoSubsystem flexAutoSubsystem;
 
+  // private final KeyboardInput keyboard;
+
   // public Command repeatWristDown = new RepeatCommand(new WristDown(coralHoldAngleSubsystem));
 
   // public Command repeatWristDown = new RepeatCommand(new WristDown(coralHoldAngleSubsystem));
@@ -135,6 +146,7 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    // keyboard = new KeyboardInput();
     configureBindings();
     flexAutoSubsystem = new FlexAutoSubsystem();
     Pathconstraints =
@@ -179,6 +191,33 @@ public void Periodic() {
     m_elevator.setLength(elevatorSubsystem.getPositionEncoder());
     m_wrist.setAngle(coralHoldAngleSubsystem.getEncoder());
     // m_wrist2.setAngle(climberSubsystem.getEncoder());
+    // m_wrist2.setAngle(climberSubsystem.getEncoder());
+
+    // String key = keyboard.getLastKeyPressed();
+
+    /*if (key.equalsIgnoreCase("X")) {
+      new SequentialCommandGroup(
+          new ParallelCommandGroup(
+              new ElevatorMoveLevel2(elevatorSubsystem), new ArmLevel2(coralHoldAngleSubsystem)),
+          new WaitCommand(1),
+          new CoralShootCommand(coralHoldSubsystem));
+    } else if (key.equalsIgnoreCase("S")) {
+      new SequentialCommandGroup(
+          new ParallelCommandGroup(
+              new ElevatorMoveLevel3(elevatorSubsystem), new ArmLevel3(coralHoldAngleSubsystem)),
+          new WaitCommand(1),
+          new CoralShootCommand(coralHoldSubsystem));
+    } else if (key.equalsIgnoreCase("W")) {
+      new SequentialCommandGroup(
+          new ParallelCommandGroup(
+              new ElevatorMoveLevel4(elevatorSubsystem), new ArmLevel4(coralHoldAngleSubsystem)),
+          new WaitCommand(1),
+          new CoralShootCommand(coralHoldSubsystem));
+    }*/
+
+    // m_wrist.setAngle(coralHoldAngleSubsystem.getEncoder());
+    // m_wrist2.setAngle(climberSubsystem.getEncoder());
+
   }
 
   /**
@@ -204,6 +243,48 @@ private void configureBindings() {
         drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboard);
 
     drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity);
+
+    coralController
+        .y()
+        .onTrue(
+            new ParallelCommandGroup(
+                new ElevatorMoveLevel3(elevatorSubsystem), new ArmLevel3(coralHoldAngleSubsystem)));
+
+    // DO NOT TOUCH
+    // NEEDS WORK
+    // KATHERINE I WILL KILL YOU
+    // OR AIDEN
+    // I WILL STAB
+
+    // coralController.povDown().onTrue(new ArmLevel2(coralHoldAngleSubsystem));
+    coralController
+        .b()
+        .onTrue(
+            new ParallelCommandGroup(
+                new ElevatorMoveLevel2(elevatorSubsystem), new ArmLevel2(coralHoldAngleSubsystem)));
+
+    // coralController
+    //     .b()
+    //     .onTrue(
+    //         new SequentialCommandGroup(
+    //             new ParallelCommandGroup(
+    //                 new ElevatorIntake(elevatorSubsystem), new
+    // ArmIntake(coralHoldAngleSubsystem)),
+    //             new WaitCommand(1),
+    //             new CoralIntakeCommand(coralHoldSubsystem)));
+    // // driverXbox.x().whileTrue(new AlignWithNearest());
+    /*driverXbox
+    .x()
+    .onTrue(
+      new SequentialCommandGroup(
+        new ParallelCommandGroup(
+          new ElevatorMoveLevel2(elevatorSubsystem),
+          new ArmLevel2(coralHoldAngleSubsystem)
+        ),
+        new WaitCommand(1),
+        new CoralShootCommand(coralHoldSubsystem)
+      )
+      );*/
     driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
     driverXbox.a().whileTrue(drivebase.centerModulesCommand());
 
@@ -213,11 +294,11 @@ private void configureBindings() {
     driverXbox.y().whileTrue(new AlgaeIntakeCommand(algaeSubsystem));
     // driverXbox.y().onFalse(new algaeStopIntake(algaeSubsystem));
 
-    driverXbox.leftBumper().onTrue(new ElevatorMoveLevel3(elevatorSubsystem));
+    // driverXbox.leftBumper().onTrue(new ElevatorMoveLevel3(elevatorSubsystem));
     driverXbox.rightTrigger().onFalse(new ElevatorStop(elevatorSubsystem));
     driverXbox.rightBumper().onTrue(new ElevatorPrevPosition(elevatorSubsystem));
     driverXbox.leftTrigger().onFalse(new ElevatorStop(elevatorSubsystem));
-    driverXbox.leftTrigger().whileTrue(new ElevatorUp(elevatorSubsystem, 0.65));
+    driverXbox.leftTrigger().whileTrue(new ElevatorUp(elevatorSubsystem, 0.64));
     driverXbox.rightTrigger().onTrue(new ElevatorDown(elevatorSubsystem, 0.1));
     drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
     driverXbox.povUp().onTrue(new WristUp(coralHoldAngleSubsystem));
@@ -248,6 +329,22 @@ public Command getAutonomousCommand(String pathString) {
     return drivebase.getAutonomousCommand(pathString);
   }
 
+  // On the disablement of auto, or the start of tele if we
+  // don't do auto, this will get the starting position of the elevator.
+  // Basically- through out auto, this will keep the elevators position.
+  // When it moves to tele, that position is now it's zero.
+  // The means we now know our offset.
+  public static void getStart() {
+
+    elevatorSubsystem.getStartPos();
+    coralHoldAngleSubsystem.setOGPos();
+    elevatorSubsystem.setOgPOSgotten();
+  }
+
+  public static boolean gottenStart() {
+    return elevatorSubsystem.getOgPOSgotten();
+  }
+
   public Command getCoralPathCommand(String chooser) {
     Optional<Alliance> ally = DriverStation.getAlliance();
     if (ally.get() == Alliance.Blue) {

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

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CoralHoldAngleSubsystem;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+public class ArmLevel2 extends Command {
+  private CoralHoldAngleSubsystem coralHold;
+
+  public ArmLevel2(CoralHoldAngleSubsystem c_Subsystem) {
+    coralHold = c_Subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(c_Subsystem);
+  }
+
+  public void execute() {
+    coralHold.setLevel(2);
+
+    if (coralHold.atLevel(2)) {
+
+      // Logger.getGlobal().log(Level.INFO, "found level");
+      coralHold.stopTilt();
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (coralHold.atLevel(2)) {
+      Logger.getGlobal().log(Level.INFO, "found level");
+      coralHold.stopTilt();
+      return true;
+    }
+    return false;
+  }
+}

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

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CoralHoldAngleSubsystem;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+public class ArmLevel3 extends Command {
+  private CoralHoldAngleSubsystem coralHold;
+
+  public ArmLevel3(CoralHoldAngleSubsystem c_Subsystem) {
+    coralHold = c_Subsystem;
+    addRequirements(c_Subsystem);
+  }
+
+  @Override
+  public void execute() {
+    coralHold.setLevel(3);
+
+    if (coralHold.atLevel(3)) {
+
+      // Logger.getGlobal().log(Level.INFO, "found level");
+      coralHold.stopTilt();
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (coralHold.atLevel(3)) {
+      Logger.getGlobal().log(Level.INFO, "found level");
+      coralHold.stopTilt();
+      return true;
+    }
+    return false;
+  }
+}

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

@@ -0,0 +1,23 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CoralHoldAngleSubsystem;
+
+public class ArmLevel4 extends Command {
+  private CoralHoldAngleSubsystem coralHold;
+
+  public ArmLevel4(CoralHoldAngleSubsystem c_Subsystem) {
+    coralHold = c_Subsystem;
+    addRequirements(c_Subsystem);
+  }
+
+  @Override
+  public void initialize() {
+    // coralHold.setLevel();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return coralHold.isFinished(4);
+  }
+}

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

@@ -35,7 +35,8 @@ public void execute() {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    // Should come back to this
-    return true;
+    // If Milo needs this changed, they can
+
+    return m_Coral.getTrigger();
   }
 }

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

@@ -2,6 +2,8 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.ElevatorSubsystem;
+import java.util.logging.Level;
+import java.util.logging.Logger;
 
 /** An example command that uses an example subsystem. */
 public class ElevatorMoveLevel2 extends Command {
@@ -19,12 +21,23 @@ public ElevatorMoveLevel2(ElevatorSubsystem subsystem) {
   }
 
   @Override
-  public void initialize() {
-    m_subsystem.setLevel(2);
+  public void execute() {
+    m_subsystem.goToCoralLevel(2);
+
+    if (m_subsystem.atLevel(2)) {
+
+      // Logger.getGlobal().log(Level.INFO, "found level");
+      m_subsystem.stop();
+    }
   }
 
   @Override
   public boolean isFinished() {
-    return m_subsystem.isFinished(2);
+    if (m_subsystem.atLevel(2)) {
+      Logger.getGlobal().log(Level.INFO, "found level");
+      m_subsystem.stop();
+      return true;
+    }
+    return false;
   }
 }