missing
8cdd3dd447
most important stuff: kinematics, autons, config, dashboard, easyopencv + april tags, and probably more im forgetting https://xkcd.com/1296/
115 lines
4.2 KiB
Java
115 lines
4.2 KiB
Java
package org.firstinspires.ftc.teamcode.teleop;
|
|
|
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
|
|
|
import org.firstinspires.ftc.teamcode.Hardware;
|
|
import org.firstinspires.ftc.teamcode.MotorPowers;
|
|
import org.firstinspires.ftc.teamcode.MovementAPI;
|
|
import org.firstinspires.ftc.teamcode.Strings;
|
|
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
|
|
import org.firstinspires.ftc.teamcode.kinematics.Poser;
|
|
|
|
@TeleOp(name = Strings.monoGamepadOpModeName)
|
|
public class MonoGamepad extends OpMode {
|
|
Hardware hardware;
|
|
MovementAPI movement;
|
|
double speed = 0.6;
|
|
LiftPosition targetLiftPos = null;
|
|
|
|
@Override
|
|
public void init() {
|
|
hardware = new Hardware(this);
|
|
movement = new MovementAPI(hardware);
|
|
|
|
hardware.setClawPosition(1);
|
|
hardware.wrist.setPosition(0.5);
|
|
}
|
|
|
|
@Override
|
|
public void loop() {
|
|
if (gamepad1.left_bumper) {
|
|
speed -= 0.005;
|
|
}
|
|
|
|
if (gamepad1.right_bumper) {
|
|
speed += 0.005;
|
|
}
|
|
|
|
speed = Math.max(0, Math.min(speed, 1));
|
|
|
|
if (gamepad1.dpad_up || gamepad1.dpad_down) {
|
|
targetLiftPos = null;
|
|
} else if (gamepad1.a) {
|
|
targetLiftPos = LiftPosition.HIGH;
|
|
} else if (gamepad1.b) {
|
|
targetLiftPos = LiftPosition.FLOOR;
|
|
}
|
|
|
|
int spoolPos = hardware.getLiftEncoder();
|
|
|
|
double pow;
|
|
if (targetLiftPos != null) {
|
|
int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
|
|
pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
|
|
} else {
|
|
pow = (gamepad1.dpad_up ? 1 : 0) + (gamepad1.dpad_down ? -1 : 0);
|
|
}
|
|
|
|
if (gamepad1.y) {
|
|
telemetry.addLine("lift status: !!! override set !!!");
|
|
} else {
|
|
if (spoolPos < -50) {
|
|
telemetry.addLine("lift status: too far below min, moving up");
|
|
pow = Math.max(pow, 0.2);
|
|
} else if (spoolPos < 0) {
|
|
telemetry.addLine("lift status: below min, stopping downward motion");
|
|
pow = Math.max(pow, 0);
|
|
} else if (spoolPos < 100) {
|
|
telemetry.addLine("lift status: close to min, slowing downward motion");
|
|
pow = Math.max(pow, -0.5);
|
|
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
|
|
telemetry.addLine("lift status: too far above max, moving down");
|
|
pow = Math.min(pow, -0.2);
|
|
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
|
|
telemetry.addLine("lift status: above max, stopping upward motion");
|
|
pow = Math.min(pow, 0);
|
|
} else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
|
|
telemetry.addLine("lift status: close to max, slowing upward motion");
|
|
pow = Math.min(pow, 0.5);
|
|
} else {
|
|
telemetry.addLine("lift status: ok");
|
|
}
|
|
}
|
|
|
|
telemetry.addLine("foobarbaz: " + pow);
|
|
hardware.spool.setPower(pow);
|
|
|
|
hardware.setClawPosition(1 - gamepad1.left_trigger);
|
|
hardware.setWristPosition((gamepad1.dpad_left ? 1 : 0) + (gamepad1.dpad_right ? -1 : 0));
|
|
|
|
MotorPowers powers = MotorPowers.calculate(
|
|
gamepad1.left_stick_x,
|
|
-gamepad1.left_stick_y,
|
|
gamepad1.right_stick_x,
|
|
speed
|
|
);
|
|
|
|
telemetry.addLine("fl = " + powers.fl);
|
|
telemetry.addLine("fr = " + powers.fr);
|
|
telemetry.addLine("bl = " + powers.bl);
|
|
telemetry.addLine("br = " + powers.br);
|
|
|
|
movement.move(powers);
|
|
|
|
telemetry.addLine("speed = " + speed);
|
|
telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
|
|
telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
|
|
telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
|
|
telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());
|
|
|
|
telemetry.addLine("lift encoder = " + spoolPos);
|
|
telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
|
|
}
|
|
}
|