@@ -97,13 +97,14 @@ public void runOpMode() throws InterruptedException {
97
97
.lineTo (new Vector2d (10 , -36 ))
98
98
.turn (Math .toRadians (90 ))
99
99
100
-
101
- .waitSeconds (10 )
102
- .splineToSplineHeading (new Pose2d (11 , -36 , Math .toRadians (180 )), Math .toRadians (180 ))
103
- .addDisplacementMarker (()->{
100
+ .addTemporalMarker (() ->{
104
101
//ARM DROPS PIXEL
105
- android .util .Log .i ("DROP" , "seconddDrop" );
102
+ android .util .Log .i ("DROP" , "Drop" );
103
+
106
104
})
105
+
106
+ .splineToSplineHeading (new Pose2d (11 , -36 , Math .toRadians (180 )), Math .toRadians (180 ))
107
+
107
108
.turn (Math .toRadians (180 ))
108
109
109
110
.splineToSplineHeading (new Pose2d (3 * 12 + 5 , -3 * 12 , 0 ), 0 )
@@ -115,19 +116,19 @@ public void runOpMode() throws InterruptedException {
115
116
traj = drive .trajectorySequenceBuilder (startPose )
116
117
.lineTo (new Vector2d (16 , -36 ))
117
118
.turn (Math .toRadians (-90 ))
118
- .addDisplacementMarker (() ->{
119
+ .addTemporalMarker (() ->{
119
120
//ARM DROPS PIXEL
120
121
android .util .Log .i ("DROP" , "drop" );
121
122
})
122
-
123
+ . lineTo ( new Vector2d ( startPose . getX (), startPose . getY ()))
123
124
.splineToSplineHeading (new Pose2d (3 * 12 + 5 , -3 * 12 , 0 ), 0 )
124
125
.build ();
125
126
break ;
126
127
case CENTER :
127
128
128
129
traj = drive .trajectorySequenceBuilder (startPose )
129
130
.lineTo (new Vector2d (16 , -36 ))
130
- .addDisplacementMarker (() -> {
131
+ .addTemporalMarker (() -> {
131
132
//ARM DROPS PIXEL
132
133
android .util .Log .i ("DROP" , "drop" );
133
134
@@ -146,7 +147,7 @@ public void runOpMode() throws InterruptedException {
146
147
147
148
.turn (Math .toRadians (90 ))
148
149
149
- .addDisplacementMarker (() -> {
150
+ .addTemporalMarker (() -> {
150
151
//PIXEL DROP
151
152
152
153
android .util .Log .i ("DROP" , "drop" );
@@ -169,6 +170,7 @@ public void runOpMode() throws InterruptedException {
169
170
//PIXEL DROP
170
171
android .util .Log .i ("DROP" , "drop" );
171
172
})
173
+ .lineTo (new Vector2d (startPose .getX (), startPose .getY ()))
172
174
//.splineToSplineHeading(new Pose2d(3*12 + 5, -3*12, 0), 0)
173
175
.lineToSplineHeading (new Pose2d (2 * 12 , -3 * 12 + 2 , Math .toRadians (90 )))
174
176
.lineToSplineHeading (new Pose2d (3 * 12 + 5 , -3 * 12 + 2 , 0 ))
@@ -177,7 +179,7 @@ public void runOpMode() throws InterruptedException {
177
179
case CENTER :
178
180
traj = drive .trajectorySequenceBuilder (startPose )
179
181
.lineTo (new Vector2d (startPose .getX (), -3 * 12 + 2 )) // drive forward to prevent the spline from cutting through the poles
180
- .addDisplacementMarker (() -> {
182
+ .addTemporalMarker (() -> {
181
183
//PIXEL DROP
182
184
android .util .Log .i ("DROP" , "drop" );
183
185
})
0 commit comments