Skip to content

Commit f6627aa

Browse files
committed
Fix bug in servo motions
1 parent 660eebe commit f6627aa

File tree

1 file changed

+21
-5
lines changed

1 file changed

+21
-5
lines changed

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualDrive.java

+21-5
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@
55
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
66
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
77

8+
import android.util.Log;
9+
810
import com.acmerobotics.dashboard.config.Config;
911
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
1012
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -58,6 +60,8 @@ public class ManualDrive extends OpMode {
5860
public static double ARM_MIN = -2.25;
5961
public static double ARM_MAX = 1.9;
6062
public static double ARM_LOWBASKET = -0.2;
63+
public static double ARM_INTAKE = -1.7;
64+
public static double ARM_SPECIMEN = -0.25;
6165

6266
static class ArmPIDF implements FeedForwardConstant {
6367

@@ -122,6 +126,10 @@ public void init() {
122126
boolean bPressed = false;
123127
boolean xPressed = false;
124128

129+
private boolean epsilonEquals(double a, double b) {
130+
return Math.abs(a - b) < 0.00001;
131+
}
132+
125133
/**
126134
* This runs the OpMode. This is only drive control with Pedro Pathing live centripetal force
127135
* correction.
@@ -152,7 +160,7 @@ public void loop() {
152160
armPID.setTargetPosition(ARM_TARGET);
153161
armPID.updatePosition(armAngle());
154162
armMotor.setPower(armPID.runPIDF());
155-
ARM_TARGET += gamepad1.right_stick_y * ARM_SPEED;
163+
ARM_TARGET -= gamepad1.right_stick_y * ARM_SPEED;
156164

157165
// Check that arm target is in right position
158166
if (ARM_TARGET < ARM_MIN) {
@@ -163,33 +171,41 @@ public void loop() {
163171

164172
if (gamepad1.y) {
165173
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);
166183
}
167184

168185
// Intake servo control
169186
if (gamepad1.a && !aPressed) {
170187
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);
172189
} else if (!gamepad1.a) {
173190
aPressed = false;
174191
}
175192
if (gamepad1.b && !bPressed) {
176193
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);
178195
} else if (!gamepad1.b) {
179196
bPressed = false;
180197
}
181198

182199
// Wrist control
183200
if (gamepad1.x && !xPressed) {
184201
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);
186203
} else if (!gamepad1.x) {
187204
xPressed = false;
188205
}
189206

190207
telemetry.addData("Arm Angle (deg)", Math.toDegrees(armAngle()));
191208
telemetry.addData("Arm Power", armPID.runPIDF());
192-
telemetry.addData("Wrist Position", wristServo.getPosition());
193209
telemetry.update();
194210
}
195211
}

0 commit comments

Comments
 (0)