Pos_estimation_gb

src/main/java/frc/robot/OI.java

@@ -31,26 +31,27 @@ private static class PORT {
     /**
      * Bidings on driver controls
      */
-    private static final XboxController xboxController = new XboxController(PORT.DRIVE_CONTROLLER_PORT);
+    private static final XboxController driveController = new XboxController(PORT.DRIVE_CONTROLLER_PORT);
     public static final XboxController operatorController = new XboxController(PORT.OPERATOR_CONTROLLER_PORT); // Changed to public
-
+    
     private static class DRIVER_MAP {
-    private static final int ABUTTON = XboxController.Button.kA.value;
-    private static final int BBUTTON = XboxController.Button.kB.value;
+	private static final int ABUTTON = XboxController.Button.kA.value;
+	private static final int BBUTTON = XboxController.Button.kB.value;
     }
 
     // drive sticks
-    public static DoubleSupplier xboxLeftStickXSupplier = () -> { return processDriveInput(-xboxController.getLeftX()); };
-    public static DoubleSupplier xboxLeftStickYSupplier = () -> { return processDriveInput(-xboxController.getLeftY()); };
-    public static DoubleSupplier xboxRightStickXSupplier = () -> { return processRotationInput(-xboxController.getRightX()); };
-    public static DoubleSupplier xboxRightStickYSupplier = () -> { return processRotationInput(xboxController.getRightY()); };
+    public static DoubleSupplier xboxLeftStickXSupplier = () -> { return processDriveInput(-driveController.getLeftX()); };
+    public static DoubleSupplier xboxLeftStickYSupplier = () -> { return processDriveInput(-driveController.getLeftY()); };
+    public static DoubleSupplier xboxRightStickXSupplier = () -> { return processRotationInput(-driveController.getRightX()); };
+    public static DoubleSupplier xboxRightStickYSupplier = () -> { return processRotationInput(driveController.getRightY()); };
 
     // drive buttons
-    public static final JoystickButton driveControllerA = new JoystickButton(xboxController, DRIVER_MAP.ABUTTON); 
-    public static final JoystickButton driveControllerB = new JoystickButton(xboxController, DRIVER_MAP.BBUTTON); 
-    public static final Trigger driveRightTrigger = new Trigger(() -> 
-								   xboxController.getRightTriggerAxis() > TRIGGER_THRESHOLD);
-
+    public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON); 
+    public static final JoystickButton driveControllerB = new JoystickButton(driveController, DRIVER_MAP.BBUTTON);
+    public static DoubleSupplier driveControllerRightTriggerSupplier = () -> { return driveController.getRightTriggerAxis();};
+
+
+
     private static double processDriveInput(double raw) {
         if (Math.abs(raw) < 0.1) {
             raw = 0 ;
@@ -89,9 +90,12 @@ private static class operatorBindings {
         public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON);
 
         // Trigger controls for coral claw
+    public static DoubleSupplier operatorRightTriggerSupplier= () -> { return operatorController.getRightTriggerAxis();};
         public static final Trigger operatorRightTrigger = new Trigger(() -> 
             operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD);
 
+    public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();};
+
         public static final Trigger operatorLeftTrigger = new Trigger(() -> 
             operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD);
 

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

@@ -44,7 +44,7 @@ public class RobotContainer {
   public RobotContainer() {
     // Initialize OI with a reference to this RobotContainer instance
     configureBindings();
-    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier));
+    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier));
     RobotMap.compressor.enableAnalog(70, 120);
     // set the default drive command to use the left and right stick values from the Xbox controller
     // The left stick controls the forward and backward movement of the robot, while the right stick
@@ -62,53 +62,39 @@ private void configureBindings() {
 		  drivetrain.resetPosition();
 	  }));
 
-      OI.driveRightTrigger.onTrue(new InstantCommand(() -> drivetrain.setSlow(true)));
-      OI.driveRightTrigger.onFalse(new InstantCommand(() -> drivetrain.setSlow(false)));
-
       // Operator controller bindings
       // Elevator level controls
-      // OperatorBindings.operatorControllerA.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW)));
-      // OperatorBindings.operatorControllerB.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.HIGH)));
-      // OperatorBindings.operatorControllerX.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.MED)));
-      // OperatorBindings.operatorControllerY.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.EXTRA_HIGH)));
-
       // OPERATOR B : INTAKE
       OI.operatorControllerB.onTrue(new IntakeCommandGroup(elevator, wrist, coralClaw));
       // OPERATOR RB : TROUGH(L1)
-      OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 1)); 
+      OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.LOW)); 
       // OPERATOR A : L2 (lowest pole)
-      OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 2));
+      OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.MED));
       // OPERATOR X : L3 (2nd pole)
-      OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 3));
+      OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.HIGH));
       // OPERATOR Y : L4 (last pole)
-      OI.operatorControllerY.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 4));
+      OI.operatorControllerY.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.EXTRA_HIGH));
 
-      // OPERATOR LB: WRIST TOGGLE 
+      // OPERATOR LB : WRIST TOGGLE 
       OI.operatorControllerLeftBumper.onTrue(new InstantCommand(() -> wrist.toggle()));
-
+
+      // OPERATOR START : TRIM UP
       OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.trimTarget(0.0175)));
+      // OPERATOR BACK : TRIM DOWN
       OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175)));
-
-
-
-      // Coral Claw controls with triggers
-      // OI.operatorRightTrigger.onTrue(new InstantCommand(() -> coralClaw.intake()));
-      // OI.operatorRightTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
-      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, 1));
-      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, 0));
-      // OI.operatorLeftTrigger.onTrue(new InstantCommand(() -> coralClaw.setOutput(coralClaw.OUTTAKE_SPEED)));
-      // OI.operatorLeftTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
-
-      // Reset encoder with Start button
-      // OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.resetEncoder()));
-
+
+      // OPERATOR RT : INTAKE
+      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorRightTriggerSupplier, 1));
+      // OPERATOR LT : OUTTAKE
+      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorLeftTriggerSupplier, 0));
+
       // Manual control with right stick for testing in simulation
-      OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
-          elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator));
+      // OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
+          // elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator));
   }
 
   public Command getAutonomousCommand() {
-      return new ScoreTest(drivetrain, gyro, camera);
+      return new ScoreTest(drivetrain, gyro, camera, elevator, wrist, coralClaw);
       // return new DriveDistance(drivetrain, gyro, 7.0, 0.0, 0.0).andThen(new DriveDistance(drivetrain, gyro, 0.0, 3.0, 0.0));
     // return new DriveWithHeadingCommand(drivetrain, gyro, OI.xboxLeftStickXSupplier, OI.xboxRightStickYSupplier, new Rotation2d(0.0));
     // return new DriveMeters(drivetrain, 0.0, 0.0, 0.0);

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

@@ -8,19 +8,21 @@
 import frc.robot.subsystems.Drivetrain;
 
 public class DriveCommand extends Command {
-
-  Drivetrain drivetrain;
-  DoubleSupplier xSupplier;
-  DoubleSupplier ySupplier;
-  DoubleSupplier omegaSupplier;
-
-  public DriveCommand(Drivetrain drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier,
-      DoubleSupplier omegaSupplier) {
-    addRequirements(drivetrain);
+
+    Drivetrain drivetrain;
+    DoubleSupplier xSupplier;
+    DoubleSupplier ySupplier;
+    DoubleSupplier omegaSupplier;
+    DoubleSupplier triggerSupplier;
+    /** drive robot according to the x and y of the drive controller, drive at half speed by default, increase by half the value of the drive trigger (0-1)**/
+    public DriveCommand(Drivetrain drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier,
+		      DoubleSupplier omegaSupplier, DoubleSupplier triggerSupplier) {
+      addRequirements(drivetrain);
     this.drivetrain = drivetrain;
     this.xSupplier = xSupplier;
     this.ySupplier = ySupplier;
     this.omegaSupplier = omegaSupplier;
+    this.triggerSupplier = triggerSupplier;
   }
 
   @Override
@@ -29,9 +31,8 @@ public void initialize() {
 
   @Override
   public void execute() {
-    drivetrain.drive(ySupplier.getAsDouble(), xSupplier.getAsDouble(), omegaSupplier.getAsDouble());
-    // opposite suppliers are passed into command, appear correct in
-    // DriveCommand.java
+      double scaleFactor = triggerSupplier.getAsDouble() * 0.5 + 0.5;
+      drivetrain.drive(ySupplier.getAsDouble() * scaleFactor, xSupplier.getAsDouble() * scaleFactor, omegaSupplier.getAsDouble());
   }
 
   @Override

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

@@ -1,4 +1,3 @@
-
 package frc.robot.commands;
 
 import frc.robot.subsystems.Elevator;
@@ -11,31 +10,15 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class ScoreCommandGroup extends ParallelCommandGroup {
-    public ScoreCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw, int level) {
-	switch (level) {
-	case 1:
-	    addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.LOW)), new InstantCommand(() -> wrist.extend()));
-	    break;
-	case 2:
-	    addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.MED)), new InstantCommand(() -> wrist.extend()));
-	    break;
-	case 3:
-	    addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.HIGH)), new InstantCommand(() -> wrist.extend()));
-	    break;
-	case 4:
-	    addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.EXTRA_HIGH)), new InstantCommand(() -> wrist.extend()));
-	    break;
-	default:
-	    System.out.println("SCORECOMMANDGROUP INVALID TARGET: " + level);
-	    break;
+    public ScoreCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw, ElevatorLevel level) {
+	if(level == ElevatorLevel.LOW) {
+	    addCommands(new InstantCommand(() -> claw.setMotorsOn(false, true)));
+	} else {
+	    addCommands(new InstantCommand(() -> claw.setMotorsOn(true, true)));
 	}
-
-
-
-
-    // 	addCommands(
-    // 		    new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION)),
-    // 		    new InstantCommand(() -> wrist.retract())
-    // 		    );
+        addCommands(
+            new InstantCommand(() -> elevator.setTarget(level)),
+            new InstantCommand(() -> wrist.extend())
+        );
     }
 }

src/main/java/frc/robot/commands/auto/DriveDistance.java

@@ -21,21 +21,23 @@ public class DriveDistance extends Command {
 	double angleDisplacement;
 
 	double xyThreshold = 0.6;
-	double xyMaxSpeed = 0.65;
+	double xyMaxSpeed;
 	double rotateMaxSpeed = 1.5;
 
-	public DriveDistance(Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) {
+    public DriveDistance(double xyMaxSpeed, Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) {
 		this.drivetrain = drivetrain;
 		this.gyro = gyro;
 		this.xDistance = xDistance;
 		this.yDistance = yDistance;
 		this.theta = theta;
+		this.xyMaxSpeed = xyMaxSpeed;
 		addRequirements(drivetrain);
 
 	}
 
 	@Override
 	public void initialize() {
+	    System.out.println("drivedistance initialized");
 		Pose2d start = drivetrain.getPose2d();
 		this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta));
 	}
@@ -84,7 +86,7 @@ public void execute() {
 
 	@Override
 	public void end(boolean interrupted) {
-		drivetrain.driveRobotRelative(0.0, 0.0, 0.0);
+		drivetrain.drive(0.0, 0.0, 0.0);
 	}
 
     @Override

src/main/java/frc/robot/commands/auto/ScoreTest.java

@@ -2,9 +2,17 @@
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Gyro;
 import frc.robot.subsystems.Camera;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Wrist;
+import frc.robot.subsystems.CoralClaw;
 import frc.robot.commands.auto.DriveDistance;
+import frc.robot.commands.IntakeCommandGroup;
+import frc.robot.commands.ScoreCommandGroup;
+import frc.robot.ElevatorLevel;
+
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 /**
  * The ScoreTest command is a SequentialCommandGroup that performs a series of driving maneuvers
@@ -37,14 +45,52 @@ public class ScoreTest extends SequentialCommandGroup {
 	 * @param gyro The gyro sensor used for orientation and heading.
 	 * @param camera The camera used for vision processing (currently commented out).
 	 */
-    public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera) {
-	// ID7 from outer
+    public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, CoralClaw claw) {
 	addCommands(
 		    new InstantCommand(() -> drivetrain.resetPosition()),
-		    new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall
-		    new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7
-		    new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command
-		    new DriveTowardsThing(drivetrain, camera)
+		    new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472),
+			new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.1, 1.0472),
+			new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW),
+			new WaitCommand(0.5),
+			new InstantCommand(() -> claw.setOutput(-0.4)),
+			new WaitCommand(1.0),
+		    new InstantCommand(() -> claw.setOutput(0.0))
 		    );
+
+	// SIDE 2
+	// addCommands(
+		    // new InstantCommand(() -> drivetrain.resetPosition()),
+		    // new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599),
+		    // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.1, 5.23599),
+		    // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW),
+		    // new WaitCommand(500),
+		    // new InstantCommand(() -> claw.setOutput(-0.4)),
+		    // new WaitCommand(1000.0),
+		    // new InstantCommand(() -> claw.setOutput(0.0))
+		    // );
+
+
+		    // new InstantCommand(() -> drivetrain.driveRobotRelative(0.25, 0.0, 0.0))
+
+	// SIDE 1 L1
+	// addCommands(
+		    // new InstantCommand(() -> drivetrain.resetPosition()),
+		    // new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall
+		    // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW),
+		    // new WaitCommand(500.0),
+		    // new InstantCommand(() -> claw.setOutput(-0.4)),
+		    // new WaitCommand(1000.0),
+		    // new InstantCommand(() -> claw.setOutput(0.0))
+		    // );
+
+
+	// ID7 from outer
+	// addCommands(
+		    // new InstantCommand(() -> drivetrain.resetPosition()),
+		    // new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall
+		    // new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7
+		    // new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command
+		    // new DriveTowardsThing(drivetrain, camera)
+		    // );
     }
 }

src/main/java/frc/robot/commands/instant/ClawCommand.java

@@ -3,17 +3,20 @@
 import frc.robot.subsystems.CoralClaw;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.DoubleSupplier;
 
 public class ClawCommand extends Command {
     CoralClaw claw;
     double speed;
     int outin;
+    DoubleSupplier triggerSupplier;
 
-    public ClawCommand(CoralClaw claw, int outin) {
+    public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) {
 	addRequirements(claw);
 	this.claw = claw;
 	this.speed = claw.INTAKE_SPEED;
 	this.outin = outin;
+	this.triggerSupplier = triggerSupplier;
     }
 
     @Override
@@ -22,13 +25,9 @@ public ClawCommand(CoralClaw claw, int outin) {
     @Override
     public void execute() {
 	if(outin == 0) { // OUT
-	    System.out.println("CLAWCOMMAND OUTTAKE");
-	    claw.outtake();
+	    claw.setOutput(-(triggerSupplier.getAsDouble() * 0.5 + 0.2));
 	} else { // INTAKE
-	    System.out.println("CLAWCOMMAND INTAKE");
-	    claw.intake();
-	    // if(!claw.isCoralDetected()) {
-	// }
+	    claw.setOutput(triggerSupplier.getAsDouble() * 0.5 + 0.2);
 	}