5
5
import static org .firstinspires .ftc .teamcode .pedroPathing .tuning .FollowerConstants .rightFrontMotorName ;
6
6
import static org .firstinspires .ftc .teamcode .pedroPathing .tuning .FollowerConstants .rightRearMotorName ;
7
7
8
+ import android .util .Log ;
9
+
8
10
import com .acmerobotics .dashboard .config .Config ;
9
11
import com .qualcomm .robotcore .eventloop .opmode .OpMode ;
10
12
import com .qualcomm .robotcore .eventloop .opmode .TeleOp ;
@@ -58,6 +60,8 @@ public class ManualDrive extends OpMode {
58
60
public static double ARM_MIN = -2.25 ;
59
61
public static double ARM_MAX = 1.9 ;
60
62
public static double ARM_LOWBASKET = -0.2 ;
63
+ public static double ARM_INTAKE = -1.7 ;
64
+ public static double ARM_SPECIMEN = -0.25 ;
61
65
62
66
static class ArmPIDF implements FeedForwardConstant {
63
67
@@ -122,6 +126,10 @@ public void init() {
122
126
boolean bPressed = false ;
123
127
boolean xPressed = false ;
124
128
129
+ private boolean epsilonEquals (double a , double b ) {
130
+ return Math .abs (a - b ) < 0.00001 ;
131
+ }
132
+
125
133
/**
126
134
* This runs the OpMode. This is only drive control with Pedro Pathing live centripetal force
127
135
* correction.
@@ -152,7 +160,7 @@ public void loop() {
152
160
armPID .setTargetPosition (ARM_TARGET );
153
161
armPID .updatePosition (armAngle ());
154
162
armMotor .setPower (armPID .runPIDF ());
155
- ARM_TARGET + = gamepad1 .right_stick_y * ARM_SPEED ;
163
+ ARM_TARGET - = gamepad1 .right_stick_y * ARM_SPEED ;
156
164
157
165
// Check that arm target is in right position
158
166
if (ARM_TARGET < ARM_MIN ) {
@@ -163,33 +171,41 @@ public void loop() {
163
171
164
172
if (gamepad1 .y ) {
165
173
ARM_TARGET = ARM_LOWBASKET ;
174
+ wristServo .setPosition (WRIST_OUT );
175
+ }
176
+ if (gamepad1 .dpad_up ) {
177
+ ARM_TARGET = ARM_SPECIMEN ;
178
+ wristServo .setPosition (WRIST_IN );
179
+ }
180
+ if (gamepad1 .dpad_down ) {
181
+ ARM_TARGET = ARM_INTAKE ;
182
+ wristServo .setPosition (WRIST_OUT );
166
183
}
167
184
168
185
// Intake servo control
169
186
if (gamepad1 .a && !aPressed ) {
170
187
aPressed = true ;
171
- intakeServo .setPower (intakeServo .getPower () == 0.0 ? -1.0 : 0.0 );
188
+ intakeServo .setPower (epsilonEquals ( intakeServo .getPower (), 0.0 ) ? -1.0 : 0.0 );
172
189
} else if (!gamepad1 .a ) {
173
190
aPressed = false ;
174
191
}
175
192
if (gamepad1 .b && !bPressed ) {
176
193
bPressed = true ;
177
- intakeServo .setPower (intakeServo .getPower () == 0.0 ? 1.0 : 0.0 );
194
+ intakeServo .setPower (epsilonEquals ( intakeServo .getPower (), 0.0 ) ? 1.0 : 0.0 );
178
195
} else if (!gamepad1 .b ) {
179
196
bPressed = false ;
180
197
}
181
198
182
199
// Wrist control
183
200
if (gamepad1 .x && !xPressed ) {
184
201
xPressed = true ;
185
- wristServo .setPosition (wristServo .getPosition () == WRIST_IN ? WRIST_OUT : WRIST_IN );
202
+ wristServo .setPosition (epsilonEquals ( wristServo .getPosition (), WRIST_IN ) ? WRIST_OUT : WRIST_IN );
186
203
} else if (!gamepad1 .x ) {
187
204
xPressed = false ;
188
205
}
189
206
190
207
telemetry .addData ("Arm Angle (deg)" , Math .toDegrees (armAngle ()));
191
208
telemetry .addData ("Arm Power" , armPID .runPIDF ());
192
- telemetry .addData ("Wrist Position" , wristServo .getPosition ());
193
209
telemetry .update ();
194
210
}
195
211
}
0 commit comments