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 'master' into fix-storage-2-electric-bogaloo

Browse Source
This commit is contained in:
ryan123rudder
2020-03-09 19:13:52 -06:00
committed by GitHub
parent f19716ba8f 96c4f644bf
commit e4b9969244
5 changed files with 52 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -5
View File
@@ -116,8 +116,7 @@ public final class Constants {
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
public static final int SHOOTER_TIMEOUT_MS = 30;
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
//public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final double SHOOTER_TURRET_MIN = -1.0;
@@ -203,9 +202,9 @@ public final class Constants {
public static final class VisionConstants {
public static final double FOV = 29.8; //Field of view of limelight
public static final double TARGET_HEIGHT = 71;
public static final double LIME_ANGLE = 25;
public static final double TURN_P_VALUE = 0.65;
public static final double TARGET_HEIGHT = 71.5;
public static final double LIME_ANGLE = 24.7;
public static final double TURN_P_VALUE = 0.6;
public static final double X_ANGLE_ERROR = 1.3;
public static final double MOTOR_DEAD_ZONE = 0.3;
public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -119,7 +119,7 @@ public class RobotContainer {
// runs the turret with joystick
m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
// moves the drum not
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter));
// drives climber with input from triggers on the opperator controller
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
// drives the leveler with an axis input from the driver controller
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
@@ -115,7 +122,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 {
src/main/java/frc4388/utility/ShooterTables.java
+20 -1
View File
@@ -20,12 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
* Add your docs here.
*/
public class ShooterTables {
double[][] m_distance = new double[50][3];
double[][] m_distance = new double[50][4];
double[][] m_angle = new double[50][2];
final int m_columnDistance = 0;
final int m_columnHood = 1;
final int m_columnVel = 2;
final int m_columnCenterDisplacement = 3;
final int m_columnAngle = 0;
final int m_columnDisplacement = 1;
@@ -55,6 +56,7 @@ public class ShooterTables {
m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]);
lineNum ++;
}
@@ -126,6 +128,23 @@ public class ShooterTables {
}
}
public double getCenterDisplacement(double distance) { //Degrees of Lime FOV
int i = 0;
while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
i ++;
}
if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
return m_distance[i][m_columnCenterDisplacement];
} else {
if (i >= m_distanceLength) {
i = m_distanceLength - 1;
}
return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance);
}
}
public double getAngleDisplacement(double angle) {
int i = 0;
while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {