Skip to content

Commit

Permalink
Merge pull request #88 from IronRiders/IshanTestsBSCode3
Browse files Browse the repository at this point in the history
  • Loading branch information
@ryanelbert5125gmail
ryanelbert5125gmail authored Mar 8, 2025
2 parents 6927b47 + 5f32e90 commit db74c25
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 133 deletions.
52 changes: 52 additions & 0 deletions src/main/java/org/ironriders/core/ControllerConstants.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
package org.ironriders.core;
public final class ControllerConstants {

// Primary (Driver) Controller
public static final int driveTranslateY = 0; // Left Stick Y-axis
public static final int driveTranslateX = 1; // Left Stick X-axis
public static final int driveRotateX = 4; // Right Stick X-axis

public static final int cancelPathfindX = 2; // X Button
public static final int pathfindToTargetY = 3; // Y Button

//public static final int scoreAlgae = 6; // Right Bumper
//public static final int scoreCoral = 5; // Left Bumper



public static final int jogControlDpad = 9; // D-Pad (Handled as POV)

// Secondary (Operator) Controller


public static final int scoreCoral = 0; // Trigger

public static final int prepareGrabCoralTrigger = 1; // back hat button
public static final int grabCoralReleaseTrigger = 1; // back hat button

public static final int controlAlgaeAxis = 1; // Y Axis?
//public static final int grabAlgaeTrigger = 1; // Y axis

public static final int coralWristHome = 8;
public static final int elevatorHome = 7;

public static final int targetStation = 2;//pov left
public static final int targetProcessor = 3;//pov righ

public static final int climbUp = 13;
public static final int climbStop = 14;
public static final int climbDown = 15;

public static final int setL4 = 9;
public static final int setL3 = 7;
public static final int setL2 = 5;
public static final int setL1 = 4;

public static final int targetReefAxis = 0; // X-Axis for reef targeting

private ControllerConstants() {
// Prevent instantiation
}
}


32 changes: 7 additions & 25 deletions src/main/java/org/ironriders/core/RobotCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,16 +74,6 @@ public RobotCommands(
NamedCommands.registerCommand("Score Coral L4",
this.scoreCoral(ElevatorConstants.Level.L4));


NamedCommands.registerCommand("Prepare to Score Coral L1",
this.prepareToScoreCoral(ElevatorConstants.Level.L1));
NamedCommands.registerCommand("Prepare to Score Coral L2",
this.prepareToScoreCoral(ElevatorConstants.Level.L2));
NamedCommands.registerCommand("Prepare to Score Coral L3",
this.prepareToScoreCoral(ElevatorConstants.Level.L3));
NamedCommands.registerCommand("Prepare to Score Coral L4",
this.prepareToScoreCoral(ElevatorConstants.Level.L4));

NamedCommands.registerCommand("Climber Down", climbCommands.set(ClimbConstants.State.DOWN));
NamedCommands.registerCommand("Climber Up", climbCommands.set(ClimbConstants.State.UP));

Expand All @@ -108,7 +98,6 @@ public Command startup() {
algaeIntakeCommands.setOnSuccess(() -> rumble());
return coralWristCommands.home()
.andThen(algaeWristCommands.home())
.andThen(algaeWristCommands.set(AlgaeWristConstants.State.EXTENDED))
.andThen(elevatorCommands.home());
}

Expand Down Expand Up @@ -146,21 +135,9 @@ public Command toggleClimber() {
// TODO
}

public Command prepareToScoreCoral(ElevatorConstants.Level level) {
return Commands.sequence(
elevatorCommands.set(level),
coralWristCommands.set(switch (level) {
case L1, L2, L3 -> CoralWristConstants.State.L1toL3;
case L4 -> CoralWristConstants.State.L4;
default -> {
throw new IllegalArgumentException(
"Cannot score coral to level: " + level);
}
}));
}

public Command scoreCoral(ElevatorConstants.Level level) {
return Commands.sequence(
// Move to correct scoring level if not already there
elevatorCommands.set(level),
coralWristCommands.set(switch (level) {
case L1, L2, L3 -> CoralWristConstants.State.L1toL3;
Expand All @@ -170,20 +147,25 @@ public Command scoreCoral(ElevatorConstants.Level level) {
"Cannot score coral to level: " + level);
}
}),
// Eject
coralIntakeCommands.set(CoralIntakeConstants.State.EJECT),
// Reset positioning after scoring
Commands.parallel(
coralWristCommands.set(CoralWristConstants.State.STOWED),
elevatorCommands.set(ElevatorConstants.Level.Down)));
}

public Command prepareToGrabCoral() {
return Commands.parallel(
return Commands.sequence(
coralWristCommands.set(CoralWristConstants.State.STATION),
elevatorCommands.set(ElevatorConstants.Level.CoralStation));
}

public Command grabCoral() {
return Commands.sequence(
Commands.runOnce(() -> {
System.out.println("fpiojIOGDUY(*SDU)GGG_DSFG*DFHG)S(D*GDSG&*(G_SG&SD*(&G_SDUIYG)))");
}),
coralIntakeCommands.set(CoralIntakeConstants.State.GRAB),
coralWristCommands.set(CoralWristConstants.State.STOWED),
this.rumble(),
Expand Down
156 changes: 48 additions & 108 deletions src/main/java/org/ironriders/core/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -1,64 +1,38 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.ironriders.core;

import org.ironriders.drive.DriveCommands;
import org.ironriders.drive.DriveConstants;
import org.ironriders.drive.DriveSubsystem;
import org.ironriders.wrist.algae.AlgaeIntakeCommands;
import org.ironriders.wrist.algae.AlgaeIntakeConstants;
import org.ironriders.wrist.algae.AlgaeIntakeSubsystem;
import org.ironriders.wrist.algae.AlgaeWristCommands;
import org.ironriders.wrist.algae.AlgaeWristConstants;
import org.ironriders.wrist.algae.AlgaeWristSubsystem;
import org.ironriders.wrist.algae.AlgaeIntakeConstants.State;
import org.ironriders.climb.ClimbCommands;
import org.ironriders.climb.ClimbSubsystem;
import org.ironriders.climb.ClimbConstants;
import org.ironriders.wrist.coral.CoralIntakeCommands;
import org.ironriders.wrist.coral.CoralIntakeConstants;
import org.ironriders.wrist.coral.CoralIntakeSubsystem;
import org.ironriders.wrist.coral.CoralWristCommands;
import org.ironriders.wrist.coral.CoralWristConstants;
import org.ironriders.wrist.coral.CoralWristSubsystem;
import org.ironriders.dash.DashboardSubsystem;
import org.ironriders.elevator.ElevatorCommands;
import org.ironriders.elevator.ElevatorSubsystem;
import org.ironriders.elevator.ElevatorConstants.Level;
import org.ironriders.elevator.ElevatorConstants;
import org.ironriders.lib.GameState;
import org.ironriders.lib.RobotUtils;
import org.ironriders.lib.field.FieldElement.ElementType;
import org.ironriders.lib.field.FieldPose.Side;
import org.ironriders.targeting.TargetingCommands;
import org.ironriders.targeting.TargetingSubsystem;
import org.ironriders.elevator.ElevatorConstants;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import org.ironriders.wrist.coral.CoralIntakeConstants;
import org.ironriders.vision.Vision;
import org.photonvision.PhotonCamera;

import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
//import edu.wpi.*;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;

/**
* 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}
* periodic methods (other than the scheduler calls). Instead, the structure of
* the robot (including
* subsystems, commands, and trigger mappings) should be declared here.
*/

public class RobotContainer {

// The robot's subsystems and commands are defined here...
Expand Down Expand Up @@ -92,8 +66,6 @@ public class RobotContainer {
private final CommandXboxController primaryController = new CommandXboxController(
DriveConstants.PRIMARY_CONTROLLER_PORT);
private final CommandGenericHID secondaryController = new CommandGenericHID(DriveConstants.KEYPAD_CONTROLLER_PORT);
private final CommandXboxController tertiaryController = new CommandXboxController(
DriveConstants.TERTIARY_CONTROLLER_PORT);

public final RobotCommands robotCommands = new RobotCommands(
driveCommands, targetingCommands, elevatorCommands,
Expand All @@ -114,22 +86,11 @@ public RobotContainer() {
}

/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary
* predicate, or via the named factories in {@link
* edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for
* {@link
* CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
* PS4} controllers or
* {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
* joysticks}.
* Use this method to define your trigger->command mappings.
*/
private void configureBindings() {

// DRIVE CONTROLS
// DRIVE CONTROLS (Primary Controller)
driveSubsystem.setDefaultCommand(
robotCommands.driveTeleop(
() -> RobotUtils.controlCurve(
Expand All @@ -145,90 +106,69 @@ private void configureBindings() {
DriveConstants.ROTATION_CONTROL_EXPONENT,
DriveConstants.ROTATION_CONTROL_DEADBAND)));

primaryController.axisMagnitudeGreaterThan(
0, DriveConstants.PATHFIND_CANCEL_THRESHOLD).onTrue(driveCommands.cancelPathfind());
primaryController.axisMagnitudeGreaterThan(
1, DriveConstants.PATHFIND_CANCEL_THRESHOLD).onTrue(driveCommands.cancelPathfind());

// SECONDARY CONTROLS
primaryController.x().onTrue(driveCommands.cancelPathfind());
primaryController.y().onTrue(driveCommands.pathfindToTarget());

// 1/2 - Coral Home, 3/4 - Elevator Home
secondaryController.button(1).onTrue(coralWristCommands.home());
secondaryController.button(3).onTrue(elevatorCommands.home());
// SECONDARY CONTROLLER (Operator)
secondaryController.button(ControllerConstants.coralWristHome).onTrue(coralWristCommands.home());
secondaryController.button(ControllerConstants.elevatorHome).onTrue(elevatorCommands.home());

// 5/6 - Target Station, 7/8 - Target Processor
secondaryController.button(5).onTrue(targetingCommands.targetNearest(ElementType.STATION));
secondaryController.button(7).onTrue(targetingCommands.targetNearest(ElementType.PROCESSOR));
secondaryController.button(ControllerConstants.targetStation).onTrue(targetingCommands.targetNearest(ElementType.STATION));
secondaryController.button(ControllerConstants.targetProcessor).onTrue(targetingCommands.targetNearest(ElementType.PROCESSOR));

// 11/12 - Climb Up, 13/14 - Climb Reset
secondaryController.button(11).onTrue(climbCommands.set(ClimbConstants.State.UP))
secondaryController.button(ControllerConstants.climbUp).onTrue(climbCommands.set(ClimbConstants.State.UP))
.onFalse(climbCommands.set(ClimbConstants.State.STOP));
secondaryController.button(15).onTrue(climbCommands.set(ClimbConstants.State.DOWN))
secondaryController.button(ControllerConstants.climbDown).onTrue(climbCommands.set(ClimbConstants.State.DOWN))
.onFalse(climbCommands.set(ClimbConstants.State.STOP));

// 9/10 - L4, 13/14 - L3 & AH, 17/18 - L2 & AL, 21/22 - L1
secondaryController.button(9).onTrue(
Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L4);
}));
secondaryController.button(13).onTrue(
Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L3);
GameState.setAlgaeTarget(ElevatorConstants.Level.L3);
}));
secondaryController.button(17).onTrue(
Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L2);
GameState.setAlgaeTarget(ElevatorConstants.Level.L2);
}));
secondaryController.button(21).onTrue(
Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L1);
}));

// 23 - Coral Left, 24 - Coral Right
secondaryController.button(23).onTrue(targetingCommands.targetReefPole(Side.Left));
secondaryController.button(24).onTrue(targetingCommands.targetReefPole(Side.Right));

// 19 - Eject Coral, 20 - Eject Algae
secondaryController.button(19).onTrue(coralIntakeCommands.set(CoralIntakeConstants.State.EJECT));
secondaryController.button(20).onTrue(algaeIntakeCommands.set(State.EJECT));

// PRIMARY CONTROLS
primaryController.rightBumper().onFalse(robotCommands.scoreAlgae());

primaryController.leftBumper().onTrue(Commands.runOnce(() -> {
Commands.deferredProxy(() -> {
return robotCommands.grabAlgae(GameState.getAlgaeTarget());
});
// Elevator Levels
secondaryController.button(ControllerConstants.setL4).onTrue(Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L4);
}));
secondaryController.button(ControllerConstants.setL3).onTrue(Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L3);
}));
secondaryController.button(ControllerConstants.setL2).onTrue(Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L2);
}));
secondaryController.button(ControllerConstants.setL1).onTrue(Commands.runOnce(() -> {
GameState.setCoralTarget(ElevatorConstants.Level.L1);
}));

primaryController.leftTrigger().onTrue(robotCommands.prepareToGrabCoral());
primaryController.leftTrigger().onFalse(robotCommands.grabCoral());
// Targeting Reef Poles (X-Axis on Secondary Controller)
secondaryController.axisGreaterThan(ControllerConstants.targetReefAxis, 0.5)
.onTrue(targetingCommands.targetReefPole(Side.Right));

primaryController.a().onTrue(driveCommands.pathfindToTarget());
primaryController.x().onTrue(driveCommands.cancelPathfind());
secondaryController.axisLessThan(ControllerConstants.targetReefAxis, -0.5)
.onTrue(targetingCommands.targetReefPole(Side.Left));

// **Moving Intaking & Scoring to Secondary Controller**

secondaryController.button(ControllerConstants.scoreCoral).onTrue(robotCommands.scoreCoral(GameState.getCoralTarget()));

// **Moving Intaking & Scoring to Secondary Controller (Using Y-Axis)**
secondaryController.axisLessThan(ControllerConstants.controlAlgaeAxis, -0.5).onTrue(robotCommands.scoreAlgae());

primaryController.rightTrigger().onTrue(Commands.runOnce(() -> {
robotCommands.scoreCoral(GameState.getCoralTarget()).schedule();
secondaryController.axisGreaterThan(ControllerConstants.controlAlgaeAxis, 0.5).onTrue(Commands.runOnce(() -> {
Commands.deferredProxy(() -> {
return robotCommands.grabAlgae(GameState.getAlgaeTarget());
});
}));

primaryController.y().onTrue(driveCommands.pathfindToTarget());

// Configure dpad as jog control. wpilib exposes dpad as goofy "pov" values
// which are an angle; we create a trigger for each discrete 45-degree angle
for (var angle = 0; angle < 360; angle += 45) {
primaryController.pov(angle).onTrue(driveCommands.jog(-angle));
}
secondaryController.axisGreaterThan(ControllerConstants.prepareGrabCoralTrigger, 0.5)
.onTrue(robotCommands.prepareToGrabCoral());

secondaryController.axisLessThan(ControllerConstants.prepareGrabCoralTrigger, -0.5)
.onTrue(robotCommands.grabCoral()); // Runs when trigger is released


}

/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
}
}

0 comments on commit db74c25

Please sign in to comment.