Skip to content

Commit 096dd30

Browse files
JoshuaANJoshuaAN
authored
Merge pull request #22 from TripleHelixProgramming/CompTesting
Comp testing
2 parents f3749d3 + b61378a commit 096dd30

File tree

12 files changed

+606
-521
lines changed

12 files changed

+606
-521
lines changed

‎src/main/java/frc/paths/FourBallPartThree.java

Lines changed: 201 additions & 201 deletions
Large diffs are not rendered by default.

‎src/main/java/frc/paths/FourBallPartTwo.java

Lines changed: 301 additions & 301 deletions
Large diffs are not rendered by default.

‎src/main/java/frc/robot/Robot.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
import frc.robot.status.commands.IdleCommand;
2222
import frc.robot.status.commands.SetColor;
2323
import frc.robot.status.groups.LEDDemoCG;
24+
import frc.robot.vision.commands.TurnOffLEDs;
2425

2526
/**
2627
* The VM is configured to automatically run this class, and to call the
@@ -112,6 +113,9 @@ public void disabledPeriodic() {
112113
mRobotContainer.configureButtonBindings();
113114
}
114115
}
116+
117+
mRobotContainer.disableLights();
118+
mRobotContainer.displaySwitch();
115119
}
116120

117121
/**

‎src/main/java/frc/robot/RobotContainer.java

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,11 @@ public class RobotContainer {
9292
private final Status mStatus = Status.getInstance();
9393
private final Climber mClimber = new Climber();
9494

95+
private final DigitalInput fiveBallAuto = new DigitalInput(3);
96+
private final DigitalInput twoBallSouthAuto = new DigitalInput(1);
97+
private final DigitalInput twoBallEastAuto = new DigitalInput(2);
98+
private final DigitalInput fourBallAuto = new DigitalInput(4);
99+
95100
/*
96101
* private final Indexer mIndexer = new Indexer();
97102
* private final PowerDistribution mPDP = new PowerDistribution(
@@ -132,11 +137,6 @@ public Command getAutonomousCommand() {
132137
// return new TrajectoryFollower(mDrive, new CollectSecondBall());
133138
Command autoCommand = null;
134139

135-
DigitalInput fiveBallAuto = new DigitalInput(0);
136-
DigitalInput twoBallSouthAuto = new DigitalInput(1);
137-
DigitalInput twoBallEastAuto = new DigitalInput(2);
138-
DigitalInput fourBallAuto = new DigitalInput(4);
139-
140140
try {
141141
if(!fiveBallAuto.get()){
142142
autoCommand = new FiveBallAuto(mDrive, mIntake, mShooter);
@@ -156,6 +156,25 @@ public Command getAutonomousCommand() {
156156
return autoCommand;
157157
}
158158

159+
public void displaySwitch() {
160+
// try {
161+
// if(!fiveBallAuto.get()){
162+
// SmartDashboard.putNumber("DIO", 3);
163+
// } else if (!twoBallSouthAuto.get()) {
164+
// SmartDashboard.putNumber("DIO", 1);
165+
// } else if (!twoBallEastAuto.get()) {
166+
// SmartDashboard.putNumber("DIO", 2);
167+
// } else if (!fourBallAuto.get()) {
168+
// SmartDashboard.putNumber("DIO", 4);
169+
// } else {
170+
// SmartDashboard.putNumber("DIO", -1);
171+
// }
172+
// } catch (Exception e) {
173+
174+
// }
175+
176+
}
177+
159178
public void setLEDs() {
160179
new SetColor(mStatus, Color.kCornsilk).schedule();
161180
}
@@ -303,12 +322,12 @@ public void configureButtonBindings() {
303322
.alongWith(new XBoxButtonCommand(X_BOX_B))); // baseline, upper goal, rear shot
304323

305324
xBoxB.whenReleased(new StopShooter(mShooter)
306-
.alongWith(new TurnOnLEDs(mLimelight))
325+
.alongWith(new TurnOffLEDs(mLimelight))
307326
.alongWith(new IdleCommand()));
308327

309328
JoystickButton xBoxX = new JoystickButton(operator, X_BOX_X);
310329
xBoxX.whenHeld(new PresetFlywheelController(mShooter, "TLR")
311-
.alongWith(new TurnOnLEDs(mLimelight))
330+
// .alongWith(new TurnOnLEDs(mLimelight))
312331
.alongWith(new XBoxButtonCommand(X_BOX_X))); // tarmac, lower goal, rear shot
313332

314333
xBoxX.whenReleased(new StopShooter(mShooter)
@@ -317,7 +336,7 @@ public void configureButtonBindings() {
317336

318337
JoystickButton xBoxY = new JoystickButton(operator, X_BOX_Y);
319338
xBoxY.whenHeld(new PresetFlywheelController(mShooter, "TUR")
320-
.alongWith(new TurnOnLEDs(mLimelight))
339+
// .alongWith(new TurnOnLEDs(mLimelight))
321340
.alongWith(new XBoxButtonCommand(X_BOX_Y))); // tarmac, upper goal, rear shot
322341

323342
xBoxY.whenReleased(new StopShooter(mShooter)

‎src/main/java/frc/robot/auto/groups/FiveBallAuto.java

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
import frc.robot.shooter.Shooter;
2727
import frc.robot.shooter.commands.FlywheelController;
2828
import frc.robot.shooter.commands.PullTrigger;
29+
import frc.robot.shooter.commands.ResetEncoder;
2930
import frc.robot.shooter.commands.ResetHood;
3031
import frc.robot.shooter.commands.StopShooter;
3132
import frc.robot.shooter.commands.StopTrigger;
@@ -38,12 +39,13 @@ public class FiveBallAuto extends SequentialCommandGroup {
3839
public FiveBallAuto(Drivetrain drive, Intake intake, Shooter shooter) {
3940
addCommands(
4041
new ResetOdometry(drive, new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(-90.0))),
42+
new ResetEncoder(shooter),
4143
new ParallelDeadlineGroup(
4244
new SequentialCommandGroup(
4345
new WaitCommand(1.0), // Give shooter time to spin up & hood to move
4446
new PullTrigger(shooter),
4547
new WaitCommand(0.5)),
46-
new ActionCommand(new ImageAction("THfade.png", 0.01).brightness(0.7)),
48+
// new ActionCommand(new ImageAction("THfade.png", 0.01).brightness(0.7)),
4749
new TrajectoryFollower(drive, new FiveBallPartOne()), // Turn to point at center
4850
new FlywheelController(shooter, 1810, 77.90)),
4951
new ParallelDeadlineGroup(

‎src/main/java/frc/robot/auto/groups/FourBallAuto.java

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
import frc.robot.shooter.Shooter;
2424
import frc.robot.shooter.commands.FlywheelController;
2525
import frc.robot.shooter.commands.PullTrigger;
26+
import frc.robot.shooter.commands.ResetEncoder;
2627
import frc.robot.shooter.commands.ResetHood;
2728
import frc.robot.shooter.commands.StopShooter;
2829
import frc.robot.shooter.commands.StopTrigger;
@@ -35,11 +36,12 @@ public class FourBallAuto extends SequentialCommandGroup{
3536

3637
public FourBallAuto(Drivetrain drive, Intake intake, Shooter shooter) {
3738
addCommands(
38-
new ActionCommand(new ImageAction("fade.png",0.02).oscillate().brightness(0.7)),
39+
// new ActionCommand(new ImageAction("fade.png",0.02).oscillate().brightness(0.7)),
3940
new ResetOdometry(drive, new Pose2d(new Translation2d(0,0),Rotation2d.fromDegrees(-90))),
41+
new ResetEncoder(shooter),
4042
new ParallelDeadlineGroup(
4143
new WaitCommand(4.25),
42-
new ResetHood(shooter),
44+
// new ResetHood(shooter),
4345
new SequentialCommandGroup(
4446
new WaitCommand(1.75),
4547
new FlywheelController(shooter, 1800, 78.25)),

‎src/main/java/frc/robot/shooter/Shooter.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,10 @@ public void resetHoodAngle() {
112112
setHoodPosition(ShooterConstants.kHoodMinAngle);
113113
}
114114

115+
public void resetHoodEncoder() {
116+
hoodEncoder.setPosition(ShooterConstants.kHoodMinAngle);
117+
}
118+
115119
public boolean checkHoodCurrentLimit() {
116120
if ( ShooterConstants.kHoodStopCurrentLimit < hoodMotor.getOutputCurrent()) {
117121
highCurrentCount++;

‎src/main/java/frc/robot/shooter/commands/ResetEncoder.java

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.shooter.commands;
6+
7+
import edu.wpi.first.wpilibj2.command.CommandBase;
8+
import frc.robot.shooter.Shooter;
9+
10+
public class ResetEncoder extends CommandBase {
11+
Shooter shooter;
12+
13+
public ResetEncoder(Shooter shooter) {
14+
this.shooter = shooter;
15+
}
16+
17+
// Called when the command is initially scheduled.
18+
@Override
19+
public void initialize() {
20+
shooter.resetHoodEncoder();
21+
}
22+
23+
@Override
24+
public boolean isFinished() {
25+
return true;
26+
}
27+
}

‎src/main/java/frc/robot/vision/Limelight.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,11 @@ public boolean hasTarget() {
3434
}
3535

3636
public void turnOffLEDs() {
37-
getDefault().getTable("limelight").getEntry("ledMode").setDouble(1.0);
37+
getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
3838
}
3939

4040
public void turnOnLEDs() {
41-
getDefault().getTable("limelight").getEntry("ledMode").setDouble(3.0);
41+
getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
4242
}
4343

4444
public static class VisionState {

‎src/main/java/frc/robot/vision/commands/TurnOffLEDs.java

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,14 @@
88
import frc.robot.vision.Limelight;
99

1010
public class TurnOffLEDs extends CommandBase {
11+
Limelight limelight;
12+
1113
public TurnOffLEDs(Limelight limelight) {
14+
this.limelight = limelight;
15+
}
16+
17+
@Override
18+
public void initialize() {
1219
limelight.turnOffLEDs();
1320
}
1421

0 commit comments

Comments
 (0)