Skip to content

Commit 0bc0052

Browse files
AyushSagar16AyushSagar16
committed
Autonomous (will fix later)
1 parent de55522 commit 0bc0052

File tree

4 files changed

+102
-66
lines changed

4 files changed

+102
-66
lines changed

‎src/main/java/org/ghrobotics/frc2024/Robot.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -162,9 +162,9 @@ private void setupTeleopControls() {
162162

163163
// driver_controller_.pov(90).whileTrue(superstructure_.setShooter(90));
164164

165-
// Useful for shooting while moving
166-
driver_controller_.b().whileTrue(
167-
superstructure_.autoArm(SmartDashboard.getNumber("Shooting Angle", 2)));
165+
driver_controller_.pov(90).whileTrue(superstructure_.setArmPercent(0.15));
166+
167+
driver_controller_.pov(270).whileTrue(superstructure_.setArmPercent(-0.15));
168168

169169

170170
// Operator Control

‎src/main/java/org/ghrobotics/frc2024/Superstructure.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -184,4 +184,5 @@ public enum Position {
184184
this.posname = name;
185185
}
186186
}
187+
187188
}

‎src/main/java/org/ghrobotics/frc2024/auto/AutoSelector.java‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,8 @@ public class AutoSelector {
5656

5757
PathPlannerPath left_one_intake_path = PathPlannerPath.fromPathFile("left_one_intake");
5858

59+
Command autonomous_command_;
60+
5961
Trigger trigger_ = new Trigger(() -> true);
6062

6163
public AutoSelector(Drive drive, RobotState robot_state, Superstructure superstructure, Arm arm, Intake intake, Shooter shooter, Feeder feeder) {
@@ -67,7 +69,7 @@ public AutoSelector(Drive drive, RobotState robot_state, Superstructure superstr
6769
shooter_ = shooter;
6870
feeder_ = feeder;
6971

70-
routine_chooser_ = new SendableChooser<Command>();
72+
7173

7274

7375
stop_all_motor = new ParallelCommandGroup(
@@ -99,9 +101,12 @@ public AutoSelector(Drive drive, RobotState robot_state, Superstructure superstr
99101
drive_
100102
);
101103

104+
routine_chooser_ = AutoBuilder.buildAutoChooser();
102105
routine_chooser_.setDefaultOption("Four Note Auto", fourNoteFull());
103106
}
104107

108+
public Command run()
109+
105110
public Command getSelectedRoutine() {
106111
return routine_chooser_.getSelected();
107112
}
@@ -337,6 +342,12 @@ public Pose2d getStartingPose() {
337342
public PathPlannerPath getPath() {
338343
return left_one_intake_path;
339344
}
345+
346+
public enum Routine {
347+
FOUR_NOTE_AUTO,
348+
THREE_NOTE_AUTO,
349+
ONE_NOTE_TAXI_AUTO
350+
}
340351

341352

342353
public static class Constants {

‎src/main/java/org/ghrobotics/frc2024/auto/FourNoteAuto.java‎

Lines changed: 86 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -1,70 +1,94 @@
1-
// package org.ghrobotics.frc2024.auto;
1+
package org.ghrobotics.frc2024.auto;
22

3-
// import org.ghrobotics.frc2024.RobotState;
4-
// import org.ghrobotics.frc2024.Superstructure;
5-
// import org.ghrobotics.frc2024.subsystems.Arm;
6-
// import org.ghrobotics.frc2024.subsystems.Drive;
7-
// import org.ghrobotics.frc2024.subsystems.Feeder;
8-
// import org.ghrobotics.frc2024.subsystems.Intake;
9-
// import org.ghrobotics.frc2024.subsystems.Shooter;
3+
import org.ghrobotics.frc2024.RobotState;
4+
import org.ghrobotics.frc2024.Superstructure;
5+
import org.ghrobotics.frc2024.commands.ArmPID;
6+
import org.ghrobotics.frc2024.subsystems.Arm;
7+
import org.ghrobotics.frc2024.subsystems.Drive;
8+
import org.ghrobotics.frc2024.subsystems.Feeder;
9+
import org.ghrobotics.frc2024.subsystems.Intake;
10+
import org.ghrobotics.frc2024.subsystems.Shooter;
1011

11-
// import com.pathplanner.lib.path.PathPlannerPath;
12+
import com.pathplanner.lib.auto.AutoBuilder;
13+
import com.pathplanner.lib.path.PathPlannerPath;
1214

13-
// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
15+
import edu.wpi.first.wpilibj2.command.InstantCommand;
16+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
17+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
18+
import edu.wpi.first.wpilibj2.command.WaitCommand;
1419

15-
// public class FourNoteAuto extends SequentialCommandGroup{
20+
public class FourNoteAuto extends SequentialCommandGroup{
1621

17-
// PathPlannerPath middle_middle_intake_path = PathPlannerPath.fromPathFile("middle_middle_intake");
18-
// PathPlannerPath middle_middle_shoot_path = PathPlannerPath.fromPathFile("middle_middle_shoot");
19-
// PathPlannerPath middle_right_intake_path = PathPlannerPath.fromPathFile("middle_right_intake");
20-
// PathPlannerPath middle_left_intake_path = PathPlannerPath.fromPathFile("middle_left_intake");
21-
// PathPlannerPath left_shoot_path = PathPlannerPath.fromPathFile("left_shoot");
22+
PathPlannerPath middle_middle_intake_path = PathPlannerPath.fromPathFile("middle_middle_intake");
23+
PathPlannerPath middle_middle_shoot_path = PathPlannerPath.fromPathFile("middle_middle_shoot");
24+
PathPlannerPath middle_right_intake_path = PathPlannerPath.fromPathFile("middle_right_intake");
25+
PathPlannerPath middle_left_intake_path = PathPlannerPath.fromPathFile("middle_left_intake");
26+
PathPlannerPath left_shoot_path = PathPlannerPath.fromPathFile("left_shoot");
2227

23-
// // Four Note Auto Path
24-
// PathPlannerPath left_intake_path = PathPlannerPath.fromPathFile("left_intake");
25-
// PathPlannerPath middle_intake_path = PathPlannerPath.fromPathFile("middle_intake");
26-
// PathPlannerPath right_intake_path = PathPlannerPath.fromPathFile("right_intake");
28+
// Four Note Auto Path
29+
PathPlannerPath left_intake_path = PathPlannerPath.fromPathFile("left_intake");
30+
PathPlannerPath middle_intake_path = PathPlannerPath.fromPathFile("middle_intake");
31+
PathPlannerPath right_intake_path = PathPlannerPath.fromPathFile("right_intake");
2732

28-
// PathPlannerPath left_one_intake_path = PathPlannerPath.fromPathFile("left_one_intake");
33+
PathPlannerPath left_one_intake_path = PathPlannerPath.fromPathFile("left_one_intake");
2934

30-
// // Constructor
31-
// public FourNoteAuto(Drive drive, RobotState robot_state, Superstructure superstructure, Arm arm, Intake intake, Shooter shooter, Feeder feeder){
32-
// addCommands(
33-
// // Part 1
34-
// new ParallelCommandGroup(
35-
// // Rev shooter, follow path to intake
36-
// new InstantCommand(() -> shooter.setPercent(0.75)),
37-
// AutoBuilder.followPath(left_intake_path),
38-
// new SequentialCommandGroup(
39-
// new ArmPID(arm, 25),
40-
// new WaitCommand(0.5),
41-
// new InstantCommand(() -> intake.setPercent(0.55)),
42-
// new InstantCommand(() -> feeder.setPercent(0.5)),
43-
// new WaitCommand(0.2),
44-
// new ArmPID(arm, 2),
45-
// new InstantCommand(() -> intake.setPercent(0.55)),
46-
// new InstantCommand(() -> feeder.stopMotor())
47-
// )
48-
// ),
49-
// // Part 2
50-
// new ParallelCommandGroup(
51-
// // Rev shooter, follow path to shoot
52-
// // new InstantCommand(() -> shooter_.setPercent(-0.75)),
53-
// AutoBuilder.followPath(middle_intake_path),
54-
// new SequentialCommandGroup(
55-
// new ArmPID(arm_, 29),
56-
// new WaitCommand(0.5),
57-
// new InstantCommand(() -> intake_.setPercent(0.55)),
58-
// new InstantCommand(() -> feeder_.setPercent(0.5)),
59-
// new WaitCommand(0.5),
60-
// new ArmPID(arm_, 2),
61-
// new InstantCommand(() -> intake_.setPercent(0.55)),
62-
// new InstantCommand(() -> feeder_.stopMotor())
63-
// )
64-
// );
65-
66-
67-
// );
68-
69-
// }
70-
// }
35+
// Constructor
36+
public FourNoteAuto(Drive drive, RobotState robot_state, Superstructure superstructure, Arm arm, Intake intake, Shooter shooter, Feeder feeder) {
37+
addCommands(
38+
// Part 1
39+
new ParallelCommandGroup(
40+
// Rev shooter, follow path to intake
41+
new InstantCommand(() -> shooter.setPercent(0.75)),
42+
AutoBuilder.followPath(left_intake_path),
43+
new SequentialCommandGroup(
44+
new ArmPID(arm, 25),
45+
new WaitCommand(0.5),
46+
new InstantCommand(() -> intake.setPercent(0.55)),
47+
new InstantCommand(() -> feeder.setPercent(0.5)),
48+
new WaitCommand(0.2),
49+
new ArmPID(arm, 2),
50+
new InstantCommand(() -> intake.setPercent(0.55)),
51+
new InstantCommand(() -> feeder.stopMotor())
52+
)
53+
),
54+
// Part 2
55+
new ParallelCommandGroup(
56+
// Rev shooter, follow path to shoot
57+
// new InstantCommand(() -> shooter_.setPercent(-0.75)),
58+
AutoBuilder.followPath(middle_intake_path),
59+
new SequentialCommandGroup(
60+
new ArmPID(arm, 29),
61+
new WaitCommand(0.5),
62+
new InstantCommand(() -> intake.setPercent(0.55)),
63+
new InstantCommand(() -> feeder.setPercent(0.5)),
64+
new WaitCommand(0.5),
65+
new ArmPID(arm, 2),
66+
new InstantCommand(() -> intake.setPercent(0.55)),
67+
new InstantCommand(() -> feeder.stopMotor())
68+
)
69+
),
70+
// Part 3
71+
new ParallelCommandGroup(
72+
AutoBuilder.followPath(right_intake_path),
73+
new SequentialCommandGroup(
74+
new ArmPID(arm, 29),
75+
new WaitCommand(0.5),
76+
new InstantCommand(() -> feeder.setPercent(0.5)),
77+
new InstantCommand(() -> intake.setPercent(0.55)),
78+
new WaitCommand(0.5),
79+
new ArmPID(arm, 2).withTimeout(1.5),
80+
new InstantCommand(() -> feeder.stopMotor()),
81+
new InstantCommand(() -> intake.setPercent(0.55))
82+
83+
)
84+
),
85+
// Part 4
86+
new ArmPID(arm, 32.85),
87+
new WaitCommand(0.2),
88+
new InstantCommand(() -> feeder.setPercent(0.75)),
89+
new InstantCommand(() -> intake.setPercent(0.5)),
90+
new WaitCommand(1.0),
91+
new ArmPID(arm, 2)
92+
);
93+
}
94+
}

0 commit comments

Comments
 (0)