Skip to content

Commit

Permalink
Merge pull request #17 from ATAARobotics/Unstable
Browse files Browse the repository at this point in the history 
Merge Unstable for Champs
  • Loading branch information
@Jacob-Guglielmin
Jacob-Guglielmin authored Apr 13, 2022
2 parents d32064c + 4781527 commit 2e6fef6
Show file tree
Hide file tree
Showing 25 changed files with 1,163 additions and 834 deletions.
1 change: 1 addition & 0 deletions Important Info.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ station, 13 being the one at ours.
Ball 2: (2.0289, 4.9690)
Ball 4: (6.3034, 5.0716)
Ball 5: (7.9558, 7.5447)
Ball 13: (7.5683, 1.0915)

================
AUTO INFORMATION
Expand Down
26 changes: 14 additions & 12 deletions src/main/deploy/bindings.properties
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,22 +25,24 @@
# Queries are not allowed - these have to be set in code unfortunately

# Drive Stick
XVelocity: LeftX
YVelocity: LeftY
RotationVelocity: RightX
ToggleFieldOriented: LeftBumper-Pressed
SwitchCameras: None
Intake: Y
ShootLow: A
ShootHighClose: B
ShootHighFar: X
SwitchFronts: Start-Pressed
XVelocity: X
YVelocity: Y
RotationVelocity: Rotate
Speed: Slider
ToggleFieldOriented: 3-Pressed
Intake: 1


# Gunner Stick
ToggleClimbArm: RightBumper
ElevatorUp: Y
ElevatorDown: A
ClimbSlow: LeftBumper
ClimbFast: Back
IntakeUpOnly: B
ReverseBalls: Start
AimRight: B
AimLeft: X
AbortVisionAlign: Left
ShootLow: Right
ShootHighFar: Up
ShootLaunchpad: Down
CancelShooterRev: Start
151 changes: 89 additions & 62 deletions src/main/java/frc/robot/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SelectCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.ClimbArmSubsystem;
import frc.robot.subsystems.HoodSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
Expand Down Expand Up @@ -77,13 +76,8 @@ public void autoInit(int autoSelected) {
swerveDrive.setBrakes(true);

//Spin up the shooter, tip the climb arm, and set the shooter to auto mode
CommandScheduler.getInstance().schedule(
new SequentialCommandGroup(
new InstantCommand(m_climbArmSubsystem::armTilt, m_climbArmSubsystem),
new InstantCommand(m_shooterSubsystem::autonomousMode, m_shooterSubsystem),
new InstantCommand(m_shooterSubsystem::shooterLow, m_shooterSubsystem)
)
);
m_climbArmSubsystem.armTilt();
m_shooterSubsystem.shooterLow();

this.autoSelected = autoSelected;

Expand Down Expand Up @@ -144,6 +138,8 @@ public void autoPeriodic() {
}
}

m_shooterSubsystem.shooterPeriodic();

xVelocity = 0;
yVelocity = 0;
rotationVelocity = 0;
Expand Down Expand Up @@ -197,6 +193,7 @@ public void autoPeriodic() {
}
break;

//TODO: Eliminate use of timer-based logic in favor for lasershark trigger-based logic specifics are in 3 ball blue but would be implemented everywhere if successful, CPU impact tbd
//WAIT
case 1:
if (newCommand) {
Expand Down Expand Up @@ -228,11 +225,11 @@ public void autoPeriodic() {
new SelectCommand(
// Maps selector values to commands
Map.ofEntries(
Map.entry(0, new ParallelCommandGroup(new InstantCommand(m_shooterSubsystem::shooterLow), new InstantCommand(m_hoodSubsystem::hoodOut, m_hoodSubsystem))),
Map.entry(1, new ParallelCommandGroup(new InstantCommand(m_shooterSubsystem::shooterHighClose), new InstantCommand(m_hoodSubsystem::hoodIn, m_hoodSubsystem))),
Map.entry(2, new ParallelCommandGroup(new InstantCommand(m_shooterSubsystem::shooterHighFar), new InstantCommand(m_hoodSubsystem::hoodOut, m_hoodSubsystem))),
Map.entry(0, new ParallelCommandGroup(new InstantCommand(m_shooterSubsystem::shooterLow), new InstantCommand(m_hoodSubsystem::hoodIn, m_hoodSubsystem))),
Map.entry(1, new ParallelCommandGroup(new InstantCommand(m_shooterSubsystem::shooterAuto), new InstantCommand(m_hoodSubsystem::hoodIn, m_hoodSubsystem))),
Map.entry(2, new ParallelCommandGroup(new InstantCommand(m_shooterSubsystem::shooterAuto), new InstantCommand(m_hoodSubsystem::hoodOut, m_hoodSubsystem))),
Map.entry(-1, new ParallelCommandGroup(
new InstantCommand(() -> DriverStation.reportError("There is no shoot level of " + this.selectShooter((int)currentCommand.getArgument()), false)),
new InstantCommand(() -> DriverStation.reportError("There is no shoot level of " + currentCommand.getArgument(), false)),
new InstantCommand(m_shooterSubsystem::shooterOff, m_shooterSubsystem)
))),
() -> this.selectShooter((int)currentCommand.getArgument()))
Expand Down Expand Up @@ -265,17 +262,23 @@ public void autoPeriodic() {
}
}

SmartDashboard.putNumber("Expected X Velocity", xVelocity);
SmartDashboard.putNumber("Expected Y Velocity", yVelocity);
SmartDashboard.putNumber("Expected Rotation Velocity", rotationVelocity);
if (RobotMap.REPORTING_DIAGNOSTICS) {
SmartDashboard.putNumber("Expected X Velocity", xVelocity);
SmartDashboard.putNumber("Expected Y Velocity", yVelocity);
SmartDashboard.putNumber("Expected Rotation Velocity", rotationVelocity);
}

swerveDrive.setDefaultCommand(new RunCommand(() -> swerveDrive.setSwerveDrive(xVelocity,
-yVelocity, rotationVelocity), swerveDrive));
swerveDrive.setSwerveDrive(
xVelocity,
-yVelocity,
rotationVelocity
);

swerveDrive.swervePeriodic(true);
}

private int selectShooter(int shooterSelect) {
if(shooterSelect >= 0 && shooterSelect <= 3) {
if(shooterSelect >= 0 && shooterSelect <= 2) {
return shooterSelect;
}
else {
Expand Down Expand Up @@ -313,71 +316,65 @@ Example program (fairly short, but you get the idea):
Each of these are an entire auto program, executed from index 0 to the end of the array.
*/

//Three ball from Q2 (Preloaded, 4, 5) - IN PROGRESS
//Three ball from Q2 (Preloaded, 4, 5)
{
//Intake out
new AutoCommand(2),
//Travel to ball 5
autoPaths.getQuadrant2EdgeBall5(),
//Wait
new AutoCommand(1, 0.5),
//Intake in
new AutoCommand(3),
//Activate shooter
new AutoCommand(4, 1),
//Intake out
new AutoCommand(2),
//Travel to ball 5
autoPaths.getQuadrant2EdgeBall5(),
//TODO: Add trigger here for full mag
//Wait
new AutoCommand(1, 2),
//Activate magazine
new AutoCommand(6),
//TODO: Add trigger here for empty mag, maybe with timer if needed for last ball to exit shooter
//Wait
new AutoCommand(1, 3),
//Deactivate shooter
new AutoCommand(5),
//Activate shooter
new AutoCommand(4, 2),
//Go back to shooting position
autoPaths.getBall5Quadrant2Edge(),
//Activate magazine
new AutoCommand(6),
//Wait
new AutoCommand(1, 1.5),
//Deactivate shooter
new AutoCommand(5),
//Intake out
new AutoCommand(2),
new AutoCommand(4, 1),
//Travel to ball 4
autoPaths.getQuadrant2EdgeBall4(),
//Wait
new AutoCommand(1, 0.5),
//Intake in
new AutoCommand(3),
//Activate shooter
new AutoCommand(4, 2),
//Travel to shooting position
autoPaths.getBall4Quadrant2Shoot(),
//Activate magazine
new AutoCommand(6),
//Wait
new AutoCommand(1, 1.5),
//Deactivate shooter
new AutoCommand(5)
autoPaths.getBall5Ball4(),
//TODO: Add trigger here for full mag(not as necessary for this smaller delay)
//Wait
new AutoCommand(1, 0.5),
//Activate magazine
new AutoCommand(6),
//Wait
//TODO: Add trigger here for empty mag, maybe with timer if needed for last ball to reach second detector or to exit shooter
new AutoCommand(1, 1.5),
//Deactivate shooter
new AutoCommand(5),
//Travel to ball 13
autoPaths.getBall4Ball13()
},

//Two ball (high) from Q1 (Preloaded, 2) - DONE
//Two ball (high) from Q1 (Preloaded, 2)
{
//Intake out
new AutoCommand(2),
//Activate shooter
new AutoCommand(4, 1),
//Travel to ball 2
autoPaths.getQuadrant1LeftBall2(),
//Wait
new AutoCommand(1, 1),
//Intake in
new AutoCommand(3),
//Activate shooter
new AutoCommand(4, 2),
//Move to the line
autoPaths.getBall2Quadrant1Line(),
//Wait
new AutoCommand(1, 0.2),
//Activate magazine
new AutoCommand(6),
//Wait
new AutoCommand(1, 4),
//Deactivate shooter
new AutoCommand(5)
new AutoCommand(5),
//Travel to Launchpad
autoPaths.getBall2Launchpad()
},

//Two ball (low) from Q1 (Preloaded, 2) - IN PROGRESS
//Two ball (low) from Q1 (Preloaded, 2)
{
//Intake out
new AutoCommand(2),
Expand Down Expand Up @@ -435,6 +432,36 @@ Example program (fairly short, but you get the idea):
new AutoCommand(5)
},

//Three ball auto (RED)
{
//Activate shooter
new AutoCommand(4, 2),
//Intake out
new AutoCommand(2),
//Travel to ball 5
autoPaths.getQuadrant2EdgeBall5(),
//Wait
new AutoCommand(1, 2),
//Activate magazine
new AutoCommand(6),
//Wait
new AutoCommand(1, 3),
//Activate shooter
new AutoCommand(4, 2),
//Travel to ball 4
autoPaths.getBall5Ball4(),
//Wait
new AutoCommand(1, 0.5),
//Activate magazine
new AutoCommand(6),
//Wait
new AutoCommand(1, 1.5),
//Deactivate shooter
new AutoCommand(5),
//Travel to ball 13
autoPaths.getBall4Ball13RED()
},

//Do literally nothing
{
//THESE BRACKETS INTENTIONALLY LEFT BLANK
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/AutoCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,9 +96,10 @@ public AutoCommand(double rotationOffset, List<Translation2d> waypoints, double
double firstRotation = Math.atan2(secondPoint.getY() - firstPoint.getY(), secondPoint.getX() - firstPoint.getX());
double lastRotation = Math.atan2(lastPoint.getY() - secondLastPoint.getY(), lastPoint.getX() - secondLastPoint.getX());

//SmartDashboard.put
//Remove the first and last waypoints from the list, as we are going to manually specify their rotation
SmartDashboard.putString("Waypoints", waypoints.toString());
if (RobotMap.REPORTING_DIAGNOSTICS) {
SmartDashboard.putString("Waypoints", waypoints.toString());
}
waypoints.remove(0);
waypoints.remove(waypoints.size() - 1);

Expand Down
Loading

0 comments on commit 2e6fef6

Please sign in to comment.