diff --git a/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderAdafruitP4991.java b/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderAdafruitP4991.java index 7b0955b..b502cbe 100644 --- a/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderAdafruitP4991.java +++ b/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderAdafruitP4991.java @@ -9,8 +9,8 @@ import com.qualcomm.robotcore.util.RobotLog; import java.util.Arrays; -@I2cDeviceType -@DeviceProperties(name = "Adafruit P4991 Rotary Encoder QT", xmlTag = "Adafruit_Encoder_P4991") +//@I2cDeviceType +//@DeviceProperties(name = "Adafruit P4991 Rotary Encoder QT", xmlTag = "Adafruit_Encoder_P4991") public class EncoderAdafruitP4991 extends I2cDeviceSynchDevice { final Object i2cLock = new Object(); I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x36); diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java index 184b993..8b79301 100644 --- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java @@ -39,6 +39,7 @@ public abstract class CyberarmEngine extends OpMode { public boolean showStateChildrenListInTelemetry = false; private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2; + private boolean useThreads = true; /** * Called when INIT button on Driver Station is pushed @@ -112,6 +113,14 @@ public abstract class CyberarmEngine extends OpMode { return; } + if (!useThreads) { + threadlessExecState(state); + + for (CyberarmState task : backgroundTasks) { + threadlessExecState(task); + } + } + // Add telemetry to show currently running state telemetry.addLine( "Running state: " +state.getClass().getSimpleName() + ". State: " + @@ -212,18 +221,36 @@ public abstract class CyberarmEngine extends OpMode { final CyberarmState finalState = state; // if (state.isRunning()) { return; } // Assume that we have already started running this state - new Thread(() -> { + if (useThreads) { + new Thread(() -> { + finalState.prestart(); + finalState.start(); + finalState.startTime = System.currentTimeMillis(); + finalState.run(); + }).start(); + } else { finalState.prestart(); finalState.start(); finalState.startTime = System.currentTimeMillis(); - finalState.run(); - }).start(); + } for (CyberarmState kid : state.children) { runState(kid); } } + /** + * Recursively exec states + * @param state State to exec + */ + private void threadlessExecState(final CyberarmState state) { + state.exec(); + + for (CyberarmState kid : state.children) { + threadlessExecState(kid); + } + } + /** * Add state to queue, will call init() on state if engine is running * @param state State to add to queue @@ -472,14 +499,35 @@ public abstract class CyberarmEngine extends OpMode { lastActionName = action.name; lastActionNameSplit = lastActionName.split("-"); } - } catch (ClassNotFoundException | NoSuchMethodException | IllegalAccessException | InstantiationException | InvocationTargetException e) { + } catch (ClassNotFoundException | NoSuchMethodException | IllegalAccessException e) { e.printStackTrace(); RuntimeException exception = new RuntimeException(e.getMessage(), e.getCause()); exception.setStackTrace(e.getStackTrace()); throw(exception); + } catch (InvocationTargetException | InstantiationException e) { + Throwable cause = e.getCause(); + + if (cause != null) { + cause.printStackTrace(); + + RuntimeException exception = new RuntimeException(cause.getMessage(), cause.getCause()); + exception.setStackTrace(cause.getStackTrace()); + } else { + e.printStackTrace(); + + RuntimeException exception = new RuntimeException(e.getMessage(), e.getCause()); + exception.setStackTrace(e.getStackTrace()); + } } } } + + /** + * Disable running states in their own threads which might help with random crashes. + */ + public void threadless() { + useThreads = false; + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java index ed83fdc..55b4b8b 100644 --- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java @@ -45,6 +45,7 @@ public abstract class CyberarmState implements Runnable { /** * State's main loop, calls exec() until hasFinished is true * DO NO OVERRIDE + * blocking */ @Override public void run() { @@ -162,6 +163,7 @@ public abstract class CyberarmState implements Runnable { */ public void setHasFinished(boolean value) { hasFinished = value; + isRunning = value; // Dubious? } /** @@ -177,6 +179,7 @@ public abstract class CyberarmState implements Runnable { */ public void finished() { hasFinished = true; + isRunning = false; } /** diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 1cc2334..ee6925e 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -1,22 +1,23 @@ package dev.cyberarm.minibots.red_crab; +import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; -import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.tfod.TfodProcessor; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; - -import java.sql.Time; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.engine.V2.Utilities; @@ -30,47 +31,52 @@ public class RedCrabMinibot { public static final int ClawArm_COLLECT = 3; /// TUNING CONSTANTS /// - public static final double DRIVETRAIN_MAX_SPEED = 0.5; - public static final double CLAW_ARM_MAX_SPEED = 0.5; - public static final double CLAW_ARM_kP = 0.025; - public static final double CLAW_ARM_POSITION_TOLERANCE = 3.3; + public static double DRIVETRAIN_MAX_SPEED = 0.5; + + public static double CLAW_ARM_MAX_SPEED = 0.5; + public static double CLAW_ARM_kP = 0.1; + public static int CLAW_ARM_POSITION_TOLERANCE = 1; + public static double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 + public static double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 + public static double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; + public static double CLAW_ARM_COLLECT_ANGLE = 200.0; + public static final double WINCH_MAX_SPEED = 0.5; - public static final double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 - public static final double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 - public static final double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; - public static final double CLAW_ARM_COLLECT_ANGLE = 200.0; - public static final double CLAW_WRIST_STOW_POSITION = 0.5; - public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.64; - public static final double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64; - public static final double CLAW_WRIST_COLLECT_POSITION = 0.64; + public static double CLAW_WRIST_STOW_POSITION = 0.7; + public static double CLAW_WRIST_DEPOSIT_POSITION = 0.64; + public static double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64; + public static double CLAW_WRIST_COLLECT_POSITION = 0.64; - public static final double CLAW_LEFT_CLOSED_POSITION = 0.2; - public static final double CLAW_LEFT_OPEN_POSITION = 0.5; - public static final double CLAW_RIGHT_CLOSED_POSITION = 0.77; - public static final double CLAW_RIGHT_OPEN_POSITION = 0.5; + public static double CLAW_LEFT_CLOSED_POSITION = 0.2; + public static double CLAW_LEFT_OPEN_POSITION = 0.5; + public static double CLAW_RIGHT_CLOSED_POSITION = 0.77; + public static double CLAW_RIGHT_OPEN_POSITION = 0.5; - public static final double DRONE_LATCH_INITIAL_POSITION = 0.5; - public static final double DRONE_LATCH_LAUNCH_POSITION = 0.7; - public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; + public static double DRONE_LATCH_INITIAL_POSITION = 0.5; + public static double DRONE_LATCH_LAUNCH_POSITION = 0.7; + public static int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000; - public static final double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8 - public static final double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0 + public static double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8 + public static double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0 /// MOTOR CONSTANTS /// - public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; - public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 72; + public static int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4; + public static double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop /// HARDWARE /// public final IMU imu; - public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm; + public final MotorEx frontLeft, frontRight, backLeft, backRight, winch; + public final DcMotorEx clawArm; public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; public final MotorGroup left, right; final CyberarmEngine engine; - public final TimeCraftersConfiguration config; + public TimeCraftersConfiguration config; + private final PIDController clawArmPIDController; + public final String webcamName = "Webcam 1"; public enum Path { LEFT, @@ -78,22 +84,22 @@ public class RedCrabMinibot { RIGHT } - public final TfodProcessor tfod; - public final VisionPortal visionPortal; + /* --- VisionProcessors --- */ + /// Tensorflow Lite Pixel detector. + /// NOTE: detects april tags as pixels, use with caution! + public TfodProcessor tfPixel = null; + /// TeamProp detector: using OpenCV for subframe saturation threshold detection. + public TeamPropVisionProcessor teamProp = null; + /// Spike Mark detector: using OpenCV for full frame saturation threshold detection. + public SpikeMarkDetectorVisionProcessor spikeMark = null; + /// Doohickey + public VisionPortal visionPortal = null; public RedCrabMinibot(boolean autonomous) { engine = CyberarmEngine.instance; config = new TimeCraftersConfiguration("cyberarm_RedCrab"); - - if (autonomous) { - tfod = TfodProcessor.easyCreateWithDefaults(); - visionPortal = VisionPortal.easyCreateWithDefaults( - engine.hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); - } else { - tfod = null; - visionPortal = null; - } + loadConstants(); /// IMU /// /// ------------------------------------------------------------------------------------ /// @@ -104,6 +110,10 @@ public class RedCrabMinibot { RevHubOrientationOnRobot.UsbFacingDirection.UP)); imu.initialize(parameters); + if (autonomous) { + imu.resetYaw(); + } + /// DRIVE TRAIN /// /// ------------------------------------------------------------------------------------ /// frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ?? @@ -134,6 +144,19 @@ public class RedCrabMinibot { left = new MotorGroup(frontLeft, backLeft); right = new MotorGroup(frontRight, backRight); + /// --- MOTOR DISTANCE PER TICK + double gearRatio = config.variable("Robot", "Drivetrain_Tuning", "gear_ratio").value(); + double motorTicks = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value(); + double wheelDiameterMM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value(); + + double wheelCircumference = Math.PI * wheelDiameterMM; + double distancePerTick = (motorTicks * gearRatio) / wheelCircumference; // raw motor encoder * gear ratio + + frontLeft.setDistancePerPulse(distancePerTick); + frontRight.setDistancePerPulse(distancePerTick); + backLeft.setDistancePerPulse(distancePerTick); + backRight.setDistancePerPulse(distancePerTick); + /// WINCH /// /// ------------------------------------------------------------------------------------ /// winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ?? @@ -147,23 +170,23 @@ public class RedCrabMinibot { /// CLAW and Co. /// /// ------------------------------------------------------------------------------------ /// - clawArm = new MotorEx(engine.hardwareMap, "clawArm"); // | Ctrl|Ex Hub, Port: ?? + clawArmPIDController = new PIDController(0, 0, 0); + clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm"); // | Ctrl|Ex Hub, Port: ?? clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Ctrl|Ex Hub, Port: ?? leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Ctrl|Ex Hub, Port: ?? rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Ctrl|Ex Hub, Port: ?? /// --- Claw Arm Motor /// --- --- (SOFT) RESET MOTOR ENCODER - clawArm.resetEncoder(); + clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); /// --- --- DIRECTION - clawArm.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + clawArm.setDirection(DcMotorSimple.Direction.FORWARD); /// --- --- BRAKING /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃 - clawArm.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); /// --- --- Run Mode - clawArm.setRunMode(Motor.RunMode.PositionControl); - clawArm.setPositionCoefficient(CLAW_ARM_kP); - clawArm.setPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); + clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); clawArm.setTargetPosition(0); /// --- Claws @@ -188,6 +211,49 @@ public class RedCrabMinibot { // hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF: } + public void reloadConfig() { + config = new TimeCraftersConfiguration("cyberarm_RedCrab"); + + loadConstants(); + } + + private void loadConstants() { + /// Drivetrain + RedCrabMinibot.DRIVETRAIN_MAX_SPEED = config.variable("Robot", "Drivetrain_Tuning", "max_speed").value(); + + /// CLAW ARM + RedCrabMinibot.CLAW_ARM_MAX_SPEED = config.variable("Robot", "ClawArm_Tuning", "max_speed").value(); + RedCrabMinibot.CLAW_ARM_POSITION_TOLERANCE = config.variable("Robot", "ClawArm_Tuning", "tolerance").value(); + RedCrabMinibot.CLAW_ARM_STOW_ANGLE = config.variable("Robot", "ClawArm_Tuning", "stow_angle").value(); + RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_float_angle").value(); + RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value(); + RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value(); + + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value(); + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value(); + + /// CLAW WRIST + RedCrabMinibot.CLAW_WRIST_STOW_POSITION = config.variable("Robot", "ClawWrist_Tuning", "stow_position").value(); + RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "deposit_position").value(); + RedCrabMinibot.CLAW_WRIST_COLLECT_FLOAT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "float_collect_position").value(); + RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "collect_position").value(); + + /// CLAWS + RedCrabMinibot.CLAW_LEFT_CLOSED_POSITION = config.variable("Robot", "Claw_Tuning", "claw_left_closed_position").value(); + RedCrabMinibot.CLAW_LEFT_OPEN_POSITION = config.variable("Robot", "Claw_Tuning", "claw_left_open_position").value(); + RedCrabMinibot.CLAW_RIGHT_CLOSED_POSITION = config.variable("Robot", "Claw_Tuning", "claw_right_closed_position").value(); + RedCrabMinibot.CLAW_RIGHT_OPEN_POSITION = config.variable("Robot", "Claw_Tuning", "claw_right_open_position").value(); + + /// HOOK ARM + RedCrabMinibot.HOOK_ARM_STOW_POSITION = config.variable("Robot", "HookArm_Tuning", "stow_position").value(); + RedCrabMinibot.HOOK_ARM_UP_POSITION = config.variable("Robot", "HookArm_Tuning", "up_position").value(); + + /// DRONE LATCH + RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "initial_position").value(); + RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value(); + RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value(); + } + public void standardTelemetry() { engine.telemetry.addLine(); @@ -228,10 +294,11 @@ public class RedCrabMinibot { engine.telemetry.addData( "Claw Arm", - "Power: %.2f, Current: %.2f mAmp, Position: %d", - clawArm.motorEx.getPower(), - clawArm.motorEx.getCurrent(CurrentUnit.MILLIAMPS), - clawArm.motorEx.getCurrentPosition()); + "Power: %.2f, Current: %.2f mAmp, Position: %d, Velocity: %.2f", + clawArm.getPower(), + clawArm.getCurrent(CurrentUnit.MILLIAMPS), + clawArm.getCurrentPosition(), + clawArm.getVelocity()); engine.telemetry.addLine(); engine.telemetry.addLine("Servos"); @@ -248,4 +315,37 @@ public class RedCrabMinibot { engine.telemetry.addLine(); } + + public void controlClawArm() { + Action action = config.action("Robot", "ClawArm_Tuning"); + + double p = 0.0, i = 0.0, d = 0.0, f = 0.0; + double ticksInDegree = Utilities.motorAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1); + + for (Variable v : action.getVariables()) { + switch (v.name.trim()) { + case "kP": + p = v.value(); + break; + case "kI": + i = v.value(); + break; + case "kD": + d = v.value(); + break; + case "kF": // feedback + f = v.value(); + break; + } + } + + clawArmPIDController.setPID(p, i, d); + int armPos = clawArm.getCurrentPosition(); + double pid = clawArmPIDController.calculate(armPos, clawArm.getTargetPosition()); + double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f; + + double power = pid + ff; + + clawArm.setPower(power); + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java new file mode 100644 index 0000000..7301120 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java @@ -0,0 +1,117 @@ +package dev.cyberarm.minibots.red_crab; + +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.Paint; +import android.graphics.Typeface; + +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionProcessor; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +public class SpikeMarkDetectorVisionProcessor implements VisionProcessor { + public enum Selection { + NONE, + LINE + } + + private enum NamedColorPart { + HUE, + SATURATION, + VALUE + } + + private Selection selection = Selection.NONE; + + /// NOTE: Rect defined with a 640x480 (WxH) frame size assumed + private final Rect rect = new Rect(1, 1, 639, 479); + private Mat subMat = new Mat(); + private Mat rotatedMat = new Mat(); + private Mat hsvMat = new Mat(); + + private double saturation; + + @Override + public void init(int width, int height, CameraCalibration calibration) { + } + + @Override + public Object processFrame(Mat frame, long captureTimeNanos) { + Core.rotate(frame, rotatedMat,Core.ROTATE_180); + Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV); + + saturation = averageSaturation(hsvMat, rect); + + // TODO: Tune this value or do more processing + if (saturation > 0.5) { + selection = Selection.LINE; + } else { + selection = Selection.NONE; + } + + return selection; + } + + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + Paint selectedPaint = new Paint(); + selectedPaint.setColor(Color.GREEN); + selectedPaint.setStyle(Paint.Style.STROKE); + selectedPaint.setStrokeWidth(scaleCanvasDensity * 4); + selectedPaint.setTextSize(scaleCanvasDensity * 28); + selectedPaint.setTextAlign(Paint.Align.CENTER); + selectedPaint.setTypeface(Typeface.MONOSPACE); + + Paint notSelectedPaint = new Paint(); + notSelectedPaint.setColor(Color.WHITE); + notSelectedPaint.setStyle(Paint.Style.STROKE); + notSelectedPaint.setStrokeWidth(scaleCanvasDensity * 4); + selectedPaint.setTextSize(scaleCanvasDensity * 28); + selectedPaint.setTextAlign(Paint.Align.CENTER); + selectedPaint.setTypeface(Typeface.MONOSPACE); + + android.graphics.Rect drawRect = makeDrawableRect(rect, scaleBmpPxToCanvasPx); + + float textYOffset = selectedPaint.getTextSize() + selectedPaint.getStrokeWidth(); + + switch ((Selection) userContext) { + case LINE: + canvas.drawRect(drawRect, notSelectedPaint); + + canvas.drawText("LINE!", drawRect.centerX(), drawRect.bottom - textYOffset, selectedPaint); + break; + case NONE: + canvas.drawRect(drawRect, notSelectedPaint); + break; + } + } + + private double averageSaturation(Mat input, Rect rect) { + // NOTE: Check whether this submat Mat leaks memory + subMat = input.submat(rect); + Scalar color = Core.mean(subMat); + + return color.val[NamedColorPart.SATURATION.ordinal()]; + } + + public Selection getSelection() { + return selection; + } + + public double getSaturation() { + return saturation; + } + + private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) { + int left = Math.round(rect.x * scaleBmpPxToCanvasPx); + int top = Math.round(rect.y * scaleBmpPxToCanvasPx); + int right = left + Math.round(rect.width * scaleBmpPxToCanvasPx); + int bottom = top + Math.round(rect.height * scaleBmpPxToCanvasPx); + + return new android.graphics.Rect(left, top, right, bottom); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TODO.md b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TODO.md new file mode 100644 index 0000000..4405789 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TODO.md @@ -0,0 +1,6 @@ +# Red Crab / Copy Bot +## TODO LIST +- [ ] Test CyberarmEngine threadless mode +- [ ] Verify drivetrain motor gearbox modules are with highest gear reduction closest to the motor. +- [ ] Tune ClawArm PIDF +- [ ] Test and tune TeamProp and SpikeMark VisionProcessor diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java new file mode 100644 index 0000000..acf432b --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java @@ -0,0 +1,156 @@ +package dev.cyberarm.minibots.red_crab; + +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.Paint; +import android.graphics.Typeface; + +import com.acmerobotics.dashboard.canvas.Stroke; + +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionProcessor; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +public class TeamPropVisionProcessor implements VisionProcessor { + public enum Location { + NONE, + LEFT, + CENTER, + RIGHT + } + + private enum NamedColorPart { + HUE, + SATURATION, + VALUE + } + + private Location location = Location.NONE; + + /// NOTE: Rects are defined with a 640x480 (WxH) frame size assumed + // 640 / 3 = ~212 + private final Rect rectLeft = new Rect(1, 1, 639, 160); + private final Rect rectCenter = new Rect(rectLeft.x + rectLeft.width, 1, 639, 160); + private final Rect rectRight = new Rect(rectCenter.x + rectCenter.width, 1, 639, 160); + private Mat subMat = new Mat(); + private Mat rotatedMat = new Mat(); + private Mat hsvMat = new Mat(); + private double saturationLeft; + private double saturationCenter; + private double saturationRight; + + @Override + public void init(int width, int height, CameraCalibration calibration) { + } + + @Override + public Object processFrame(Mat frame, long captureTimeNanos) { + Core.rotate(frame, rotatedMat,Core.ROTATE_180); + Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV); + + saturationLeft = averageSaturation(hsvMat, rectLeft); + saturationCenter = averageSaturation(hsvMat, rectLeft); + saturationRight = averageSaturation(hsvMat, rectLeft); + + if (saturationLeft > saturationCenter && saturationLeft > saturationRight) { + location = Location.LEFT; + } else if (saturationCenter > saturationLeft && saturationCenter > saturationRight) { + location = Location.CENTER; + } else { + location = Location.RIGHT; + } + + return location; + } + + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + Paint selectedPaint = new Paint(); + selectedPaint.setColor(Color.GREEN); + selectedPaint.setStyle(Paint.Style.STROKE); + selectedPaint.setStrokeWidth(scaleCanvasDensity * 4); + selectedPaint.setTextSize(scaleCanvasDensity * 28); + selectedPaint.setTextAlign(Paint.Align.CENTER); + selectedPaint.setTypeface(Typeface.MONOSPACE); + + Paint notSelectedPaint = new Paint(); + notSelectedPaint.setColor(Color.WHITE); + notSelectedPaint.setStyle(Paint.Style.STROKE); + notSelectedPaint.setStrokeWidth(scaleCanvasDensity * 4); + selectedPaint.setTextSize(scaleCanvasDensity * 28); + selectedPaint.setTextAlign(Paint.Align.CENTER); + selectedPaint.setTypeface(Typeface.MONOSPACE); + + android.graphics.Rect drawRectLeft = makeDrawableRect(rectLeft, scaleBmpPxToCanvasPx); + android.graphics.Rect drawRectCenter = makeDrawableRect(rectCenter, scaleBmpPxToCanvasPx); + android.graphics.Rect drawRectRight = makeDrawableRect(rectRight, scaleBmpPxToCanvasPx); + + float textYOffset = selectedPaint.getTextSize() + selectedPaint.getStrokeWidth(); + + switch ((Location) userContext) { + case LEFT: + canvas.drawRect(drawRectLeft, selectedPaint); + canvas.drawRect(drawRectCenter, notSelectedPaint); + canvas.drawRect(drawRectRight, notSelectedPaint); + + canvas.drawText("LEFT", drawRectCenter.centerX(), drawRectCenter.bottom - textYOffset, selectedPaint); + break; + case CENTER: + canvas.drawRect(drawRectLeft, notSelectedPaint); + canvas.drawRect(drawRectCenter, selectedPaint); + canvas.drawRect(drawRectRight, notSelectedPaint); + + canvas.drawText("CENTER", drawRectCenter.centerX(), drawRectCenter.bottom - textYOffset, selectedPaint); + break; + case RIGHT: + canvas.drawRect(drawRectLeft, notSelectedPaint); + canvas.drawRect(drawRectCenter, notSelectedPaint); + canvas.drawRect(drawRectRight, selectedPaint); + + canvas.drawText("RIGHT", drawRectRight.centerX(), drawRectRight.bottom - textYOffset, selectedPaint); + break; + case NONE: + canvas.drawRect(drawRectLeft, notSelectedPaint); + canvas.drawRect(drawRectCenter, notSelectedPaint); + canvas.drawRect(drawRectRight, notSelectedPaint); + break; + } + } + + private double averageSaturation(Mat input, Rect rect) { + // NOTE: Check whether this submat Mat leaks memory + subMat = input.submat(rect); + Scalar color = Core.mean(subMat); + + return color.val[NamedColorPart.SATURATION.ordinal()]; + } + + public Location getLocation() { + return location; + } + + public double getSaturationLeft() { + return saturationLeft; + } + + public double getSaturationCenter() { + return saturationCenter; + } + + public double getSaturationRight() { + return saturationRight; + } + + private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) { + int left = Math.round(rect.x * scaleBmpPxToCanvasPx); + int top = Math.round(rect.y * scaleBmpPxToCanvasPx); + int right = left + Math.round(rect.width * scaleBmpPxToCanvasPx); + int bottom = top + Math.round(rect.height * scaleBmpPxToCanvasPx); + + return new android.graphics.Rect(left, top, right, bottom); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json new file mode 100644 index 0000000..3ede577 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json @@ -0,0 +1 @@ +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-17 10:19:36 -0600","spec_version":2,"revision":277},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ServoMove - Move wrist","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.7"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"00-01","comment":"@ServoMove - Move left claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.2"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"00-02","comment":"@ServoMove - Move right claw","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.79"},{"name":"servoName","value":"SxrightClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"01-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx3000"},{"name":"lerpMM_DOWN","value":"Dx1000"},{"name":"lerpMM_UP","value":"Dx1000"},{"name":"maxPower","value":"Dx0.35"},{"name":"minPower","value":"Dx0.25"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"01-01","comment":"@ClawArmMove - position arm","enabled":true,"variables":[{"name":"angle","value":"Dx150.0"},{"name":"power","value":"Dx0.25"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"01-02","comment":"@ServoMove - Move wrist into position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx450"},{"name":"position","value":"Dx0.64"},{"name":"servoName","value":"SxclawWrist"},{"name":"timeoutMS","value":"Ix500"}]},{"name":"02-00","comment":"@Rotate - Rotate robot 0","enabled":false,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"03-00","comment":"@Rotate - Rotate robot 180","enabled":false,"variables":[{"name":"headingDEGREES","value":"Dx180"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.75"},{"name":"minPower","value":"Dx0.15"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx1.5"}]},{"name":"04-00","comment":"@Move - Strafe towards backstage","enabled":false,"variables":[{"name":"distanceMM","value":"Dx1000"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxPower","value":"Dx0.5"},{"name":"minPower","value":"Dx0.25"},{"name":"strafe","value":"Bxtrue"},{"name":"timeoutMS","value":"Ix50000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"20-00","comment":"@PathSelector - Pick path; sense Center","enabled":false,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx200.0"},{"name":"collect_float_angle","value":"Dx180"},{"name":"deposit_angle","value":"Dx130.0"},{"name":"gear_ratio","value":"Dx80"},{"name":"kD","value":"Dx0.05"},{"name":"kF","value":"Dx0.01"},{"name":"kI","value":"Dx0.1"},{"name":"kP","value":"Dx0.4"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Dx4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.64"},{"name":"collect_position","value":"Dx0.64"},{"name":"deposit_position","value":"Dx0.64"},{"name":"stow_position","value":"Dx0.2"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx20.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Dx28"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0.7"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxPower","value":"Dx0.75"},{"name":"minPower","value":"Dx0.15"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx0"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.75"},{"name":"minPower","value":"Dx0.15"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java index 0262035..0843747 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_BLUE_Audience" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java index e14b4cf..bd646d8 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_BLUE_Backstage" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java index b15f54c..56fe2bb 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_RED_Audience" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java index e1ab5e0..67cb487 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java @@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.ClawArmTask; @Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine { @Override public void setup() { + RedCrabMinibot robot = new RedCrabMinibot(true); + blackboardSet("clawArmPower", 0.0); + + addTask(new ClawArmTask(robot)); + setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(true), + robot, RedCrabMinibot.class, "Autonomous_RED_Backstage" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java index 45d138e..6e59e91 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java @@ -4,12 +4,17 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.states.ClawArmTask; import dev.cyberarm.minibots.red_crab.states.Pilot; @TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT") public class RedCrabTeleOpEngine extends CyberarmEngine { @Override public void setup() { - addState(new Pilot(new RedCrabMinibot(false))); + RedCrabMinibot robot = new RedCrabMinibot(false); + + addTask(new ClawArmTask(robot)); + + addState(new Pilot(robot)); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java index 66b1f37..1246343 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java @@ -1,5 +1,6 @@ package dev.cyberarm.minibots.red_crab.states; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; @@ -25,14 +26,16 @@ public class ClawArmMove extends CyberarmState { @Override public void start() { - robot.clawArm.setPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); + robot.clawArm.setTargetPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); - robot.clawArm.set(power); } @Override public void exec() { - if (robot.clawArm.atTargetPosition() || runTime() >= timeoutMS) { + int tolerance = robot.clawArm.getTargetPositionTolerance(); + int position = robot.clawArm.getCurrentPosition(); + + if (Utilities.isBetween(position, position - tolerance, position + tolerance) || runTime() >= timeoutMS) { this.finished(); } } @@ -40,8 +43,11 @@ public class ClawArmMove extends CyberarmState { @Override public void telemetry() { engine.telemetry.addLine(); + engine.telemetry.addData("Motor Power", robot.clawArm.getPower()); engine.telemetry.addData("Motor Position", robot.clawArm.getCurrentPosition()); engine.telemetry.addData("Motor Angle", Utilities.motorAngle(motorTicks, gearRatio, robot.clawArm.getCurrentPosition())); + engine.telemetry.addData("Motor Target Position", Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); + engine.telemetry.addData("Motor Target Angle", targetAngle); engine.telemetry.addData("Timeout MS", timeoutMS); progressBar(20, runTime() / timeoutMS); } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java index 4ed845a..1e14eec 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java @@ -1,2 +1,16 @@ -package dev.cyberarm.minibots.red_crab.states;public class ClawArmTask { +package dev.cyberarm.minibots.red_crab.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class ClawArmTask extends CyberarmState { + private final RedCrabMinibot robot; + public ClawArmTask(RedCrabMinibot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.controlClawArm(); + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java index d363b6b..6701fc6 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java @@ -93,13 +93,13 @@ public class Move extends CyberarmState { engine.telemetry.addData("lerp MM UP", lerpMM_UP); engine.telemetry.addData("lerp MM DOWN", lerpMM_DOWN); engine.telemetry.addData("Distance MM", distanceMM); - engine.telemetry.addData("Distance Travelled MM", (strafe ? robot.left.getDistance() : robot.frontLeft.getDistance())); + engine.telemetry.addData("Distance Travelled MM", robot.frontLeft.getDistance()); engine.telemetry.addData("Timeout MS", timeoutMS); progressBar(20, runTime() / timeoutMS); } private void tankMove(){ - double travelledDistance = Math.abs(robot.left.getDistance()); + double travelledDistance = Math.abs(robot.frontLeft.getDistance()); double power = lerpPower(travelledDistance); double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu)); @@ -107,17 +107,19 @@ public class Move extends CyberarmState { double leftPower = power; double rightPower = power; // use +10% of power at 7 degrees of error to correct angle - double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1); - if (angleDiff < -0.5) { + double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.1); + if (angleDiff > -0.5) { leftPower += correctivePower; - } else if (angleDiff > 0.5) { + } else if (angleDiff < 0.5) { rightPower += correctivePower; } robot.left.set(leftPower); robot.right.set(rightPower); - if (robot.left.atTargetPosition() && robot.right.atTargetPosition()) { + if (runTime() >= timeoutMS || + (robot.frontLeft.atTargetPosition() || robot.frontRight.atTargetPosition()) || + Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM)) { robot.left.set(0); robot.right.set(0); @@ -133,11 +135,11 @@ public class Move extends CyberarmState { double frontPower = power; double backPower = power; - // use +10% of power at 7 degrees of error to correct angle - double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1); - if (angleDiff < -0.5) { + // use +40% of power at 7 degrees of error to correct angle + double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.40); + if (angleDiff > -0.5) { frontPower += correctivePower; - } else if (angleDiff > 0.5) { + } else if (angleDiff < 0.5) { backPower += correctivePower; } @@ -146,7 +148,8 @@ public class Move extends CyberarmState { robot.backLeft.set(-backPower); robot.backRight.set(backPower); - if (robot.frontLeft.atTargetPosition() && robot.backRight.atTargetPosition()) { + if (runTime() >= timeoutMS || (robot.frontLeft.atTargetPosition() || robot.backRight.atTargetPosition()) || + Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM) || Math.abs(robot.backRight.getDistance()) >= Math.abs(distanceMM)) { robot.frontLeft.set(0); robot.frontRight.set(0); robot.backLeft.set(0); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java index 8780c82..b36aaf2 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java @@ -1,6 +1,9 @@ package dev.cyberarm.minibots.red_crab.states; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; import java.util.ArrayList; import java.util.List; @@ -11,49 +14,60 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot; public class PathSelector extends CyberarmState { private final RedCrabMinibot robot; private final int timeoutMS; - private final int fallbackPath; + private final int path; + private final double minConfidence; private List recognitions = new ArrayList<>(); public PathSelector(RedCrabMinibot robot, String groupName, String actionName) { this.robot = robot; this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); - this.fallbackPath = robot.config.variable(groupName, actionName, "fallbackPath").value(); + this.path = robot.config.variable(groupName, actionName, "path").value(); + this.minConfidence = robot.config.variable(groupName, actionName, "minConfidence").value(); } @Override public void init() { - robot.tfod.setClippingMargins(0, 0, 0, 0); - robot.tfod.setMinResultConfidence(0.8f); + robot.tfPixel = TfodProcessor.easyCreateWithDefaults(); + robot.visionPortal = VisionPortal.easyCreateWithDefaults( + engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.tfPixel); - engine.blackboardSet("autonomousPath", fallbackPath); + robot.tfPixel.setClippingMargins(0, 0, 0, 0); + robot.tfPixel.setMinResultConfidence((float) minConfidence); + + engine.blackboardSet("autonomousPath", 0); } @Override public void exec() { - recognitions = robot.tfod.getRecognitions(); + if (engine.blackboardGetInt("autonomousPath") != 0) { + this.finished(); + + return; + } + + recognitions = robot.tfPixel.getRecognitions(); for (Recognition recognition : recognitions) { double x = (recognition.getLeft() + recognition.getRight()) / 2; double y = (recognition.getTop() + recognition.getBottom()) / 2; if (recognition.getLabel().equals("pixel")) { - if (x < 120) { - engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.LEFT.ordinal()); - } else if (x >= 120 && x < 240) { - engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.CENTER.ordinal()); - } else { - engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.RIGHT.ordinal()); - } + engine.blackboardSet("autonomousPath", path); } } if (runTime() >= timeoutMS) { - robot.visionPortal.close(); - robot.tfod.shutdown(); + stopVision(); + finished(); } } + private void stopVision() { + robot.visionPortal.close(); + robot.tfPixel.shutdown(); + } + @Override public void telemetry() { engine.telemetry.addLine(); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 4db680d..6ee06ee 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -69,6 +69,9 @@ public class Pilot extends CyberarmState { case "dpad_right": clawArmPosition = RedCrabMinibot.ClawArm_COLLECT_FLOAT; break; + case "start": + robot.reloadConfig(); + break; } } } @@ -164,9 +167,9 @@ public class Pilot extends CyberarmState { RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) { - robot.clawArm.set(0); - } else { - robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED); +// robot.clawArm.setPower(0); +// } else { +// robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java index e9d640f..454e4a0 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java @@ -13,6 +13,7 @@ public class Rotate extends CyberarmState { final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees; final private int timeoutMS; + private boolean commitToRotation = false; public Rotate(RedCrabMinibot robot, String groupName, String actionName) { this.robot = robot; @@ -25,7 +26,7 @@ public class Rotate extends CyberarmState { this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value(); this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value(); - this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREEES").value(); + this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREES").value(); this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); } @@ -45,13 +46,17 @@ public class Rotate extends CyberarmState { double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0)); - if (angleDiff > 0) { - robot.left.set(-power); - robot.right.set(power); - } else { - robot.left.set(power); - robot.right.set(-power); + if (!commitToRotation) { + if (angleDiff < 0) { + robot.left.set(-power); + robot.right.set(power); + } else { + robot.left.set(power); + robot.right.set(-power); + } } + + commitToRotation = Math.abs(angleDiff) > 170; } @Override