Skip to content

Commit 40b6762

Browse files
SantiagoFM27SantiagoFM27
committed
Descripción breve de los cambios realizados
1 parent bd5fe94 commit 40b6762

File tree

11 files changed

+334
-194
lines changed

11 files changed

+334
-194
lines changed

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto1Piece.java

-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
import com.pedropathing.localization.Pose;
66
import com.pedropathing.pathgen.BezierLine;
77
import com.pedropathing.pathgen.Path;
8-
import com.pedropathing.pathgen.PathChain;
98
import com.pedropathing.pathgen.Point;
109
import com.pedropathing.util.Timer;
1110
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto.java renamed to ‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoNormal.java

+9-19
Original file line numberDiff line numberDiff line change
@@ -1,31 +1,15 @@
11
package org.firstinspires.ftc.teamcode;
22

33
import com.arcrobotics.ftclib.command.CommandOpMode;
4-
import com.arcrobotics.ftclib.command.InstantCommand;
5-
import com.arcrobotics.ftclib.command.button.GamepadButton;
6-
import com.arcrobotics.ftclib.command.button.Trigger;
7-
import com.arcrobotics.ftclib.gamepad.GamepadEx;
8-
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
9-
import com.arcrobotics.ftclib.gamepad.TriggerReader;
104
import com.pedropathing.follower.Follower;
115
import com.pedropathing.localization.Pose;
126
import com.pedropathing.pathgen.BezierLine;
137
import com.pedropathing.pathgen.Path;
148
import com.pedropathing.pathgen.PathChain;
159
import com.pedropathing.pathgen.Point;
1610
import com.pedropathing.util.Timer;
17-
import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
1811
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
19-
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
20-
import com.qualcomm.robotcore.hardware.IMU;
21-
22-
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
23-
import org.firstinspires.ftc.teamcode.Commands.TeleopCommands.BasketScoreCommand;
24-
import org.firstinspires.ftc.teamcode.Commands.TeleopCommands.TakeSampleCommand;
2512
import org.firstinspires.ftc.teamcode.Constants.Constants;
26-
import org.firstinspires.ftc.teamcode.Constants.Constants.Ids;
27-
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
28-
import org.firstinspires.ftc.teamcode.Commands.JoystickCMD;
2913
import org.firstinspires.ftc.teamcode.Constants.PedroPathingConstants.FConstants;
3014
import org.firstinspires.ftc.teamcode.Constants.PedroPathingConstants.LConstants;
3115
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Arm;
@@ -34,10 +18,9 @@
3418
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Slider;
3519
import org.firstinspires.ftc.teamcode.Subsystems.Arm.SliderAngle;
3620
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Wrist;
37-
import org.firstinspires.ftc.teamcode.Subsystems.MecanumDrivetrain;
3821

39-
@Autonomous(name = "Auto", group = "Examples")
40-
public class Auto extends CommandOpMode {
22+
@Autonomous(name = "AutoNormal", group = "Examples")
23+
public class AutoNormal extends CommandOpMode {
4124

4225
private Follower follower;
4326
private Timer pathTimer, actionTimer, opmodeTimer;
@@ -69,6 +52,8 @@ public class Auto extends CommandOpMode {
6952
private final Pose goLeftToPiece3 = new Pose(-61, -10, Math.toRadians(80.0));
7053
private final Pose leavePiece3 = new Pose(-61, -58, Math.toRadians(80.0));
7154

55+
private final Pose goFrontFinal = new Pose(-57, -10, Math.toRadians(80.0));
56+
7257

7358

7459
private Path goToBasketPath;
@@ -107,6 +92,11 @@ public void buildPaths() {
10792
.setLinearHeadingInterpolation(goFrontToPiece3.getHeading(), goLeftToPiece3.getHeading())
10893
.addPath(new BezierLine(new Point(goLeftToPiece3), new Point(leavePiece3)))
10994
.setLinearHeadingInterpolation(goLeftToPiece3.getHeading(), leavePiece3.getHeading())
95+
96+
// Go front final
97+
.addPath(new BezierLine(new Point(leavePiece3), new Point(goFrontFinal)))
98+
.setLinearHeadingInterpolation(leavePiece3.getHeading(), goFrontFinal.getHeading())
99+
110100
.build();
111101

112102
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoPark.java

+270
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,270 @@
1+
package org.firstinspires.ftc.teamcode;
2+
3+
import com.arcrobotics.ftclib.command.CommandOpMode;
4+
import com.pedropathing.follower.Follower;
5+
import com.pedropathing.localization.Pose;
6+
import com.pedropathing.pathgen.BezierLine;
7+
import com.pedropathing.pathgen.Path;
8+
import com.pedropathing.pathgen.PathChain;
9+
import com.pedropathing.pathgen.Point;
10+
import com.pedropathing.util.Timer;
11+
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
12+
13+
import org.firstinspires.ftc.teamcode.Constants.Constants;
14+
import org.firstinspires.ftc.teamcode.Constants.PedroPathingConstants.FConstants;
15+
import org.firstinspires.ftc.teamcode.Constants.PedroPathingConstants.LConstants;
16+
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Arm;
17+
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Gripper;
18+
import org.firstinspires.ftc.teamcode.Subsystems.Arm.GripperAngle;
19+
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Slider;
20+
import org.firstinspires.ftc.teamcode.Subsystems.Arm.SliderAngle;
21+
import org.firstinspires.ftc.teamcode.Subsystems.Arm.Wrist;
22+
23+
@Autonomous(name = "AutoPark", group = "Examples")
24+
public class AutoPark extends CommandOpMode {
25+
26+
private Follower follower;
27+
private Timer pathTimer, actionTimer, opmodeTimer;
28+
private int pathState;
29+
30+
boolean hasScoreAPiece = false;
31+
32+
SliderAngle sliderAngle;
33+
Slider slider;
34+
Gripper gripper;
35+
GripperAngle gripperAngle;
36+
Arm arm;
37+
Wrist wrist;
38+
39+
// Leave pices points
40+
private final Pose startPose = new Pose(-36, -60.0, Math.toRadians(90.0));
41+
private final Pose goToBasket = new Pose(-55, -55, Math.toRadians(240.0));
42+
private final Pose goToPiece1 = new Pose(-38, -40, Math.toRadians(80.0));
43+
44+
private final Pose goFrontToPiece1 = new Pose(-38, -10, Math.toRadians(80.0));
45+
private final Pose goLeftToPiece1 = new Pose(-48, -10, Math.toRadians(80.0));
46+
private final Pose leavePiece1 = new Pose(-48, -58, Math.toRadians(80.0));
47+
48+
private final Pose goFrontToPiece2 = new Pose(-48, -10, Math.toRadians(80.0));
49+
private final Pose goLeftToPiece2 = new Pose(-57, -10, Math.toRadians(80.0));
50+
private final Pose leavePiece2 = new Pose(-57, -58, Math.toRadians(80.0));
51+
52+
private final Pose goFrontToPiece3 = new Pose(-57, -10, Math.toRadians(80.0));
53+
private final Pose goLeftToPiece3 = new Pose(-61, -10, Math.toRadians(80.0));
54+
private final Pose leavePiece3 = new Pose(-61, -58, Math.toRadians(80.0));
55+
56+
private final Pose park = new Pose(65, -56, Math.toRadians(80.0));
57+
58+
59+
60+
private Path goToBasketPath;
61+
private PathChain leavePiecePC;
62+
63+
public void buildPaths() {
64+
// Go to basket
65+
goToBasketPath = new Path(new BezierLine(new Point(startPose), new Point(goToBasket)));
66+
goToBasketPath.setLinearHeadingInterpolation(startPose.getHeading(), goToBasket.getHeading());
67+
68+
// leave pieces
69+
leavePiecePC = follower.pathBuilder()
70+
.addPath(new BezierLine(new Point(goToBasket), new Point(goToPiece1)))
71+
.setLinearHeadingInterpolation(goToBasket.getHeading(), goToPiece1.getHeading())
72+
73+
// leave piece 1
74+
.addPath(new BezierLine(new Point(goToPiece1), new Point(goFrontToPiece1)))
75+
.setLinearHeadingInterpolation(goToPiece1.getHeading(), goFrontToPiece1.getHeading())
76+
.addPath(new BezierLine(new Point(goFrontToPiece1), new Point(goLeftToPiece1)))
77+
.setLinearHeadingInterpolation(goFrontToPiece1.getHeading(), goLeftToPiece1.getHeading())
78+
.addPath(new BezierLine(new Point(goLeftToPiece1), new Point(leavePiece1)))
79+
.setLinearHeadingInterpolation(goLeftToPiece1.getHeading(), leavePiece1.getHeading())
80+
81+
// leave piece 2
82+
.addPath(new BezierLine(new Point(leavePiece1), new Point(goFrontToPiece2)))
83+
.setLinearHeadingInterpolation(leavePiece1.getHeading(), goFrontToPiece2.getHeading())
84+
.addPath(new BezierLine(new Point(goFrontToPiece2), new Point(goLeftToPiece2)))
85+
.setLinearHeadingInterpolation(goFrontToPiece2.getHeading(), goLeftToPiece2.getHeading())
86+
.addPath(new BezierLine(new Point(goLeftToPiece2), new Point(leavePiece2)))
87+
.setLinearHeadingInterpolation(goLeftToPiece2.getHeading(), leavePiece2.getHeading())
88+
89+
// leave piece 2
90+
.addPath(new BezierLine(new Point(leavePiece2), new Point(goFrontToPiece3)))
91+
.setLinearHeadingInterpolation(leavePiece2.getHeading(), goFrontToPiece3.getHeading())
92+
.addPath(new BezierLine(new Point(goFrontToPiece3), new Point(goLeftToPiece3)))
93+
.setLinearHeadingInterpolation(goFrontToPiece3.getHeading(), goLeftToPiece3.getHeading())
94+
.addPath(new BezierLine(new Point(goLeftToPiece3), new Point(leavePiece3)))
95+
.setLinearHeadingInterpolation(goLeftToPiece3.getHeading(), leavePiece3.getHeading())
96+
97+
// Go front final
98+
.addPath(new BezierLine(new Point(leavePiece3), new Point(park)))
99+
.setLinearHeadingInterpolation(leavePiece3.getHeading(), park.getHeading())
100+
101+
.build();
102+
103+
}
104+
105+
public void autonomousPathUpdate() {
106+
switch (pathState) {
107+
case 0:
108+
follower.followPath(goToBasketPath);
109+
setPathState(1);
110+
break;
111+
case 1:
112+
if(!follower.isBusy()) {
113+
if (basketScore()) {
114+
setPathState(2);
115+
}
116+
}
117+
break;
118+
case 2:
119+
if(!follower.isBusy()) {
120+
if (quesadillaPosition()) {
121+
setPathState(3);
122+
}
123+
}
124+
break;
125+
case 3:
126+
if(!follower.isBusy()) {
127+
follower.followPath(leavePiecePC);
128+
setPathState(4);
129+
}
130+
break;
131+
case 4:
132+
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
133+
if(!follower.isBusy()) {
134+
quesadillaPosition();
135+
136+
/* Set the state to a Case we won't use or define, so it just stops running an new paths */
137+
setPathState(-1);
138+
}
139+
break;
140+
}
141+
}
142+
143+
public void setPathState(int pState) {
144+
pathState = pState;
145+
pathTimer.resetTimer();
146+
}
147+
148+
@Override
149+
public void initialize() {
150+
pathTimer = new Timer();
151+
opmodeTimer = new Timer();
152+
opmodeTimer.resetTimer();
153+
154+
com.pedropathing.util.Constants.setConstants(FConstants.class, LConstants.class);
155+
follower = new Follower(hardwareMap);
156+
follower.setStartingPose(startPose);
157+
buildPaths();
158+
159+
sliderAngle = new SliderAngle(hardwareMap, telemetry);
160+
slider = new Slider(hardwareMap, telemetry);
161+
162+
gripper = new Gripper(hardwareMap, telemetry);
163+
gripperAngle = new GripperAngle(hardwareMap, telemetry);
164+
arm = new Arm(hardwareMap, telemetry);
165+
wrist = new Wrist(hardwareMap, telemetry);
166+
167+
}
168+
169+
@Override
170+
public void runOpMode() throws InterruptedException {
171+
initialize();
172+
173+
waitForStart();
174+
175+
// Start instances
176+
opmodeTimer.resetTimer();
177+
setPathState(0);
178+
gripper.close();
179+
180+
// run the scheduler
181+
while (!isStopRequested() && opModeIsActive()) {
182+
// These loop the movements of the robot
183+
follower.update();
184+
autonomousPathUpdate();
185+
186+
// Feedback to Driver Hub
187+
telemetry.addData("path state", pathState);
188+
telemetry.addData("x", follower.getPose().getX());
189+
telemetry.addData("y", follower.getPose().getY());
190+
telemetry.addData("heading", follower.getPose().getHeading());
191+
telemetry.update();
192+
193+
run();
194+
}
195+
reset();
196+
}
197+
198+
public boolean semiQuesadillaPosition() {
199+
arm.goToPosition(Constants.Arm.homePositon);
200+
wrist.goToPosition(Constants.Wrist.homePositon);
201+
sliderAngle.setTargetPosition(2.7);
202+
slider.goToIntakePosition();
203+
gripperAngle.goToIntakePosition();
204+
gripper.close();
205+
206+
return slider.isAtPosition(Constants.Slider.intakePosition);
207+
}
208+
209+
public boolean quesadillaPosition() {
210+
arm.goToPosition(Constants.Arm.homePositon);
211+
wrist.goToPosition(Constants.Wrist.homePositon);
212+
sliderAngle.goToQuesadillaPosition();
213+
slider.goToHomePosition();
214+
gripperAngle.goToIntakePosition();
215+
gripper.close();
216+
217+
return sliderAngle.isAtPosition(Constants.SliderAngle.quesadillaPosition);
218+
}
219+
public boolean basketScore() {
220+
if (!hasScoreAPiece) {
221+
if (!slider.isAtPosition(Constants.Slider.basketScorePosition)) {
222+
arm.goToPosition(Constants.Arm.basketScorePosition);
223+
wrist.goToPosition(Constants.Wrist.basketScorePosition);
224+
225+
if (sliderAngle.isAtPosition(Constants.SliderAngle.homePosition)) {
226+
slider.goToBasketPosition();
227+
} else {
228+
sliderAngle.goToHomePosition();
229+
}
230+
} else {
231+
sliderAngle.goToBasketPosition();
232+
if (sliderAngle.isAtPosition(Constants.SliderAngle.basketScorePosition)) {
233+
try {Thread.sleep(1200);} catch (InterruptedException e) {}
234+
gripper.open();
235+
hasScoreAPiece = true;
236+
}
237+
238+
}
239+
} else {
240+
if (!sliderAngle.isAtPosition(Constants.SliderAngle.homePosition)) {
241+
sliderAngle.goToHomePosition();
242+
} else {
243+
slider.goToHomePosition();
244+
245+
if (slider.isAtPosition(Constants.Slider.homePosition)) {
246+
hasScoreAPiece = false;
247+
return true;
248+
}
249+
}
250+
}
251+
252+
return false;
253+
}
254+
255+
public void grabPiece() {
256+
// Grab the piece
257+
gripper.open();
258+
arm.goToPosition(3.0);
259+
wrist.goToPosition(2.0);
260+
slider.goToIntakePosition();
261+
sliderAngle.goToPosition(6.0);
262+
try {Thread.sleep(2000);} catch (InterruptedException e) {}
263+
gripper.close();
264+
try {Thread.sleep(500);} catch (InterruptedException e) {}
265+
266+
// Go to quesadilla position
267+
arm.goToPosition(Constants.Arm.homePositon);
268+
wrist.goToPosition(Constants.Wrist.homePositon);
269+
}
270+
}

0 commit comments

Comments
 (0)