Skip to content

Implement SwerveImu for threadedGyro #20

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
armadilloz wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Implement SwerveImu for threadedGyro #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
158 changes: 86 additions & 72 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,25 +4,25 @@

package frc.robot;

import java.io.File;

import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.intake.SpinIntake;
import frc.robot.commands.roller.SpinRoller;
import frc.robot.commands.tilt.TiltDown;
import frc.robot.commands.tilt.TiltUp;
import frc.robot.subsystems.GyroSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.RollerSubsystem;
import frc.robot.subsystems.TiltSubsystem;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import frc.robot.utils.logging.io.gyro.RealGyroIo;
import frc.robot.utils.logging.io.gyro.ThreadedGyro;
import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU;
import frc.robot.utils.simulation.RobotVisualizer;
import swervelib.SwerveInputStream;
import swervelib.imu.SwerveIMU;

import java.io.File;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -31,78 +31,90 @@
* subsystems, commands, and trigger mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
//private final RollerSubsystem rollerSubsystem;
//private final TiltSubsystem tiltSubsystem;
private final IntakeSubsystem intakeSubsystem;
private RobotVisualizer robotVisualizer = null;
private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"YAGSL"));
private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT);
private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT);
// The robot's subsystems and commands are defined here...
//private final RollerSubsystem rollerSubsystem;
//private final TiltSubsystem tiltSubsystem;
private final IntakeSubsystem intakeSubsystem;
private RobotVisualizer robotVisualizer = null;
private SwerveSubsystem drivebase = null;
private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT);
private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT);

// Replace with CommandPS4Controller or CommandJoystick if needed
//new CommandXboxController(OperatorConstants.kDriverControllerPort);
// Replace with CommandPS4Controller or CommandJoystick if needed
//new CommandXboxController(OperatorConstants.kDriverControllerPort);

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the trigger bindings
switch (Constants.currentMode) {
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
switch (Constants.currentMode) {
case REAL -> {
//rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo());
//tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo());
intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo(), new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL));
// For now, we are not creating an instance of the gyro subsystem - just the threadedIO...
RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo();
ThreadedGyro threadedGyro = gyroIo.getThreadedGyro();
SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro);
drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU);
}
case REPLAY -> {
//rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo());
//tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo());
intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL));
// create the drive subsystem with null gyro (use default json)
drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null);
}
case SIM -> {
robotVisualizer = new RobotVisualizer();
//rollerSubsystem = new RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer));
//tiltSubsystem = new TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer));
//rollerSubsystem = new RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer));
//tiltSubsystem = new TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer));
intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL));
// create the drive subsystem with null gyro (use default json)
drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null);
}

default -> {
throw new RuntimeException("Did not specify Robot Mode");
}
}
configureBindings();
putShuffleboardCommands();
}
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driveJoystick.getY() * -1,
() -> driveJoystick.getX() * -1)
.withControllerRotationAxis(steerJoystick::getX)
.deadband(Constants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true)
.allianceRelativeControl(false);
}
configureBindings();
putShuffleboardCommands();
}

/**
* 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}.
*/
private void configureBindings() {
// Schedule `ExampleCommand` when `exampleCondition` changes to `true`
//new Trigger(m_exampleSubsystem::exampleCondition)
// .onTrue(new ExampleCommand(m_exampleSubsystem));


/**
* 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}.
*/
private void configureBindings() {
// Schedule `ExampleCommand` when `exampleCondition` changes to `true`
//new Trigger(m_exampleSubsystem::exampleCondition)
// .onTrue(new ExampleCommand(m_exampleSubsystem));
// Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
// cancelling on release.
// m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
// TODO: Clean this up a little - create command in method and only create the one actually needed
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driveJoystick.getY() * -1,
() -> driveJoystick.getX() * -1)
.withControllerRotationAxis(steerJoystick::getX)
.deadband(Constants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true)
.allianceRelativeControl(false);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity);
}

// Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
// cancelling on release.
// m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity);
}
public void putShuffleboardCommands() {
public void putShuffleboardCommands() {
if (Constants.DEBUG) {
/*SmartDashboard.putData(
"Spin Roller",
Expand All @@ -120,17 +132,19 @@ public void putShuffleboardCommands() {
"Spin Intake",
new SpinIntake(intakeSubsystem));
}
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return null;
}
public RobotVisualizer getRobotVisualizer() {
return robotVisualizer;
}
}

/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return null;
}

public RobotVisualizer getRobotVisualizer() {
return robotVisualizer;
}
}
Loading