Skip to content

Commit c6724c9

Browse files
TheExoneratedManiacTheExoneratedManiac
committed
worked on new auto
1 parent f9e6222 commit c6724c9

File tree

6 files changed

+106
-72
lines changed

6 files changed

+106
-72
lines changed

‎simgui.json

+4-4
Original file line numberDiff line numberDiff line change
@@ -305,7 +305,7 @@
305305
0.0,
306306
0.8500000238418579
307307
],
308-
"height": 0,
308+
"height": 43,
309309
"name": "Target Speed vs Actual",
310310
"series": [
311311
{
@@ -352,7 +352,7 @@
352352
0.0,
353353
0.8500000238418579
354354
],
355-
"height": 201,
355+
"height": 235,
356356
"series": [
357357
{
358358
"color": [
@@ -413,7 +413,7 @@
413413
0.0,
414414
0.8500000238418579
415415
],
416-
"height": 346,
416+
"height": 184,
417417
"series": [
418418
{
419419
"color": [
@@ -452,7 +452,7 @@
452452
0.0,
453453
0.8500000238418579
454454
],
455-
"height": 346,
455+
"height": 184,
456456
"series": [
457457
{
458458
"color": [

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

+2
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,8 @@ public static final class DriveConstants {
7979
public static final int frontRightTurningCanId = 10;
8080
public static final int rearRightTurningCanId = 25;
8181

82+
public static final double drivingSpeed = 1;
83+
8284
public static final boolean gyroReversed = false;
8385
}
8486

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

+28-27
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,8 @@ enum Notes {
7171
DRIVE,
7272
NEXT2STAGE,
7373
NEXT2AMP,
74-
GOFORWARDS
74+
GOFORWARDS,
75+
CENTERSHOOTDRIVE
7576
}
7677

7778
private final SendableChooser<Positions> startingPositionChooser = new SendableChooser<>();
@@ -93,7 +94,7 @@ public void robotInit() {
9394
AHRS ahrs = new AHRS(SerialPort.Port.kMXP);
9495

9596
var driverCamera = CameraServer.startAutomaticCapture();
96-
driverCamera.setPixelFormat(PixelFormat.kYUYV);
97+
//driverCamera.setPixelFormat(PixelFormat.kYUYV);
9798
driverCamera.setResolution(1280, 720);
9899

99100
if (Robot.isSimulation()) {
@@ -140,6 +141,7 @@ public void robotInit() {
140141
noteChooser1.addOption("Next 2 Amp", Notes.NEXT2AMP);
141142
noteChooser1.addOption("Next 2 Stage", Notes.NEXT2STAGE);
142143
noteChooser1.addOption("Go Forwards", Notes.GOFORWARDS);
144+
noteChooser1.addOption("Shoot note then drive", Notes.CENTERSHOOTDRIVE);
143145
SmartDashboard.putData("note chooser 1", noteChooser1);
144146

145147
noteChooser2.setDefaultOption("be useless", Notes.NOTHING);
@@ -164,9 +166,9 @@ public void robotInit() {
164166
* {@link JoystickButton}.
165167
*/
166168
private void configureButtonBindings() {
167-
/* new JoystickButton(driverController, PS4Controller.Button.kCross.value)
168-
.and(DriverStation::isTeleop)
169-
.whileTrue(drive.setXCommand());*/
169+
/* new JoystickButton(driverController, PS4Controller.Button.kCross.value)
170+
.and(DriverStation::isTeleop)
171+
.whileTrue(drive.setXCommand());*/
170172
new JoystickButton(driverController, PS4Controller.Button.kL1.value)
171173
.whileTrue(shooter.manualShootCommand().deadlineWith(leds.rainbowFlagScroll()));
172174
new JoystickButton(driverController, PS4Controller.Button.kTriangle.value)
@@ -187,8 +189,12 @@ private void configureButtonBindings() {
187189
.whileTrue(intake.spinReverseCommand());
188190
new JoystickButton(driverController, PS4Controller.Button.kR3.value)
189191
.whileTrue(shooter.ampCommand());
190-
new JoystickButton(driverController, PS4Controller.Button.kR1.value).whileTrue(climber.climbCommand()).onFalse(climber.stopClimbCommand());
191-
new JoystickButton(driverController, PS4Controller.Button.kCross.value).whileTrue(climber.reverseClimbCommand()).onFalse(climber.stopClimbCommand());
192+
new JoystickButton(driverController, PS4Controller.Button.kR1.value)
193+
.whileTrue(climber.climbCommand())
194+
.onFalse(climber.stopClimbCommand());
195+
new JoystickButton(driverController, PS4Controller.Button.kCross.value)
196+
.whileTrue(climber.reverseClimbCommand())
197+
.onFalse(climber.stopClimbCommand());
192198
}
193199

194200
private void configureAutomaticBindings() {
@@ -203,9 +209,7 @@ private void configureAutomaticBindings() {
203209
public Command getAutonomousCommand() {
204210
return switch (startingPositionChooser.getSelected()) {
205211
case AMP -> {
206-
Command auto =
207-
shooter
208-
.autoShootCommand1().andThen(shootAndIntermediary());
212+
Command auto = shooter.autoShootCommand1().andThen(shootAndIntermediary());
209213
boolean done = false;
210214
switch (noteChooser1.getSelected()) {
211215
case AMP -> auto = auto.andThen(followPathAndShoot("amp 3 p1"));
@@ -254,9 +258,7 @@ public Command getAutonomousCommand() {
254258
yield auto;
255259
}
256260
case CENTER -> {
257-
Command auto =
258-
shooter
259-
.autoShootCommand1().andThen(shootAndIntermediary());
261+
Command auto = shooter.autoShootCommand1().andThen(shootAndIntermediary());
260262
boolean done = false;
261263
switch (noteChooser1.getSelected()) {
262264
case CENTER -> auto = auto.andThen(followPathAndShoot("center 3 p1"));
@@ -268,6 +270,7 @@ public Command getAutonomousCommand() {
268270
case NOTHING -> {
269271
done = true;
270272
}
273+
case CENTERSHOOTDRIVE -> auto = shootThenDrive();
271274
default -> {
272275
done = true;
273276
}
@@ -302,9 +305,7 @@ public Command getAutonomousCommand() {
302305
yield auto;
303306
}
304307
case STAGE -> {
305-
Command auto =
306-
shooter
307-
.autoShootCommand1().andThen(shootAndIntermediary());
308+
Command auto = shooter.autoShootCommand1().andThen(shootAndIntermediary());
308309
boolean done = false;
309310
switch (noteChooser1.getSelected()) {
310311
case AMP -> auto = auto.andThen(followPathAndShoot("stage 3 p1"));
@@ -357,12 +358,9 @@ public Command getAutonomousCommand() {
357358
}
358359
yield auto;
359360
}
360-
361+
361362
case NEXT2AMP -> {
362-
Command auto =
363-
shooter
364-
.autoShootCommand1()
365-
.andThen(shootAndIntermediary());
363+
Command auto = shooter.autoShootCommand1().andThen(shootAndIntermediary());
366364
boolean done = false;
367365
switch (noteChooser1.getSelected()) {
368366
case NOTHING -> {
@@ -378,10 +376,7 @@ public Command getAutonomousCommand() {
378376
yield auto;
379377
}
380378
case NEXT2STAGE -> {
381-
Command auto =
382-
shooter
383-
.autoShootCommand1()
384-
.andThen(shootAndIntermediary());
379+
Command auto = shooter.autoShootCommand1().andThen(shootAndIntermediary());
385380
boolean done = false;
386381
switch (noteChooser1.getSelected()) {
387382
case NOTHING -> {
@@ -416,6 +411,14 @@ private ParallelCommandGroup shootAndIntermediary() {
416411
return intermediary.intermediaryCommand().alongWith(shooter.autoShootCommand2());
417412
}
418413

414+
private SequentialCommandGroup shootThenDrive() {
415+
return drive
416+
.pointForward()
417+
.andThen(drive.autoDriveCommand())
418+
.withTimeout(1)
419+
.andThen(drive.setXCommand());
420+
}
421+
419422
/**
420423
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
421424
* that you want ran during disabled, autonomous, teleoperated and test.
@@ -480,8 +483,6 @@ public void teleopInit() {
480483
leds.greenPurpleScroll().schedule();
481484
}
482485

483-
484-
485486
/** This function is called periodically during operator control. */
486487
@Override
487488
public void teleopPeriodic() {}

‎src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

+26-26
Original file line numberDiff line numberDiff line change
@@ -2,42 +2,42 @@
22

33
import com.revrobotics.CANSparkLowLevel.MotorType;
44
import com.revrobotics.CANSparkMax;
5-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
65
import edu.wpi.first.wpilibj2.command.Command;
76
import edu.wpi.first.wpilibj2.command.SubsystemBase;
87
import frc.robot.Constants;
98

10-
public class ClimberSubsystem extends SubsystemBase{
11-
private final CANSparkMax leftSpark =
9+
public class ClimberSubsystem extends SubsystemBase {
10+
private final CANSparkMax leftSpark =
1211
new CANSparkMax(Constants.ClimberConstants.climberLeftSpark, MotorType.kBrushless);
1312

14-
private final CANSparkMax rightSpark = new CANSparkMax(Constants.ClimberConstants.climberRightSpark, MotorType.kBrushless);
13+
private final CANSparkMax rightSpark =
14+
new CANSparkMax(Constants.ClimberConstants.climberRightSpark, MotorType.kBrushless);
1515

16-
public void startClimb() {
17-
// spark.setVoltage(SmartDashboard.getNumber("Spin voltage", 0));
18-
rightSpark.set(Constants.ClimberConstants.climberSpeed);
19-
leftSpark.set(Constants.ClimberConstants.reverseClimberSpeed);
20-
}
16+
public void startClimb() {
17+
// spark.setVoltage(SmartDashboard.getNumber("Spin voltage", 0));
18+
rightSpark.set(Constants.ClimberConstants.climberSpeed);
19+
leftSpark.set(Constants.ClimberConstants.reverseClimberSpeed);
20+
}
2121

22-
public void reverseClimb() {
23-
rightSpark.set(Constants.ClimberConstants.reverseClimberSpeed);
24-
leftSpark.set(Constants.ClimberConstants.climberSpeed);
25-
}
22+
public void reverseClimb() {
23+
rightSpark.set(Constants.ClimberConstants.reverseClimberSpeed);
24+
leftSpark.set(Constants.ClimberConstants.climberSpeed);
25+
}
2626

27-
public void stopClimb() {
28-
leftSpark.set(0);
29-
rightSpark.set(0);
30-
}
27+
public void stopClimb() {
28+
leftSpark.set(0);
29+
rightSpark.set(0);
30+
}
3131

32-
public Command climbCommand() {
33-
return run(this::startClimb).finallyDo(interrupted -> stopClimb());
34-
}
32+
public Command climbCommand() {
33+
return run(this::startClimb).finallyDo(interrupted -> stopClimb());
34+
}
3535

36-
public Command reverseClimbCommand() {
37-
return run(this::reverseClimb).finallyDo(interrupted -> stopClimb());
38-
}
36+
public Command reverseClimbCommand() {
37+
return run(this::reverseClimb).finallyDo(interrupted -> stopClimb());
38+
}
3939

40-
public Command stopClimbCommand() {
41-
return runOnce(this::stopClimb);
42-
}
40+
public Command stopClimbCommand() {
41+
return runOnce(this::stopClimb);
42+
}
4343
}

‎src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

+29-1
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,9 @@
1414
import static edu.wpi.first.units.Units.Volts;
1515

1616
import com.choreo.lib.Choreo;
17+
import com.revrobotics.CANSparkLowLevel.MotorType;
18+
import com.revrobotics.CANSparkMax;
19+
1720
import edu.wpi.first.math.MathUtil;
1821
import edu.wpi.first.math.VecBuilder;
1922
import edu.wpi.first.math.controller.PIDController;
@@ -137,7 +140,8 @@ public void drive(
137140
Measure<Velocity<Distance>> xSpeed,
138141
Measure<Velocity<Distance>> ySpeed,
139142
Measure<Velocity<Angle>> rot,
140-
ReferenceFrame orientation) {
143+
ReferenceFrame orientation)
144+
{
141145
var speeds =
142146
switch (orientation) {
143147
case ROBOT -> new ChassisSpeeds(xSpeed, ySpeed, rot);
@@ -146,6 +150,7 @@ public void drive(
146150
drive(speeds);
147151
}
148152

153+
149154
/**
150155
* Drives the robot with the given chassis speeds. Note that chassis speeds are relative to the
151156
* robot's reference frame.
@@ -356,4 +361,27 @@ public void setForward() {
356361
new SwerveModuleState(0, Rotation2d.fromDegrees(0))
357362
});
358363
}
364+
365+
private final CANSparkMax rearLeftDrivingSpark =
366+
new CANSparkMax(Constants.DriveConstants.rearLeftDrivingCanId, MotorType.kBrushless);
367+
private final CANSparkMax rearRightDrivingSpark =
368+
new CANSparkMax(Constants.DriveConstants.rearRightDrivingCanId, MotorType.kBrushless);
369+
private final CANSparkMax frontRightDrivingSpark =
370+
new CANSparkMax(Constants.DriveConstants.frontRightDrivingCanId, MotorType.kBrushless);
371+
private final CANSparkMax frontLeftDrivingSpark =
372+
new CANSparkMax(Constants.DriveConstants.frontLeftDrivingCanId, MotorType.kBrushless);
373+
374+
public void autoDrive() {
375+
// spark.setVoltage(SmartDashboard.getNumber("Spin voltage", 0));
376+
rearLeftDrivingSpark.set(Constants.DriveConstants.drivingSpeed);
377+
rearRightDrivingSpark.set(Constants.DriveConstants.drivingSpeed);
378+
frontLeftDrivingSpark.set(Constants.DriveConstants.drivingSpeed);
379+
frontRightDrivingSpark.set(Constants.DriveConstants.drivingSpeed);
380+
}
381+
382+
public Command autoDriveCommand() {
383+
return run(this::autoDrive);
384+
}
385+
359386
}
387+

‎src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java

+17-14
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ public LEDSubsystem() {
3939
leftUpperDiagonal = new AddressableLEDBufferView(ledData, 43, 64);
4040
leftLowerDiagonal = new AddressableLEDBufferView(ledData, 20, 42).reversed();
4141
leftVertical = new AddressableLEDBufferView(ledData, 0, 19);
42-
across = new AddressableLEDBufferView(ledData,65 , 87);
42+
across = new AddressableLEDBufferView(ledData, 65, 87);
4343
}
4444

4545
@Override
@@ -53,14 +53,15 @@ public Command runAnimation(Animation animation) {
5353

5454
public Command runSpecialAnimation(Animation animation) {
5555
return run(() -> {
56-
animation.update(rightUpperDiagonal);
57-
animation.update(rightLowerDiagonal);
58-
animation.update(rightVertical);
59-
animation.update(leftUpperDiagonal);
60-
animation.update(leftLowerDiagonal);
61-
animation.update(leftVertical);
62-
animation.update(across);
63-
}).ignoringDisable(true);
56+
animation.update(rightUpperDiagonal);
57+
animation.update(rightLowerDiagonal);
58+
animation.update(rightVertical);
59+
animation.update(leftUpperDiagonal);
60+
animation.update(leftLowerDiagonal);
61+
animation.update(leftVertical);
62+
animation.update(across);
63+
})
64+
.ignoringDisable(true);
6465
}
6566

6667
public Command blinkRed() {
@@ -84,7 +85,9 @@ public Command blinkYellow() {
8485
}
8586

8687
public Command greenPurpleGradient() {
87-
return runSpecialAnimation(Animation.gradient(Color.kGreen, Color.kPurple).scrollAtRelativeSpeed(Percent.per(Second).of(30)));
88+
return runSpecialAnimation(
89+
Animation.gradient(Color.kGreen, Color.kPurple)
90+
.scrollAtRelativeSpeed(Percent.per(Second).of(30)));
8891
}
8992

9093
private static final Animation rainbowFlag =
@@ -117,13 +120,13 @@ public Command rainbowFlagScroll() {
117120

118121
private static final Animation redBlue =
119122
Animation.steps(
120-
Map.of(
121-
0.0 / 2, Color.kRed,
122-
1.0 / 4, Color.kBlue));
123+
Map.of(
124+
0.0 / 2, Color.kRed,
125+
1.0 / 4, Color.kBlue));
123126

124127
private static final Animation greenPurpleScroll =
125128
greenPurple.scrollAtRelativeSpeed(Percent.per(Second).of(50));
126-
129+
127130
private static final Animation redBlueScroll =
128131
redBlue.scrollAtRelativeSpeed(Percent.per(Second).of(50));
129132

0 commit comments

Comments
 (0)