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

Merge branch 'add-storage' of https://github.com/Team4388/RiseOfRidgebotics2020 into add-storage

Browse Source
This commit is contained in:
ryan123rudder
2020-02-21 20:42:45 -07:00
parent 629046c032 47b5cdc200
commit 815cc6dcbc
5 changed files with 64 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/TrackTarget.java
+13
View File
@@ -7,6 +7,7 @@
package frc4388.robot.commands;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Shooter;
@@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase {
double yAngle = 0;
double target = 0;
public double distance;
public static double fireVel;
public static double fireAngle;
/**
* Uses the Limelight to track the target
@@ -46,6 +49,8 @@ public class TrackTarget extends CommandBase {
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
@@ -65,6 +70,14 @@ public class TrackTarget extends CommandBase {
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
SmartDashboard.putNumber("Distance to Target", distance);
double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
double xVel = (distance*VisionConstants.GRAV)/(yVel);
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
}
}