83 lines
2.7 KiB
Java
83 lines
2.7 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
public abstract class Gamepad extends OpModeWithMovement {
|
|
protected double speed = 0.7;
|
|
protected boolean slowMode = false;
|
|
protected boolean prevX = false;
|
|
protected boolean armUp = false;
|
|
protected boolean prevL3 = false;
|
|
protected double armHeight = 0;
|
|
|
|
@Override
|
|
public void init() {
|
|
this.construct();
|
|
|
|
api.print("Status", "Initializing, please wait");
|
|
api.pause(1);
|
|
|
|
// motors.shoulder1.runToPosition(0);
|
|
// motors.shoulder2.runToPosition(0);
|
|
// motors.shoulder1.setMode(API.MotorMode.RUN_TO_POSITION);
|
|
// motors.shoulder2.setMode(API.MotorMode.RUN_TO_POSITION);
|
|
// motors.shoulder1.start(1);
|
|
// motors.shoulder2.start(1);
|
|
motors.shoulder2.setDirection(API.Direction.REVERSE);
|
|
|
|
api.clear();
|
|
api.print("Press play to start");
|
|
}
|
|
|
|
@Override
|
|
public void start() {
|
|
api.clear();
|
|
}
|
|
|
|
@Override
|
|
public void loop() {
|
|
// double ms = getRuntime() / 1000;
|
|
|
|
double currentSpeed = gamepad1.y ? 1 : slowMode ? 0.35 : speed;
|
|
movement.move(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, currentSpeed, true);
|
|
|
|
api.print("Speed", Double.toString(speed));
|
|
api.print("Slow mode", slowMode ? "true" : "false");
|
|
api.print("Arm up", armUp ? "true" : "false");
|
|
api.print("Arm height", Double.toString(armHeight));
|
|
|
|
if (gamepad1.x && !prevX) slowMode = !slowMode;
|
|
if (gamepad2.left_stick_button && !prevL3) armUp = !armUp;
|
|
|
|
if (gamepad1.right_bumper) speed = Math.min(speed + 0.01, 1);
|
|
else if (gamepad1.left_bumper) speed = Math.max(speed - 0.01, 0.2);
|
|
|
|
motors.carousel.controlWithTwoButtons(gamepad2.x, gamepad2.y, 0.9);
|
|
|
|
// motors.shoulder1.runToPosition((int)armHeight);
|
|
// motors.shoulder2.runToPosition((int)armHeight);
|
|
motors.shoulder1.start(gamepad2.left_stick_y);
|
|
motors.shoulder2.start(gamepad2.left_stick_y);
|
|
motors.elbow.start(gamepad2.right_stick_y);
|
|
|
|
// armHeight += gamepad2.left_stick_y;
|
|
|
|
// if (gamepad2.a && !gamepad2.b) posThingy += 0.05; //motors.hand.start(1);
|
|
// else if (!gamepad2.a && gamepad2.b) posThingy -= 0.05; //motors.hand.start(-1);
|
|
// else motors.hand.stop();
|
|
|
|
// motors.hand.setPosition(posThingy);
|
|
|
|
// System.out.println(posThingy);
|
|
|
|
if (armUp) {
|
|
motors.hand.start(1);
|
|
} else {
|
|
motors.hand.start(-1);
|
|
}
|
|
|
|
prevX = gamepad1.x;
|
|
prevL3 = gamepad2.left_stick_button;
|
|
|
|
// while (getRuntime() / 1000 - ms < 15) ; // no empty while loop here!
|
|
}
|
|
}
|