@Override
public void init() {
super.init();
speedMultiplier = 1;
powerHangingSpeed = .7f;
pincerHangingSpeed = .3f;
pivotHangingSpeed = .25f;
speedReversePressed = false;
speedChangePressed = false;
hangPivotBrakePressed = false;
hangBrakePressed = false;
hangPivotPressed = false;
// No retention on the claw yet
keepMove = 0.0f;
keepPivot = 0;
hangPivot = .75f;
servoPlease.setPosition(.5);
LWing.setPosition(.5);
RWing.setPosition(.5);
}
/** Loop repeatedly called during TeleOp while the robot is running. */
@Override
public void loop() {
// Scale values from analog sticks into range readable by motors
float leftY = (float) scaleInput(Range.clip(gamepad1.left_stick_y, -1, 1));
float rightY = (float) scaleInput(Range.clip(gamepad1.right_stick_y, -1, 1));
float hangMove = -(float) scaleInput(Range.clip(gamepad2.right_stick_y, -1, 1));
float hangPincher = -(float) scaleInput(Range.clip(gamepad2.left_stick_y, -.5, .5));
// hangingPower.setPower(hangMove);
// Control motors for hanging the robot
if (hangMove > 0.1) hangingPower.setPower(hangMove);
else if (hangMove < -0.1) hangingPower.setPower(hangMove);
else hangingPower.setPower(0);
if (gamepad1.right_bumper) servoPlease.setPosition(.5);
else if (gamepad1.left_bumper) servoPlease.setPosition(0);
hangingPincer.setPower(hangPincher);
if (gamepad2.right_bumper) {
if (!hangPivotPressed) {
hangPivotPressed = true;
if (hangPivot == .75f) hangPivot = 1;
else if (hangPivot == 1) hangPivot = .75f;
}
} else if (hangPivotPressed) hangPivotPressed = false;
// Control motors for pivoting the hanger
// Power is based on how far the joystick is moved
if (gamepad2.dpad_up) {
hangingPivot.setPower(-hangPivot);
hangingPivot2.setPower(-hangPivot);
} else if (gamepad2.dpad_down) {
hangingPivot.setPower(hangPivot);
hangingPivot2.setPower(hangPivot);
} else {
// Stop the pivot motors
hangingPivot.setPower(0);
hangingPivot2.setPower(0);
}
// Control servos for controlling the wings
// *** X Y for left, A B for right
if (gamepad2.x) LWing.setPosition(0.0);
else if (gamepad2.y) LWing.setPosition(1.0);
else LWing.setPosition(0.5);
if (gamepad2.a) RWing.setPosition(1.0);
else if (gamepad2.b) RWing.setPosition(0.0);
else RWing.setPosition(0.5);
// Reverse the tank drive direction
if (gamepad1.start) {
if (!speedReversePressed) {
speedMultiplier *= -1;
speedReversePressed = true;
}
} else if (speedReversePressed) speedReversePressed = false;
SetDrivePower(speedMultiplier * rightY, speedMultiplier * leftY);
telemetry.addData("Drive Right", speedMultiplier * rightY);
telemetry.addData("Drive Left", speedMultiplier * leftY);
}