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

changes to paths

Browse Source
This commit is contained in:
Aarav Shah
2021-04-03 11:09:19 -06:00
parent 7bca9f4d55
commit 03f822162d
20 changed files with 191 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -222,10 +222,10 @@ public final class Constants {
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[] 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 double[] aRed = {-3.1, 1.8};
public static final double[] bRed = {-6.3, 5.4};
public static final double[] aBlue = {9.3, 9.3};
public static final double[] bBlue = {4.4, 9.2};
}
public static final class OIConstants {
src/main/java/frc4388/robot/Robot.java
+4
View File
@@ -82,6 +82,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousInit() {
//m_robotContainer.buildAutos();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
@@ -105,6 +106,9 @@ public class Robot extends TimedRobot {
m_autonomousCommand.schedule();
SmartDashboard.putString("Is Auto Start?", "YEA");
}
else{
}
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+7 -93
View File
@@ -100,6 +100,8 @@ public class RobotContainer {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private final Joystick m_joystick = new Joystick(0);
public boolean isGS = false;
/* Autos */
double m_totalTimeAuto;
@@ -286,100 +288,13 @@ public class RobotContainer {
}
public void buildAutos() {
String[] sixBallAutoMiddlePaths = new String[]{
"SixBallMidComplete"
};
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
String[] sixBallAutoMiddle0Paths = new String[]{
"SixBallMid0"
};
m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
String[] sixBallAutoMiddle1Paths = new String[]{
"SixBallMid1"
};
m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
String[] slalom = new String[]{
"Slalom"
};
m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
String[] barrel = new String[]{
"Barrel"
};
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
String[] barrelStart = new String[]{
"BarrelStart"
};
m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
String[] bounce = new String[]{
"Bounce1",
"Bounce2",
"Bounce3",
"Bounce4"
};
m_bounce = new Bounce(m_robotDrive, buildPaths(bounce));
String[] barrelMany = new String[]{
"BarrelManyWaypoints"
};
m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
String[] eightBallAutoMiddlePaths = new String[]{
"EightBallMidComplete"
};
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
String[] driveOffLineForwardPaths = new String[]{
"DriveOffLineForward"
};
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
String[] driveOffLineBackwardPaths = new String[]{
"DriveOffLineBackward"
};
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
String[] fiveBallAutoMiddlePaths = new String[]{
"FiveBallMidComplete"
};
m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
String[] tenBallAutoMiddlePaths = new String[]{
"TenBallMidComplete"
};
m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
String[] sequentialTestPaths = new String[]{
"Seq1",
"Seq2"
};
m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
String[] galacticSearchPaths = new String[]{
"aRed",
"aBlue",
"bRed",
"bBlue"
"GSC_ARED",
"GSC_ABLUE",
"GSC_BRED",
"GSC_BBLUE"
};
m_robotLime.identifyPath();
m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
}
@@ -397,7 +312,6 @@ public class RobotContainer {
// Run path following command, then stop at the end.
try {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
//return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java
+65
View File
@@ -0,0 +1,65 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import frc4388.robot.subsystems.LimeLight;
public class ExecuteCommand extends CommandBase {
/**
* Creates a new ExecuteCommand.
*/
RamseteCommand[] m_paths;
LimeLight m_limeLight;
public ExecuteCommand(LimeLight limeLight, RamseteCommand[] paths) {
// Use addRequirements() here to declare subsystem dependencies.
m_limeLight = limeLight;
m_paths = paths;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
String gsPath = m_limeLight.galacticSearchPath;
switch (gsPath)
{
case "A_RED":
new RunPath(m_paths[0]);
break;
case "A_BLUE":
new RunPath(m_paths[1]);
break;
case "B_RED":
new RunPath(m_paths[2]);
break;
case "B_BLUE":
new RunPath(m_paths[3]);
break;
case "test":
new RunPath(m_paths[0]);
break;
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -22,12 +23,12 @@ public class GalacticSearch extends SequentialCommandGroup {
* Creates a new GalacticSearch.
*/
public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) {
addCommands(
paths[0]
);
/* if (m_limeLight.galacticSearchPath == "A_RED")
// addCommands(
// new IdentifyPath(m_limeLight),
// new ExecuteCommand(m_limeLight, paths)
// );
if (m_limeLight.galacticSearchPath == "A_RED")
{
System.out.println(m_limeLight.galacticSearchPath +"YOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO");
addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "A_BLUE")
@@ -41,6 +42,10 @@ public class GalacticSearch extends SequentialCommandGroup {
else if (m_limeLight.galacticSearchPath == "B_BLUE")
{
addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake)));
}*/
}
else if (m_limeLight.galacticSearchPath == "test")
{
addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
}
}
}
src/main/java/frc4388/robot/commands/auto/IdentifyPath.java
+3 -2
View File
@@ -47,7 +47,7 @@ public class IdentifyPath extends CommandBase {
target = m_limeLight.getV();
xAngle = m_limeLight.getX();
yAngle = m_limeLight.getY();
m_limeLight.limeOn();
//m_limeLight.limeOn();
// //Identify which of four paths
// m_limeLight.changePipeline(1);//Dual Targetting Lowest
// if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
@@ -117,6 +117,7 @@ public class IdentifyPath extends CommandBase {
path = "B_RED";
}
path = "test";
}
public boolean withinError(double angle, double goal)
@@ -154,7 +155,7 @@ public class IdentifyPath extends CommandBase {
{
SmartDashboard.putString("GalacticSearchPath", path);
m_limeLight.galacticSearchPath = path;
m_limeLight.limeOff();
//m_limeLight.limeOff();
return true;
}
return false;
src/main/java/frc4388/robot/commands/auto/RunPath.java
+27
View File
@@ -0,0 +1,27 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class RunPath extends SequentialCommandGroup {
/**
* Creates a new RunPath.
*/
public RunPath(RamseteCommand path) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
path
);
}
}
src/main/java/frc4388/robot/subsystems/LimeLight.java
+53
View File
@@ -8,7 +8,9 @@
package frc4388.robot.subsystems;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
public class LimeLight extends SubsystemBase {
/**
@@ -48,6 +50,57 @@ public class LimeLight extends SubsystemBase {
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
}
public void identifyPath(){
changePipeline(1);
if (withinCoords(VisionConstants.aBlue))
{
galacticSearchPath = "A_BLUE";
}
changePipeline(2);
if (withinCoords(VisionConstants.bBlue))
{
galacticSearchPath = "B_BLUE";
}
changePipeline(1);
if (withinCoords(VisionConstants.aRed))
{
galacticSearchPath = "A_RED";
}
changePipeline(1);
if (withinCoords(VisionConstants.bRed))
{
galacticSearchPath = "B_RED";
}
SmartDashboard.putString("GalacticSearchPath", galacticSearchPath);
}
public boolean withinError(double angle, double goal)
{
if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
{
return true;
}
else
{
return false;
}
}
public boolean withinCoords(double[] coords)
{
if (withinError(getX(), coords[0]) && withinError(getY(), coords[1]))
{
return true;
}
else
{
return false;
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run