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

Added 6BallTop fully and set framework for two others

Browse Source
This commit is contained in:
ryan123rudder
2021-09-21 20:35:05 -06:00
parent 37632212be
commit ace6170848
49 changed files with 13250 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+55 -12
View File
@@ -41,6 +41,7 @@ import frc4388.robot.commands.auto.GalacticSearch;
import frc4388.robot.commands.auto.IdentifyPath;
import frc4388.robot.commands.auto.SequentialTest;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.SixBallTop;
import frc4388.robot.commands.auto.Slalom;
import frc4388.robot.commands.auto.TenBallAutoMiddle;
import frc4388.robot.commands.auto.Wait;
@@ -120,21 +121,21 @@ public class RobotContainer {
/* Autos */
double m_totalTimeAuto;
SixBallAutoMiddle m_sixBallAutoMiddle;
//SixBallAutoMiddle m_sixBallAutoMiddle;
SixBallAutoMiddle m_sixBallAutoMiddle0;
//SixBallAutoMiddle m_sixBallAutoMiddle0;
SixBallAutoMiddle m_sixBallAutoMiddle1;
//SixBallAutoMiddle m_sixBallAutoMiddle1;
EightBallAutoMiddle m_eightBallAutoMiddle;
//EightBallAutoMiddle m_eightBallAutoMiddle;
DriveOffLineForward m_driveOffLineForward;
DriveOffLineBackward m_driveOffLineBackward;
FiveBallAutoMiddle m_fiveBallAutoMiddle;
//FiveBallAutoMiddle m_fiveBallAutoMiddle;
TenBallAutoMiddle m_tenBallAutoMiddle;
//TenBallAutoMiddle m_tenBallAutoMiddle;
Slalom m_slalom;
@@ -150,6 +151,12 @@ public class RobotContainer {
GalacticSearch m_galacticSearch;
SixBallTop m_sixBallTop;
EightBallMid m_eightBallMid;
FiveBallBottom m_fiveBallBottom;
public static boolean m_isShooterManual = false;
/**
@@ -345,7 +352,7 @@ public class RobotContainer {
String[] sixBallAutoMiddlePaths = new String[]{
"SixBallMidComplete"
};
/*
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
String[] sixBallAutoMiddle0Paths = new String[]{
@@ -359,7 +366,7 @@ public class RobotContainer {
};
m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
*/
String[] slalom = new String[]{
"Slalom"
};
@@ -397,6 +404,7 @@ public class RobotContainer {
"EightBallMidComplete"
};
/*
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
String[] driveOffLineForwardPaths = new String[]{
@@ -404,7 +412,7 @@ public class RobotContainer {
};
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
*/
String[] driveOffLineBackwardPaths = new String[]{
"DriveOffLineBackward"
};
@@ -415,6 +423,7 @@ public class RobotContainer {
"FiveBallMidComplete"
};
/*
m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
String[] tenBallAutoMiddlePaths = new String[]{
@@ -424,15 +433,34 @@ public class RobotContainer {
m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
*/
String[] galacticSearchPaths = new String[]{
"GSC_ARED",
"GSC_ABLUE",
"GSC_BRED",
"GSC_BBLUE"
};
m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
String[] sixBallTopPaths = new String[]{
"6BallTop"
};
m_sixBallTop = new SixBallTop(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(sixBallTopPaths));
String[] eightBallMidPaths = new String[]{
"2BallOffensive",
"LoopFrom2BallOffensive"
};
m_eightBallMid = new EightBallMid(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(eightBallMidPaths));
String[] fiveBallBottomPaths = new String[]{
"5BallBottom1",
"5BallBottom2",
"5BallBottom3"
};
m_fiveBallBottom = new FiveBallBottom(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(fiveBallBottomPaths));
}
public void idenPath()
@@ -455,14 +483,29 @@ public class RobotContainer {
//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));
//return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_slalom.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_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
//return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
//return m_sixBallTop.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
//return m_eightBallMid.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
return m_fiveBallBottom.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
/*
TODO
11BallTop
6BallOffensive (Trench)
6BallOffensive2
8BallOffensive
5BallBottom*
8BallMid*
6BallTop*
*/
} catch (Exception e) {
src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java
+2 -2
View File
@@ -28,7 +28,7 @@ public class AutoPath1FromCenter extends SequentialCommandGroup {
m_drive = subsystem;
m_pneumatics = subsystem2;
addCommands( new Wait(m_drive, 0, 1),
addCommands( new Wait(m_drive, 0),
//shoot pre-loaded 3 balls
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 75, 44, -90),
//Start Intake Ball 1
@@ -39,7 +39,7 @@ public class AutoPath1FromCenter extends SequentialCommandGroup {
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 3
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
new Wait(m_drive, 0, 2)
new Wait(m_drive, 0)
//Shoot 3 Balls
);
}
src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java
+2 -2
View File
@@ -28,7 +28,7 @@ public class AutoPath2FromRight extends SequentialCommandGroup {
m_drive = subsystem;
m_pneumatics = subsystem2;
addCommands( new Wait(m_drive, 0, 1),
addCommands( new Wait(m_drive, 0),
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 77),
//Start Intake Ball 1
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
@@ -48,7 +48,7 @@ public class AutoPath2FromRight extends SequentialCommandGroup {
//Move to 10th Ball
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 13.30),
//Shoot 5 more Balls (Total 10 Ball Autonomous Path)
new Wait(m_drive, 0, 2)
new Wait(m_drive, 0)
);
}
}
src/main/java/frc4388/robot/commands/auto/EightBallMid.java
+60
View File
@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.commands.intake.RunExtenderOutIn;
import frc4388.robot.commands.intake.RunIntake;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.subsystems.Drive;
// 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 EightBallMid extends SequentialCommandGroup {
/**
* Creates a new EightBallMid.
*/
public EightBallMid(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
//TODO REWRITE
//Shoot and Extend Intake
new CalibrateShooter(shooter, shooterAim, shooterHood),
new ParallelDeadlineGroup(
new Wait(drive,5),
new TrackTarget(shooterAim),
new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle())),
new RunExtenderOutIn(intake)
),
//Intake and Path
new ParallelDeadlineGroup(
paths[0],
new RunIntake(intake)
),
//Shoot
new ParallelDeadlineGroup(
new Wait(drive,5),
new TrackTarget(shooterAim),
new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle()))
)
);
}
}
src/main/java/frc4388/robot/commands/auto/FiveBallBottom.java
+60
View File
@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.commands.intake.RunExtenderOutIn;
import frc4388.robot.commands.intake.RunIntake;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.subsystems.Drive;
// 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 FiveBallBottom extends SequentialCommandGroup {
/**
* Creates a new FiveBallBottom.
*/
public FiveBallBottom(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
//TODO Rewrite
addCommands(
//Shoot and Extend Intake
new CalibrateShooter(shooter, shooterAim, shooterHood),
new ParallelDeadlineGroup(
new Wait(drive,5),
new TrackTarget(shooterAim),
new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle())),
new RunExtenderOutIn(intake)
),
//Intake and Path
new ParallelDeadlineGroup(
paths[0],
new RunIntake(intake)
),
//Shoot
new ParallelDeadlineGroup(
new Wait(drive,5),
new TrackTarget(shooterAim),
new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle()))
)
);
}
}
src/main/java/frc4388/robot/commands/auto/SixBallTop.java
+59
View File
@@ -0,0 +1,59 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.commands.intake.RunExtenderOutIn;
import frc4388.robot.commands.intake.RunIntake;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.subsystems.Drive;
// 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 SixBallTop extends SequentialCommandGroup {
/**
* Creates a new SixBallTop.
*/
public SixBallTop(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
//Shoot and Extend Intake
new CalibrateShooter(shooter, shooterAim, shooterHood),
new ParallelDeadlineGroup(
new Wait(drive,5),
new TrackTarget(shooterAim),
new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle())),
new RunExtenderOutIn(intake)
),
//Intake and Path
new ParallelDeadlineGroup(
paths[0],
new RunIntake(intake)
),
//Shoot
new ParallelDeadlineGroup(
new Wait(drive,5),
new TrackTarget(shooterAim),
new RunCommand(() -> shooterHood.runAngleAdjustPID(shooterHood.addFireAngle()))
)
);
}
}
src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java
+3 -3
View File
@@ -35,15 +35,15 @@ public class TenBallAutoMiddle extends SequentialCommandGroup {
// super(new FooCommand(), new BarCommand());
addCommands(
new ParallelDeadlineGroup(
new Wait(drive, 0.1, 0),
new Wait(drive, 0.1),
new CalibrateShooter(shooter, shooterAim, shooterHood)
),
new ParallelDeadlineGroup(
new Wait(drive, 1, 0),
new Wait(drive, 1),
new RunCommand(() -> shooterAim.runShooterWithInput(-0.75), shooterAim)
),
new ParallelDeadlineGroup(
new Wait(drive, 4, 0),
new Wait(drive, 4),
new PrepChecker(shooter, shooterAim),
new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake),
new ShootPrepGroup(shooter, shooterAim, shooterHood, storage)
src/main/java/frc4388/robot/commands/auto/Wait.java
+2 -2
View File
@@ -23,12 +23,12 @@ public class Wait extends CommandBase {
/**
* Creates a new WaitCommand.
*/
public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
public Wait(SubsystemBase subsystem, double seconds) {
// Use addRequirements() here to declare subsystem dependencies.
m_waitTime = (long) (seconds * 1000);
m_subsystem = subsystem;
m_waitNum = waitNum;
// m_waitNum = waitNum;
addRequirements(m_subsystem);
}
src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java
+2 -2
View File
@@ -44,7 +44,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
m_endAngle = endAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new Wait(m_drive, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
}
@@ -63,7 +63,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
m_endAngle = m_currentAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new Wait(m_drive, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
}
src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java
+2 -2
View File
@@ -46,9 +46,9 @@ public class RunExtenderOutIn extends CommandBase {
@Override
public void execute() {
if (m_intake.isExtended){
m_intake.runExtender(0.3);
m_intake.runExtender(0.5);
} else {
m_intake.runExtender(-0.3);
m_intake.runExtender(-0.5);
}
}
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+14
View File
@@ -11,6 +11,7 @@ import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
@@ -115,6 +116,19 @@ public class TrackTarget extends CommandBase {
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
m_shooterAim.m_targetDistance = distance;
}
else{
//Sweeping
double turn = -0.5;
if (m_shooterAim.getShooterRotatePosition() > ShooterConstants.TURRET_RIGHT_SOFT_LIMIT - 1)
{
turn = -0.5;
}
if (m_shooterAim.getShooterRotatePosition() < ShooterConstants.TURRET_LEFT_SOFT_LIMIT + 1)
{
turn = 0.5;
}
m_shooterAim.runShooterWithInput(turn);
}
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+1
View File
@@ -132,6 +132,7 @@ public class ShooterAim extends SubsystemBase {
return m_targetDistance * Math.sin(getTargetAngleDegrees());
}
/**
* Gets the angle of the Shooter relative to the inner port.
* Use for tuning the Shooter Displacement