Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'full-robot-test' of https://github.com/Team4388/2022NoWayHome into full-robot-test

Browse Source
This commit is contained in:
Ryan Manley
2022-03-06 14:51:33 -07:00
parent e39d6d8675 9299ee0974
commit f490efabf6
1 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+8 -5
View File
@@ -153,10 +153,7 @@ public class Robot extends TimedRobot {
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
} }
public void updateOdoChooser() { public void updateOdoChooser() {
Pose2d currentPose = m_robotContainer.getOdometry();
odoChooser.setDefaultOption("(" + currentPose.getX() + ", " + currentPose.getY() + ", " + currentPose.getRotation().getDegrees() + ")", currentPose);
for (Map.Entry<String,Pose2d> entry : odoChoices.entrySet()) { for (Map.Entry<String,Pose2d> entry : odoChoices.entrySet()) {
odoChooser.addOption(entry.getKey(), entry.getValue()); odoChooser.addOption(entry.getKey(), entry.getValue());
} }
@@ -164,7 +161,7 @@ public class Robot extends TimedRobot {
public void addOdoChoices(Pose2d... points) { public void addOdoChoices(Pose2d... points) {
for (Pose2d point : points) { for (Pose2d point : points) {
String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + ")"; String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + "°)";
odoChoices.put(key, point); odoChoices.put(key, point);
} }
} }
@@ -203,6 +200,9 @@ public class Robot extends TimedRobot {
LOGGER.fine("autonomousInit()"); LOGGER.fine("autonomousInit()");
selectedOdo = odoChooser.getSelected(); selectedOdo = odoChooser.getSelected();
if (selectedOdo == null) {
selectedOdo = m_robotContainer.getOdometry();
}
m_robotContainer.resetOdometry(selectedOdo); m_robotContainer.resetOdometry(selectedOdo);
m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -227,6 +227,9 @@ public class Robot extends TimedRobot {
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
selectedOdo = odoChooser.getSelected(); selectedOdo = odoChooser.getSelected();
if (selectedOdo == null) {
selectedOdo = m_robotContainer.getOdometry();
}
m_robotContainer.resetOdometry(selectedOdo); m_robotContainer.resetOdometry(selectedOdo);
// This makes sure that the autonomous stops running when // This makes sure that the autonomous stops running when