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

Updated vision code for 2022

Browse Source
This commit is contained in:
aarav18
2022-03-05 11:29:40 -07:00
parent 71e082e7bd
commit 46bda7913f
8 changed files with 403 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/AimToCenter.java
+9 -2
View File
@@ -6,13 +6,16 @@ package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
public class AimToCenter extends CommandBase {
/** Creates a new AimWithOdometry. */
Turret m_turret;
SwerveDrive m_drive;
VisionOdometry m_visionOdometry;
// use odometry to find x and y later
double x;
@@ -21,11 +24,12 @@ public class AimToCenter extends CommandBase {
// public static Gains m_aimGains;
public AimToCenter(Turret turret, SwerveDrive drive) {
public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) {
// Use addRequirements() here to declare subsystem dependencies.
m_turret = turret;
m_drive = drive;
addRequirements(m_turret, m_drive);
m_visionOdometry = visionOdometry;
addRequirements(m_turret, m_drive, m_visionOdometry);
}
// Called when the command is initially scheduled.
@@ -40,6 +44,9 @@ public class AimToCenter extends CommandBase {
public void execute() {
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
m_turret.runshooterRotatePID(m_targetAngle);
// Check if limelight is within range
m_visionOdometry.setLEDs(Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE);
}
public static double angleToCenter(double x, double y, double gyro) {