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

GalacticSearch

Browse Source
This commit is contained in:
ryan123rudder
2021-04-01 16:44:38 -06:00
parent 6593c4cbaf
commit 7bca9f4d55
4 changed files with 100 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -3
View File
@@ -216,12 +216,16 @@ public final class Constants {
public static final double GRAV = 385.83;
//Galactic Search
public static final double searchError = 0.5;
public static final double bothCloseVisibleY = -17.69;
public static final double searchError = 1;
/*public static final double bothCloseVisibleY = -17.69;
public static final double closeLeftVisibleY = -12.57;
public static final double closeRightVisibleY = -11.35;
public static final double farLeftVisibleX = 3.58;
public static final double farRightVisibleX = 7.04;
public static final double farRightVisibleX = 7.04;*/
public static final double[] aRed = {-3.9, 0.8};
public static final double[] bRed = {5.4, 5};
public static final double[] aBlue = {9.27, 7};
public static final double[] bBlue = {4.9, 8.6};
}
public static final class OIConstants {
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -409,7 +409,7 @@ public class RobotContainer {
//return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
return new IdentifyPath(m_robotLime).andThen(() -> m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)));
return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
} catch (Exception e) {
System.err.println("ERROR");
src/main/java/frc4388/robot/commands/auto/GalacticSearch.java
+11 -6
View File
@@ -7,6 +7,7 @@
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.intake.RunIntake;
@@ -21,21 +22,25 @@ public class GalacticSearch extends SequentialCommandGroup {
* Creates a new GalacticSearch.
*/
public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) {
if (m_limeLight.galacticSearchPath == "A_RED")
addCommands(
paths[0]
);
/* if (m_limeLight.galacticSearchPath == "A_RED")
{
addCommands(new SequentialCommandGroup(paths[0], new RunIntake(m_intake)));
System.out.println(m_limeLight.galacticSearchPath +"YOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO");
addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "A_BLUE")
{
addCommands(new SequentialCommandGroup(paths[1], new RunIntake(m_intake)));
addCommands(new ParallelCommandGroup(paths[1], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "B_RED")
{
addCommands(new SequentialCommandGroup(paths[2], new RunIntake(m_intake)));
addCommands(new ParallelCommandGroup(paths[2], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "B_BLUE")
{
addCommands(new SequentialCommandGroup(paths[3], new RunIntake(m_intake)));
}
addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake)));
}*/
}
}
src/main/java/frc4388/robot/commands/auto/IdentifyPath.java
+81 -38
View File
@@ -25,6 +25,7 @@ public class IdentifyPath extends CommandBase {
public String path;
boolean closeVisible;
boolean finished;
boolean pathFound;
public IdentifyPath(LimeLight limeLight) {
m_limeLight = limeLight;
@@ -37,6 +38,7 @@ public class IdentifyPath extends CommandBase {
public void initialize() {
path = "";
closeVisible = false;
pathFound = false;
}
// Called every time the scheduler runs while the command is scheduled.
@@ -46,51 +48,92 @@ public class IdentifyPath extends CommandBase {
xAngle = m_limeLight.getX();
yAngle = m_limeLight.getY();
m_limeLight.limeOn();
//Identify which of four paths
m_limeLight.changePipeline(1);//Dual Targetting Lowest
if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
{
closeVisible = true;
}
else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS
{
closeVisible = false;
}
// //Identify which of four paths
// m_limeLight.changePipeline(1);//Dual Targetting Lowest
// if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
// {
// closeVisible = true;
// }
// else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS
// {
// closeVisible = false;
// }
if (closeVisible)
{
m_limeLight.changePipeline(2); //Dual Targetting Highest
if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
{
path = "A_BLUE";
}
if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH
{
path = "B_BLUE";
}
}
// if (closeVisible)
// {
// m_limeLight.changePipeline(2); //Dual Targetting Highest
// if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
// {
// path = "A_BLUE";
// }
// if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH
// {
// path = "B_BLUE";
// }
// }
else{
m_limeLight.changePipeline(1); //Dual Targetting Lowest
if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
{
path = "A_RED";
}
else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
{
path = "B_RED";
}
}
// else{
// m_limeLight.changePipeline(1); //Dual Targetting Lowest
// if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
// {
// path = "A_RED";
// }
// else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
// {
// path = "B_RED";
// }
// }
SmartDashboard.putBoolean("CloseVisible", closeVisible);
System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch");
System.out.println(path);
SmartDashboard.putBoolean("PathFound", pathFound);
// System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch");
// System.out.println(path);
m_limeLight.changePipeline(1);
if (withinCoords(VisionConstants.aBlue))
{
pathFound = true;
path = "A_BLUE";
}
m_limeLight.changePipeline(2);
if (withinCoords(VisionConstants.bBlue))
{
pathFound = true;
path = "B_BLUE";
}
m_limeLight.changePipeline(1);
if (withinCoords(VisionConstants.aRed))
{
pathFound = true;
path = "A_RED";
}
m_limeLight.changePipeline(1);
if (withinCoords(VisionConstants.bRed))
{
pathFound = true;
path = "B_RED";
}
}
public boolean withinError(double angle, double input)
public boolean withinError(double angle, double goal)
{
if(input > (angle - VisionConstants.searchError) && input < (angle + VisionConstants.searchError))
if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
{
return true;
}
else
{
return false;
}
}
public boolean withinCoords(double[] coords)
{
if (withinError(xAngle, coords[0]) && withinError(yAngle, coords[1]))
{
return true;
}