79 lines
2.7 KiB
Java
79 lines
2.7 KiB
Java
// 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 frc.robot;
|
|
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import frc.robot.constants.OIConstants;
|
|
import frc.robot.subsystems.Drivetrain;
|
|
import frc.robot.utilities.PoseSendable;
|
|
|
|
/*
|
|
* The objective for this part of testing is to see if we can get pose estimation values to work for the gyro angle
|
|
* required for Field Oriented AND can we find a way to reset the robot's pose to properly offset using the pose estimator.
|
|
*
|
|
* We should try to solve in this branch:
|
|
* - Does Field Oriented Drive work in a perfect world scenario when using the pose estimator as the rotation source?
|
|
* - Can we reset our pose in such a way using the pose estimator that we get consistent behavior of field oriented drive
|
|
* regardless of what orientation the robot is booted up in?
|
|
*
|
|
* Any changes we make should be able to be carried forward to part 3
|
|
*/
|
|
public class RobotContainer {
|
|
private Drivetrain drivetrain;
|
|
|
|
private CommandXboxController driver;
|
|
|
|
private PoseSendable poseSendable;
|
|
|
|
public RobotContainer() {
|
|
drivetrain = new Drivetrain(new Pose2d());
|
|
|
|
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
|
|
|
poseSendable = new PoseSendable();
|
|
|
|
configureBindings();
|
|
shuffleboardConfig();
|
|
}
|
|
|
|
private void configureBindings() {
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.teleopCommand(
|
|
driver::getLeftY,
|
|
driver::getLeftX,
|
|
() -> { return -driver.getRightX(); },
|
|
OIConstants.kTeleopDriveDeadband
|
|
)
|
|
);
|
|
|
|
driver.a().onTrue(drivetrain.zeroHeadingCommand());
|
|
|
|
driver.b().onTrue(
|
|
new InstantCommand(() -> {
|
|
System.out.println("X: " + poseSendable.getX());
|
|
System.out.println("Y: " + poseSendable.getY());
|
|
System.out.println("Rotation: " + poseSendable.getRotation());
|
|
drivetrain.resetOdometry(poseSendable.getPose());
|
|
}
|
|
));
|
|
}
|
|
|
|
private void shuffleboardConfig() {
|
|
ShuffleboardTab tab = Shuffleboard.getTab("Testing");
|
|
tab.add("Drivetrain Pose", poseSendable)
|
|
.withPosition(0, 0)
|
|
.withSize(2, 3);
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return null;
|
|
}
|
|
}
|