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; public static final double GRAV = 385.83;
//Galactic Search //Galactic Search
public static final double searchError = 0.5; public static final double searchError = 1;
public static final double bothCloseVisibleY = -17.69; /*public static final double bothCloseVisibleY = -17.69;
public static final double closeLeftVisibleY = -12.57; public static final double closeLeftVisibleY = -12.57;
public static final double closeRightVisibleY = -11.35; public static final double closeRightVisibleY = -11.35;
public static final double farLeftVisibleX = 3.58; 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 { 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_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_barrelStart.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 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) { } catch (Exception e) {
System.err.println("ERROR"); 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; 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.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.intake.RunIntake; import frc4388.robot.commands.intake.RunIntake;
@@ -21,21 +22,25 @@ public class GalacticSearch extends SequentialCommandGroup {
* Creates a new GalacticSearch. * Creates a new GalacticSearch.
*/ */
public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) { 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") 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") 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") 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; public String path;
boolean closeVisible; boolean closeVisible;
boolean finished; boolean finished;
boolean pathFound;
public IdentifyPath(LimeLight limeLight) { public IdentifyPath(LimeLight limeLight) {
m_limeLight = limeLight; m_limeLight = limeLight;
@@ -37,6 +38,7 @@ public class IdentifyPath extends CommandBase {
public void initialize() { public void initialize() {
path = ""; path = "";
closeVisible = false; closeVisible = false;
pathFound = false;
} }
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@@ -46,51 +48,92 @@ public class IdentifyPath extends CommandBase {
xAngle = m_limeLight.getX(); xAngle = m_limeLight.getX();
yAngle = m_limeLight.getY(); yAngle = m_limeLight.getY();
m_limeLight.limeOn(); m_limeLight.limeOn();
//Identify which of four paths // //Identify which of four paths
m_limeLight.changePipeline(1);//Dual Targetting Lowest // m_limeLight.changePipeline(1);//Dual Targetting Lowest
if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS // if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
{ // {
closeVisible = true; // closeVisible = true;
} // }
else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS // else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS
{ // {
closeVisible = false; // closeVisible = false;
} // }
if (closeVisible) // if (closeVisible)
{ // {
m_limeLight.changePipeline(2); //Dual Targetting Highest // m_limeLight.changePipeline(2); //Dual Targetting Highest
if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH // if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
{ // {
path = "A_BLUE"; // path = "A_BLUE";
} // }
if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH // if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH
{ // {
path = "B_BLUE"; // path = "B_BLUE";
} // }
} // }
else{ // else{
m_limeLight.changePipeline(1); //Dual Targetting Lowest // m_limeLight.changePipeline(1); //Dual Targetting Lowest
if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH // if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
{ // {
path = "A_RED"; // path = "A_RED";
} // }
else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH // else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
{ // {
path = "B_RED"; // path = "B_RED";
} // }
} // }
SmartDashboard.putBoolean("CloseVisible", closeVisible); 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("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); // 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; return true;
} }