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

Merge branch 'buff-driver-station' of https://github.com/Team4388/RiseOfRidgebotics2020 into buff-driver-station

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-10 18:42:03 -06:00
parent 41597c99de ba715805d3
commit 7efbea847a
26 changed files with 1229 additions and 525 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+14 -7
View File
@@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
@@ -31,6 +32,7 @@ public class TrackTarget extends CommandBase {
double yAngle = 0;
double target = 0;
public double distance;
public double realDistance;
public static double fireVel;
public static double fireAngle;
@@ -48,6 +50,7 @@ public class TrackTarget extends CommandBase {
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
// Called when the command is initially scheduled.
@@ -65,8 +68,14 @@ public class TrackTarget extends CommandBase {
xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
realDistance = (1.09 * distance) - 12.8;
if (target == 1.0) { // If target in view
// Aiming Left/Right
xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance);
turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
turnAmount = 0;
@@ -79,10 +88,8 @@ public class TrackTarget extends CommandBase {
}
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
SmartDashboard.putNumber("Distance to Target", distance);
SmartDashboard.putNumber("Distance to Target", realDistance);
SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));
//START Equation Code
double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
double xVel = (distance * VisionConstants.GRAV) / (yVel);
@@ -92,8 +99,8 @@ public class TrackTarget extends CommandBase {
//END Equation Code
//START CSV Code
fireVel = m_shooter.m_shooterTable.getVelocity(distance);
fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
fireVel = m_shooter.m_shooterTable.getVelocity(realDistance);
fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different
//fireAngle = 33;
//END CSV Code
@@ -116,7 +123,7 @@ public class TrackTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (xAngle < 1 && xAngle > -1 && target == 1)
if (xAngle < 0.5 && xAngle > -0.5 && target == 1)
{
m_shooterAim.m_isAimReady = true;
} else {