Skip to content

Commit

Permalink
made feedactive command
Browse files Browse the repository at this point in the history
  • Loading branch information
@GBKP
GBKP committed Jan 20, 2024
1 parent 89a7948 commit e71a465
Showing 1 changed file with 39 additions and 105 deletions.
144 changes: 39 additions & 105 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,23 +13,18 @@

package frc.robot;

import static edu.wpi.first.units.BaseUnits.Voltage;
import static edu.wpi.first.units.Units.*;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.*;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
import frc.robot.commands.DriveCommands;
import frc.robot.commands.FeedForwardCharacterization;
import frc.robot.subsystems.drive.*;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.feeder.FeederIO;
Expand Down Expand Up @@ -68,7 +63,7 @@ public RobotContainer() {
// Real robot, instantiate hardware IO implementations
drive =
new Drive(
new GyroIOPigeon2(),
new GyroIOPigeon2(),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
Expand All @@ -95,6 +90,7 @@ public RobotContainer() {
new ModuleIOSim());
flywheel = new Flywheel(new FlywheelIOSim());
feeder = new Feeder(new FeederIOSim());

break;

default:
Expand All @@ -120,113 +116,51 @@ public RobotContainer() {
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());

// Set up feedforward characterization
// autoChooser.addOption(
// "Drive FF Characterization",
// new FeedForwardCharacterization(
// drive, drive::runCharacterizationVolts, drive::getCharacterizationDriveVelocity));
// autoChooser.addOption(
// "Flywheel FF Characterization",
// new FeedForwardCharacterization(
// flywheel, flywheel::runVolts, flywheel::getCharacterizationDriveVelocity));
autoChooser.addOption(
"Drive FF Characterization",
new FeedForwardCharacterization(
drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity));
autoChooser.addOption(
"Flywheel FF Characterization",
new FeedForwardCharacterization(
flywheel, flywheel::runVolts, flywheel::getCharacterizationVelocity));

// Configure the button bindings
configureButtonBindings();
}

public static SysIDMode sysIDMode = SysIDMode.Disabled;

enum SysIDMode {
Disabled,
DriveMotors,
TurnMotors,
EverythingElse
}

This comment has been minimized.

Sign in to view

Copy link
@a1cd

a1cd Jan 30, 2024

Contributor

something somehow went wrong here @GloryBoyK , im trying to figure it out

This comment has been minimized.

Sign in to view

Copy link
@a1cd

a1cd Jan 30, 2024

Contributor

I think a more thurough review process would help here.
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
switch (sysIDMode) {
case Disabled:
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.onTrue(
Commands.runOnce(
() ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
drive)
.ignoringDisable(true));
controller
.a()
.whileTrue(
Commands.startEnd(
() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
break;
case DriveMotors:
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
var drivetrainDriveSysID =
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateDriveCharacterizationData,
drive,
"DrivetrainDriveMotors"));
controller
.x()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kForward))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.y()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.a()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
break;
case TurnMotors:
var drivetrainTurnSysID =
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateTurnCharacterizationData,
drive,
"DrivetrainDriveMotors"));
controller
.x()
.whileTrue(drivetrainTurnSysID.dynamic(Direction.kForward)
.andThen(drivetrainTurnSysID.dynamic(Direction.kReverse))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kForward))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kReverse))
.andThen(Commands.runOnce(drive::stopWithX, drive))
)
.onFalse(Commands.runOnce(drive::stopWithX, drive));
break;
case EverythingElse:
break;
}
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller
.leftTrigger()
.and(feeder::getSensorFeed)
.onTrue(new RunCommand(() -> feeder.runVolts(6.0)).until(() -> !feeder.getSensorFeed()));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.onTrue(
Commands.runOnce(
() ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
drive)
.ignoringDisable(true));
controller
.a()
.whileTrue(
Commands.startEnd(
() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
}

/**
Expand Down

0 comments on commit e71a465

Please sign in to comment.