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

tried using getTargetPoints + turret already aimed auto

Browse Source
This commit is contained in:
aarav18
2022-03-21 23:47:30 -06:00
parent 1833d51855
commit d35cc8432e
2 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+7 -1
View File
@@ -451,10 +451,16 @@ public class RobotContainer {
// new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive),
// "Diamond"));
// * assume turret is already pointed towards target.
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
// * aim with RotateUntilTarget
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
// new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
}
public static XboxController getDriverController() {
src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
+8 -1
View File
@@ -4,9 +4,12 @@
package frc4388.robot.commands.DriveCommands;
import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.VisionObscuredException;
public class RotateUntilTarget extends CommandBase {
@@ -47,6 +50,10 @@ public class RotateUntilTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return this.visionOdometry.m_camera.getLatestResult().hasTargets();
try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; }
return true;
// return this.visionOdometry.m_camera.getLatestResult().hasTargets();
}
}