Skip to content

Commit 84a4a02

Browse files
Professor348Professor348
authored
Merge pull request #9 from LOAD-Robotics/development
Development
2 parents a1d5d6d + d521216 commit 84a4a02

10 files changed

Lines changed: 395 additions & 203 deletions

File tree

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java‎

Lines changed: 142 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import com.pedropathing.geometry.BezierLine;
99
import com.pedropathing.geometry.Pose;
1010
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
11+
import com.skeletonarmy.marrow.TimerEx;
1112
import com.skeletonarmy.marrow.prompts.OptionPrompt;
1213
import com.skeletonarmy.marrow.prompts.Prompter;
1314

@@ -20,6 +21,7 @@
2021
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
2122

2223
import dev.nextftc.core.commands.delays.Delay;
24+
import dev.nextftc.core.commands.delays.WaitUntil;
2325
import dev.nextftc.core.commands.groups.ParallelRaceGroup;
2426
import dev.nextftc.core.commands.groups.SequentialGroup;
2527
import dev.nextftc.core.commands.utility.InstantCommand;
@@ -29,6 +31,7 @@
2931
@Autonomous(name = "Auto_Main_", group = "Main", preselectTeleOp="Teleop_Main_")
3032
public class Auto_Main_ extends NextFTCOpMode {
3133

34+
TimerEx timer25Sec = new TimerEx(25);
3235
// Variable to store the selected auto program
3336
Auto selectedAuto = null;
3437
// Create the prompter object for selecting Alliance and Auto
@@ -61,10 +64,14 @@ public void onInit() {
6164
prompter.prompt("auto",
6265
new OptionPrompt<>("Select Auto",
6366
new MOE_365_FAR(),
67+
new Near_15Ball(),
68+
new Near_15Ball2(),
6469
new Near_12Ball(),
6570
new Near_9Ball(),
6671
new Far_9Ball(),
67-
new Far_6Ball()
72+
new Far_6Ball(),
73+
new Far_3Ball()
74+
//new test()
6875
));
6976
prompter.onComplete(() -> {
7077
selectedAlliance = prompter.get("alliance");
@@ -97,10 +104,11 @@ public void onWaitForStart() {
97104

98105
@Override
99106
public void onStartButtonPressed() {
100-
Robot.lights.setAllianceDisplay(selectedAlliance);
107+
Robot.lights.setSolidAllianceDisplay(selectedAlliance);
101108
// Schedule the proper auto
102109
selectedAuto.runAuto();
103110
turretOn = selectedAuto.getTurretEnabled();
111+
timer25Sec.restart();
104112

105113
// Indicate that initialization is done
106114
telemetry.addLine("Initialized");
@@ -113,11 +121,7 @@ public void onUpdate() {
113121
telemetry.addData("Alliance", selectedAlliance);
114122
Robot.turret.updateAimbot(turretOn, true, 0);
115123
Robot.turret.updateFlywheel();
116-
117124
MecanumDrivetrainClass.robotPose = Robot.drivetrain.follower.getPose();
118-
119-
telemetry.addLine();
120-
telemetry.addData("Mirror -90", Math.toDegrees(paths.autoMirror(new Pose(0,0,-90)).getHeading()));
121125
telemetry.update();
122126
}
123127

@@ -151,7 +155,7 @@ abstract static class Auto{
151155
public abstract String toString();
152156
}
153157

154-
private class Far_6Ball extends Auto{
158+
private class Far_9Ball extends Auto{
155159
@Override
156160
public Pose getStartPose(){
157161
return paths.farStart;
@@ -171,16 +175,20 @@ public void runAuto(){
171175
Commands.runPath(paths.farShoot_to_farPreload, true, 1),
172176
Commands.runPath(paths.farPreload_to_farShoot, true, 1),
173177
Commands.shootBalls(),
178+
Commands.setFlywheelState(Turret.flywheelState.ON),
179+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
180+
Commands.runPath(paths.farShoot_to_hpPreload, true, 1),
181+
Commands.runPath(paths.hpPreload_to_farShoot, true, 1),
182+
Commands.shootBalls(),
174183
Commands.runPath(paths.farShoot_to_farLeave, true, 1)
175184
).schedule();
176185
}
177186

178187
@NonNull
179188
@Override
180-
public String toString(){return "Far Zone 6 Ball";}
189+
public String toString(){return "Far 9 Ball";}
181190
}
182-
183-
private class Far_9Ball extends Auto{
191+
private class Far_6Ball extends Auto{
184192
@Override
185193
public Pose getStartPose(){
186194
return paths.farStart;
@@ -200,20 +208,44 @@ public void runAuto(){
200208
Commands.runPath(paths.farShoot_to_farPreload, true, 1),
201209
Commands.runPath(paths.farPreload_to_farShoot, true, 1),
202210
Commands.shootBalls(),
203-
Commands.setFlywheelState(Turret.flywheelState.ON),
204-
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
205-
Commands.runPath(paths.farShoot_to_midPreload, true, 1),
206-
Commands.runPath(paths.midPreload_to_farShoot, true, 1),
207-
Commands.shootBalls(),
208211
Commands.runPath(paths.farShoot_to_farLeave, true, 1)
209212
).schedule();
210213
}
211214

212215
@NonNull
213216
@Override
214-
public String toString(){return "Far Zone 9 Ball";}
217+
public String toString(){return "Far 6 Ball";}
215218
}
219+
private class Far_3Ball extends Auto{
216220

221+
@Override
222+
Pose getStartPose() {
223+
return paths.farStart;
224+
}
225+
226+
@Override
227+
boolean getTurretEnabled() {
228+
return true;
229+
}
230+
231+
@Override
232+
void runAuto() {
233+
new SequentialGroup(
234+
Commands.setFlywheelState(Turret.flywheelState.ON),
235+
Commands.runPath(paths.farStart_to_farShoot, true, 1),
236+
Commands.shootBalls(),
237+
new WaitUntil(() -> timer25Sec.isDone()),
238+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
239+
Commands.runPath(paths.farShoot_to_hpPreload, true, 1)
240+
).schedule();
241+
}
242+
243+
@NonNull
244+
@Override
245+
public String toString() {
246+
return "Far 3 Ball + HP Intake";
247+
}
248+
}
217249
private class Near_9Ball extends Auto{
218250
@Override
219251
public Pose getStartPose(){
@@ -247,9 +279,8 @@ public void runAuto(){
247279

248280
@NonNull
249281
@Override
250-
public String toString(){return "Near Zone 9 Ball";}
282+
public String toString(){return "Near 9 Ball";}
251283
}
252-
253284
private class Near_12Ball extends Auto{
254285
@Override
255286
public Pose getStartPose(){
@@ -285,9 +316,100 @@ public void runAuto(){
285316

286317
@NonNull
287318
@Override
288-
public String toString(){return "Near Zone 12 Ball";}
319+
public String toString(){return "Near 12 Ball";}
320+
}
321+
private class Near_15Ball extends Auto{
322+
323+
@Override
324+
Pose getStartPose() {
325+
return paths.nearStart;
326+
}
327+
328+
@Override
329+
boolean getTurretEnabled() {
330+
return true;
331+
}
332+
333+
@Override
334+
void runAuto() {
335+
new SequentialGroup(
336+
Commands.setFlywheelState(Turret.flywheelState.ON),
337+
Commands.runPath(paths.nearStart_to_midShoot, true, 1),
338+
Commands.shootBalls(),
339+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
340+
Commands.runPath(paths.midShoot_to_midPreload, true, 1),
341+
Commands.runPath(paths.midPreload_to_midShoot, true, 1),
342+
Commands.shootBalls(),
343+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
344+
Commands.runPath(paths.midShoot_to_openGateIntake, true, 1),
345+
Commands.waitForArtifacts(),
346+
Commands.runPath(paths.openGateIntake_to_midShoot, true, 1),
347+
Commands.shootBalls(),
348+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
349+
Commands.runPath(paths.midShoot_to_farPreload, true, 1),
350+
Commands.setIntakeMode(Intake.intakeMode.OFF),
351+
Commands.runPath(paths.farPreload_to_midShoot, true, 1),
352+
Commands.shootBalls(),
353+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
354+
Commands.runPath(paths.midShoot_to_nearPreload, true, 1),
355+
Commands.runPath(paths.nearPreload_to_midShoot, false, 1),
356+
Commands.shootBalls(),
357+
Commands.runPath(paths.midShoot_to_nearLeave, true, 1)
358+
).schedule();
359+
}
360+
361+
@NonNull
362+
@Override
363+
public String toString() {
364+
return "Near 15 Ball (With Far Spike Mark)";
365+
}
289366
}
367+
private class Near_15Ball2 extends Auto{
290368

369+
@Override
370+
Pose getStartPose() {
371+
return paths.nearStart;
372+
}
373+
374+
@Override
375+
boolean getTurretEnabled() {
376+
return true;
377+
}
378+
379+
@Override
380+
void runAuto() {
381+
new SequentialGroup(
382+
Commands.setFlywheelState(Turret.flywheelState.ON),
383+
Commands.runPath(paths.nearStart_to_midShoot, true, 1),
384+
Commands.shootBalls(),
385+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
386+
Commands.runPath(paths.midShoot_to_midPreload, true, 1),
387+
Commands.runPath(paths.midPreload_to_midShoot, true, 1),
388+
Commands.shootBalls(),
389+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
390+
Commands.runPath(paths.midShoot_to_openGateIntake, true, 1),
391+
Commands.waitForArtifacts(),
392+
Commands.runPath(paths.openGateIntake_to_midShoot, true, 1),
393+
Commands.shootBalls(),
394+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
395+
Commands.runPath(paths.midShoot_to_openGateIntake, true, 1),
396+
Commands.waitForArtifacts(),
397+
Commands.runPath(paths.openGateIntake_to_midShoot, true, 1),
398+
Commands.shootBalls(),
399+
Commands.setIntakeMode(Intake.intakeMode.INTAKING),
400+
Commands.runPath(paths.midShoot_to_nearPreload, true, 1),
401+
Commands.runPath(paths.nearPreload_to_midShoot, false, 1),
402+
Commands.shootBalls(),
403+
Commands.runPath(paths.midShoot_to_nearLeave, true, 1)
404+
).schedule();
405+
}
406+
407+
@NonNull
408+
@Override
409+
public String toString() {
410+
return "Near 15 Ball (No Far Spike Mark)";
411+
}
412+
}
291413
private class MOE_365_FAR extends Auto{
292414
@Override
293415
public Pose getStartPose(){
@@ -341,6 +463,6 @@ public void runAuto(){
341463

342464
@NonNull
343465
@Override
344-
public String toString(){return "MOE 365 Far Zone Auto";}
466+
public String toString(){return "MOE 365 Far";}
345467
}
346468
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java‎

Lines changed: 24 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -16,10 +16,7 @@
1616
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
1717
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
1818
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
19-
import org.firstinspires.ftc.teamcode.Prism.Color;
20-
import org.firstinspires.ftc.teamcode.Prism.Direction;
2119
import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
22-
import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
2320

2421
import dev.nextftc.control.ControlSystem;
2522
import dev.nextftc.control.KineticState;
@@ -364,40 +361,45 @@ public static class GoBildaPrismBarClass {
364361
// Maximum length of 4 daisy chained strips is 36 (12 + 12 + 6 + 6)
365362
// Scrimmage length of 2 daisy chained strips is 24 (12 + 12)
366363

364+
// Artboards current status:
365+
GoBildaPrismDriver.Artboard solidRED = GoBildaPrismDriver.Artboard.ARTBOARD_0;
366+
GoBildaPrismDriver.Artboard solidBLUE = GoBildaPrismDriver.Artboard.ARTBOARD_1;
367+
GoBildaPrismDriver.Artboard blinkingRED = GoBildaPrismDriver.Artboard.ARTBOARD_2;
368+
GoBildaPrismDriver.Artboard blinkingBLUE = GoBildaPrismDriver.Artboard.ARTBOARD_3;
369+
GoBildaPrismDriver.Artboard rainbow = GoBildaPrismDriver.Artboard.ARTBOARD_4;
370+
367371
GoBildaPrismDriver prism;
368-
int stripBrightness = 50;
372+
int stripBrightness = 25;
369373
public void init(@NonNull OpMode opmode, int stripLength){
370374
prism = opmode.hardwareMap.get(GoBildaPrismDriver.class, "prism");
371375
prism.setStripLength(stripLength);
376+
prism.clearAllAnimations();
372377
}
373378

374-
public void setStripSolidColor(Color color){
375-
prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Solid(color, stripBrightness));
379+
public void setStripBrightness(int brightness){
380+
stripBrightness = brightness;
376381
}
377382

378-
public void setStripRainbow(){
379-
PrismAnimations.AnimationBase colorA = new PrismAnimations.Rainbow(Direction.Backward);
380-
colorA.setIndexes(0, 12);
381-
PrismAnimations.AnimationBase colorB = new PrismAnimations.Rainbow(Direction.Forward);
382-
colorB.setIndexes(13, 24);
383-
prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, colorA);
384-
prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_2, colorB);
383+
public void setDisplayedArtboard(GoBildaPrismDriver.Artboard board){
384+
prism.loadAnimationsFromArtboard(board);
385385
}
386386

387-
public void clearStripAnimations(){
388-
prism.clearAllAnimations();
387+
public void setStripRainbow(){
388+
setDisplayedArtboard(rainbow);
389389
}
390390

391-
public void setStripBrightness(int brightness){
392-
stripBrightness = brightness;
391+
public void setSolidAllianceDisplay(LoadHardwareClass.Alliance alliance){
392+
if (alliance == LoadHardwareClass.Alliance.RED){
393+
setDisplayedArtboard(solidRED);
394+
}else if (alliance == LoadHardwareClass.Alliance.BLUE){
395+
setDisplayedArtboard(solidBLUE);
396+
}
393397
}
394-
395-
public void setAllianceDisplay(LoadHardwareClass.Alliance alliance){
396-
clearStripAnimations();
398+
public void setBlinkingAllianceDisplay(LoadHardwareClass.Alliance alliance){
397399
if (alliance == LoadHardwareClass.Alliance.RED){
398-
setStripSolidColor(Color.RED);
400+
setDisplayedArtboard(blinkingRED);
399401
}else if (alliance == LoadHardwareClass.Alliance.BLUE){
400-
setStripSolidColor(Color.BLUE);
402+
setDisplayedArtboard(blinkingBLUE);
401403
}
402404
}
403405
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ public enum intakeMode {
2323
SHOOTING,
2424
NO_BELT,
2525
REVERSING,
26+
REVERSE_NOBELT,
2627
OFF
2728
}
2829

@@ -74,14 +75,17 @@ public void setMode(intakeMode direction) {
7475
intake.setPower(1);
7576
belt.setPower(1);
7677
}else if (direction == intakeMode.SHOOTING){
77-
intake.setPower(0); //TODO change this to 0 if using servo powered intake belt
78+
intake.setPower(0);
7879
belt.setPower(1);
7980
}else if (direction == intakeMode.REVERSING){
8081
intake.setPower(-1);
8182
belt.setPower(-1);
8283
}else if (direction == intakeMode.NO_BELT){
8384
intake.setPower(1);
8485
belt.setPower(0);
86+
}else if (direction == intakeMode.REVERSE_NOBELT){
87+
intake.setPower(-1);
88+
belt.setPower(0);
8589
}else{
8690
intake.setPower(0);
8791
belt.setPower(0);
@@ -112,6 +116,8 @@ public intakeMode getMode(){
112116
return intakeMode.NO_BELT;
113117
}else if (intakePower == -1 && beltPower == -1){
114118
return intakeMode.REVERSING;
119+
}else if (intakePower == -1 && beltPower == 0){
120+
return intakeMode.REVERSE_NOBELT;
115121
}else{
116122
return intakeMode.OFF;
117123
}

0 commit comments

Comments
 (0)