Skip to content

Commit

Permalink
Merge branch 'main' into 29-auto-align-to-tag-using-pathfinding
Browse files Browse the repository at this point in the history
  • Loading branch information
@HeroSoLos
HeroSoLos committed Feb 16, 2024
2 parents c0d45a8 + 619b716 commit 343b9c0
Show file tree
Hide file tree
Showing 9 changed files with 421 additions and 75 deletions.
47 changes: 32 additions & 15 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ public class DrivetrainConstants { // TODO Get new for new robot
public static final double ROT_MULT = 0.015; // TODO Tune for Driver

public static final double SLOW_ROT_MULT = 0.007; // TODO Tune for Driver
public static final double SLOW_SPEED_MULT = 0.01; // TODO Tune for Driver
public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver

public static final double SYS_TEST_SPEED_DRIVE = 0.5;
public static final double SYS_TEST_SPEED_TURN = 1d;
Expand Down Expand Up @@ -82,19 +82,19 @@ public class CAN {

// TODO check
public static final int COLLECTOR_MOTOR = 9;
public static final int INDEXER_MOTOR = 11;
public static final int PIVOT_ANGLE_MOTOR = 12;
public static final int FLYWHEEL_MOTOR_TOP = 13;
public static final int FLYWHEEL_MOTOR_BOTTOM = 14;
public static final int CLIMB_RIGHT = 15;
public static final int CLIMB_LEFT = 16;
public static final int INDEXER_MOTOR = 10;
public static final int PIVOT_ANGLE_MOTOR = 11;
public static final int FLYWHEEL_MOTOR_TOP = 12;
public static final int FLYWHEEL_MOTOR_BOTTOM = 13;
public static final int CLIMB_RIGHT = 14;
public static final int CLIMB_LEFT = 15;

// TODO check
// Cancoders
public static final int FLYWHEEL_CANCODER = 35;
public static final int CLIMB_CANCODERR = 36;
public static final int CLIMB_CANCODERL = 37;
public static final int PIVOT_ANGLE_CANCODER = 38;
public static final int PIVOT_ANGLE_CANCODER = 35;

public static final String CANBUS_FD = "Canivore";
public static final String RIO_CANBUS = "rio";
Expand Down Expand Up @@ -132,16 +132,14 @@ public static class AutonomousConstants {
public static final double DRIVE_BASE_RADIUS = Units.inchesToMeters(19.09); // TODO check

public static final double CONTROL_LOOP_PERIOD = 0.004; // IS this right?


public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); //TODO get constants

//For Pathfinding
// TODO get real poses to pathfind to
public static final Pose2d TARGET_POSE = new Pose2d(new Translation2d(-1, 2), new Rotation2d(0d));
public static final double GOAL_END_VELOCITY = 0; // Goal end velocity in meters/sec
public static final double ROTATION_DELAY_DISTACE = 0d; // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.


public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); //TODO get constants
// For AlignToTag
public static final Pose2d AMP_POSE = new Pose2d(new Translation2d(1.85, 6.5), new Rotation2d(0d));
}

public static class TunerConstants {
Expand Down Expand Up @@ -428,7 +426,7 @@ public class ShooterConstants {
};
}

public class CandConstants {
public class CandConstants { // TODO get real
//Amp
public static final double AMP_TOP_RPM = 0;
public static final double AMP_BOTTOM_RPM = 0;
Expand All @@ -441,6 +439,25 @@ public class CandConstants {
//Podium
public static final double PODIUM_RPM = 0;
public static final double PODIUM_ANGLE = 0;

//C1
public static final double C1_RPM = 0;
public static final double C1_ANGLE = 0;

//C2
public static final double C2_RPM = 0;
public static final double C2_ANGLE = 0;

//C3
public static final double C3_RPM = 0;
public static final double C3_ANGLE = 0;

//Line
public static final double LINE_RPM = 0;
public static final double LINE_ANGLE = 0;

// TODO find time to shoot
public static final double TIME_TO_SHOOT = 2d; // Time in seconds it takes from indexer start to flywheel exit
}

public class ClimbConstants { //TODO: find real values
Expand Down
138 changes: 87 additions & 51 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
Expand All @@ -35,9 +36,14 @@
import frc.robot.command.PointAtTag;
import frc.robot.command.Sing;
import frc.robot.command.shoot.AmpShot;
import frc.robot.command.shoot.CandC1;
import frc.robot.command.shoot.CandC2;
import frc.robot.command.shoot.CandC3;
import frc.robot.command.shoot.CandLine;
import frc.robot.command.shoot.PodiumShot;
import frc.robot.command.shoot.PointBlankShot;
import frc.robot.command.shoot.SmartShoot;
import frc.robot.command.shoot.Stow;
import frc.robot.command.tests.ClimbSystemTest;
import frc.robot.command.tests.CollectorSystemTest;
import frc.robot.command.tests.DrivetrainSystemTest;
Expand Down Expand Up @@ -71,7 +77,6 @@ public class RobotContainer extends LightningContainer {
Collector collector;
// Flywheel flywheel;
// Pivot pivot;
// Shooter shooter;
// Indexer indexer;
// Climber climber;
LEDs leds;
Expand All @@ -94,15 +99,14 @@ protected void initializeSubsystems() {

driver = new XboxController(ControllerConstants.DriverControllerPort); // Driver controller
coPilot = new XboxController(ControllerConstants.CopilotControllerPort); // CoPilot controller

limelights = new Limelights();
drivetrain = TunerConstants.getDrivetrain(limelights);

// indexer = new Indexer();
// collector = new Collector();
// flywheel = new Flywheel();
// pivot = new Pivot();
// shooter = new Shooter(pivot, flywheel, indexer, collector);
// climber = new Climber(drivetrain);
leds = new LEDs();
nervo = new Nervo();
Expand Down Expand Up @@ -133,61 +137,80 @@ protected void initializeNamedCommands() {
NamedCommands.registerCommand("led-Shoot",
leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5));

// make sure named commands is initialized before autobuilder!
// NamedCommands.registerCommand("Cand-Sub", new PointBlankShot(flywheel, pivot, DriverStation.isAutonomousEnabled()));
// NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer));
// NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer));
// NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer));
// NamedCommands.registerCommand("Cand-Line", new CandLine(flywheel, pivot, indexer));
// NamedCommands.registerCommand("AMP", new AmpShot(flywheel, pivot, DriverStation.isAutonomousEnabled()));
// NamedCommands.registerCommand("Stow", new Stow(flywheel, pivot));
// NamedCommands.registerCommand("Smart-Shoot", new SmartShoot(flywheel, pivot, drivetrain, indexer, leds));
// NamedCommands.registerCommand("Chase-Pieces", new ChasePieces(drivetrain, collector, limelights));
// NamedCommands.registerCommand("Collect", new Collect(() -> 1d, collector)); //TODO use smart collect
// NamedCommands.registerCommand("Spit", new Collect(() -> -1d, collector)); //TODO use smart collect
// NamedCommands.registerCommand("Index-Up", new Index(indexer, () -> IndexerConstants.INDEXER_DEFAULT_POWER));
// NamedCommands.registerCommand("PathFind", new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain, drive)); // TODO find a way to use this

// make sure named commands are initialized before autobuilder!
autoChooser = AutoBuilder.buildAutoChooser();
LightningShuffleboard.set("Auton", "Auto Chooser", autoChooser);


}
@Override
protected void configureButtonBindings() {
/* driver */
// activates between field centric and robot centric drive + logs
new Trigger(() -> driver.getLeftTriggerAxis() > 0.25d).onTrue(new InstantCommand(() -> drivetrain.setRobotCentricControl(true))).whileTrue(
drivetrain.applyRequest(() -> driveRobotCentric
.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.ROT_MULT))) // Drive counterclockwise with negative X (left)
.onFalse(new InstantCommand(() -> drivetrain.setRobotCentricControl(false)));
new Trigger(() -> driver.getLeftTriggerAxis() > 0.25d)
.onTrue(new InstantCommand(() -> drivetrain.setRobotCentricControl(true)))
.whileTrue(drivetrain.applyRequest(() -> driveRobotCentric
.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.ROT_MULT))) // Drive counterclockwise with
// negative X (left)
.onFalse(new InstantCommand(() -> drivetrain.setRobotCentricControl(false)));

// activates between regular speed and slow mode + logs
new Trigger(() -> driver.getRightTriggerAxis() > 0.25d).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).whileTrue(
drivetrain.applyRequest(() -> drive
.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.SLOW_ROT_MULT))) // Drive counterclockwise with negative X (left)
.onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false)));
new Trigger(() -> driver.getRightTriggerAxis() > 0.25d)
.onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true)))
.whileTrue(drivetrain.applyRequest(() -> drive
.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed
* DrivetrainConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed
* DrivetrainConstants.SLOW_SPEED_MULT) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.SLOW_ROT_MULT))) // Drive counterclockwise with negative X (left)
.onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false)));

// activates robot centric driving and slow mode
new Trigger(() -> drivetrain.isRobotCentricControl() && drivetrain.inSlowMode()).whileTrue(
drivetrain.applyRequest(() -> driveRobotCentric
.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.SLOW_ROT_MULT))); // Drive counterclockwise with negative X (left)
new Trigger(() -> drivetrain.isRobotCentricControl() && drivetrain.inSlowMode())
.whileTrue(drivetrain.applyRequest(() -> driveRobotCentric
.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed
* DrivetrainConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed
* DrivetrainConstants.SLOW_SPEED_MULT) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.SLOW_ROT_MULT))); // Drive counterclockwise with negative X (left)

// resets the gyro of the robot
new Trigger(() -> driver.getStartButton() && driver.getBackButton())
.onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative));

// makes the robot chase pieces
// new Trigger(driver::getRightBumper).whileTrue(new ChasePieces(drivetrain, collector, limelights));

// parks the robot
new Trigger(driver::getXButton).whileTrue(drivetrain.applyRequest(() -> brake));

// smart shoot for the robot
// new Trigger(driver::getAButton).whileTrue(new SmartShoot(flywheel, pivot, drivetrain, indexer, leds));

Expand All @@ -206,28 +229,36 @@ protected void configureButtonBindings() {
// Test auto align
/* copilot */
// cand shots for the robot
// new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot));
// new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot, false));
// new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot));
// new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot));

// new Trigger(coPilot::getBButton).whileTrue(new Climb(climber, drivetrain).deadlineWith(leds.enableState(LED_STATES.CLIMBING)));
// new Trigger(coPilot::getBButton).whileTrue(new Climb(climber,
// drivetrain).deadlineWith(leds.enableState(LED_STATES.CLIMBING)));

/*BIAS */
// new Trigger(() -> coPilot.getPOV() == 0).onTrue(new InstantCommand(() -> pivot.increaseBias())); // UP
// new Trigger(() -> coPilot.getPOV() == 180).onTrue(new InstantCommand(() -> pivot.decreaseBias())); // DOWN
/* BIAS */
// new Trigger(() -> coPilot.getPOV() == 0).onTrue(new InstantCommand(() ->
// pivot.increaseBias())); // UP
// new Trigger(() -> coPilot.getPOV() == 180).onTrue(new InstantCommand(() ->
// pivot.decreaseBias())); // DOWN

// new Trigger(() -> coPilot.getPOV() == 90).onTrue(new InstantCommand(() -> flywheel.increaseBias())); // RIGHT
// new Trigger(() -> coPilot.getPOV() == 270).onTrue(new InstantCommand(() -> flywheel.decreaseBias())); // LEFT
// new Trigger(() -> coPilot.getPOV() == 90).onTrue(new InstantCommand(() ->
// flywheel.increaseBias())); // RIGHT
// new Trigger(() -> coPilot.getPOV() == 270).onTrue(new InstantCommand(() ->
// flywheel.decreaseBias())); // LEFT

// new Trigger(coPilot::getRightBumper).whileTrue(new Index(indexer,() -> IndexerConstants.INDEXER_DEFAULT_POWER));
// new Trigger(coPilot::getLeftBumper).whileTrue(new Index(indexer,() -> -IndexerConstants.INDEXER_DEFAULT_POWER));
// new Trigger(coPilot::getRightBumper).whileTrue(new Index(indexer,() ->
// IndexerConstants.INDEXER_DEFAULT_POWER));
// new Trigger(coPilot::getLeftBumper).whileTrue(new Index(indexer,() ->
// -IndexerConstants.INDEXER_DEFAULT_POWER));

/* Other */
// new Trigger(() -> (limelights.getStopMe().hasTarget() || limelights.getChamps().hasTarget())).whileTrue(leds.enableState(LED_STATES.HAS_VISION));
// new Trigger(() -> collector.hasPiece()).whileTrue(leds.enableState(LED_STATES.HAS_PIECE).withTimeout(2)).onTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2));

}


@Override
protected void configureDefaultCommands() {
/* driver */
Expand All @@ -241,7 +272,8 @@ protected void configureDefaultCommands() {
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(),
ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate
* DrivetrainConstants.ROT_MULT) // Drive counterclockwise with negative X (left)
* DrivetrainConstants.ROT_MULT) // Drive counterclockwise with
// negative X (left)
));


Expand Down Expand Up @@ -279,6 +311,9 @@ protected void configureSystemTests() {
// SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector,
// Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER));

// SystemTest.registerTest("Shooter Test", new ShooterSystemTest(shooter, flywheel,
// collector, indexer, pivot));

// SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector,
// indexer, pivot, Constants.FlywheelConstants.SYS_TEST_SPEED));

Expand All @@ -287,13 +322,14 @@ protected void configureSystemTests() {

// Sing chooser
SendableChooser<SystemTestCommand> songChooser = new SendableChooser<>();
songChooser.setDefaultOption(MusicConstants.BOH_RHAP_FILEPATH, new SingSystemTest(drivetrain, MusicConstants.BOH_RHAP_FILEPATH, sing));
for (String filepath: MusicConstants.SET_LIST){
songChooser.setDefaultOption(MusicConstants.BOH_RHAP_FILEPATH,
new SingSystemTest(drivetrain, MusicConstants.BOH_RHAP_FILEPATH, sing));
for (String filepath : MusicConstants.SET_LIST) {
songChooser.addOption(filepath, new SingSystemTest(drivetrain, filepath, sing));
}

LightningShuffleboard.set("SystemTest", "Songs List", songChooser);

songChooser.onChange((SystemTestCommand command) -> {
if (command != null) {
command.schedule();
Expand Down
Loading

0 comments on commit 343b9c0

Please sign in to comment.