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 ee6925e..0871bd3 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,6 +1,7 @@ package dev.cyberarm.minibots.red_crab; import com.arcrobotics.ftclib.controller.PIDController; +import com.arcrobotics.ftclib.controller.PIDFController; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; @@ -10,6 +11,7 @@ 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 com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; @@ -34,6 +36,7 @@ public class RedCrabMinibot { public static double DRIVETRAIN_MAX_SPEED = 0.5; public static double CLAW_ARM_MAX_SPEED = 0.5; + public static double CLAW_ARM_VELOCITY_DEGREES = 10; 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 @@ -75,7 +78,7 @@ public class RedCrabMinibot { final CyberarmEngine engine; public TimeCraftersConfiguration config; - private final PIDController clawArmPIDController; + private final PIDFController clawArmPIDFController; public final String webcamName = "Webcam 1"; public enum Path { @@ -170,7 +173,7 @@ public class RedCrabMinibot { /// CLAW and Co. /// /// ------------------------------------------------------------------------------------ /// - clawArmPIDController = new PIDController(0, 0, 0); + clawArmPIDFController = new PIDFController(0.4, 0.01, 0.1, 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: ?? @@ -185,7 +188,7 @@ public class RedCrabMinibot { /// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃 clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); /// --- --- Run Mode - clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + clawArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); clawArm.setTargetPositionTolerance(CLAW_ARM_POSITION_TOLERANCE); clawArm.setTargetPosition(0); @@ -223,6 +226,7 @@ public class RedCrabMinibot { /// CLAW ARM RedCrabMinibot.CLAW_ARM_MAX_SPEED = config.variable("Robot", "ClawArm_Tuning", "max_speed").value(); + RedCrabMinibot.CLAW_ARM_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "velocityDEGREES").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(); @@ -235,7 +239,7 @@ public class RedCrabMinibot { /// 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_FLOAT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "collect_float_position").value(); RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION = config.variable("Robot", "ClawWrist_Tuning", "collect_position").value(); /// CLAWS @@ -299,6 +303,7 @@ public class RedCrabMinibot { clawArm.getCurrent(CurrentUnit.MILLIAMPS), clawArm.getCurrentPosition(), clawArm.getVelocity()); + engine.telemetry.addData("PIDF", "P: %.8f, I: %.8f, D: %.8f, F: %.8f", clawArmPIDFController.getP(), clawArmPIDFController.getI(), clawArmPIDFController.getD(), clawArmPIDFController.getF()); engine.telemetry.addLine(); engine.telemetry.addLine("Servos"); @@ -339,13 +344,14 @@ public class RedCrabMinibot { } } - clawArmPIDController.setPID(p, i, d); + clawArmPIDFController.setPIDF(p, i, d, f); int armPos = clawArm.getCurrentPosition(); - double pid = clawArmPIDController.calculate(armPos, clawArm.getTargetPosition()); - double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f; + double pid = clawArmPIDFController.calculate(armPos, clawArm.getTargetPosition()); +// double ff = Math.cos(Math.toRadians(clawArm.getTargetPosition() / ticksInDegree)) * f; - double power = pid + ff; + double power = Range.clip(pid, -CLAW_ARM_MAX_SPEED, CLAW_ARM_MAX_SPEED); - clawArm.setPower(power); + clawArm.setVelocityPIDFCoefficients(p, i, d, f); + clawArm.setVelocity(CLAW_ARM_VELOCITY_DEGREES, AngleUnit.DEGREES); } } 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 index 7301120..6cfd3fd 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java @@ -47,7 +47,7 @@ public class SpikeMarkDetectorVisionProcessor implements VisionProcessor { saturation = averageSaturation(hsvMat, rect); // TODO: Tune this value or do more processing - if (saturation > 0.5) { + if (saturation > 75.0) { selection = Selection.LINE; } else { selection = Selection.NONE; @@ -82,7 +82,7 @@ public class SpikeMarkDetectorVisionProcessor implements VisionProcessor { case LINE: canvas.drawRect(drawRect, notSelectedPaint); - canvas.drawText("LINE!", drawRect.centerX(), drawRect.bottom - textYOffset, selectedPaint); + canvas.drawText("LINE!", drawRect.centerX(), textYOffset, selectedPaint); break; case NONE: canvas.drawRect(drawRect, notSelectedPaint); 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 index acf432b..bb1fbdf 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java @@ -33,9 +33,9 @@ public class TeamPropVisionProcessor implements VisionProcessor { /// 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 final Rect rectLeft = new Rect(1, 1, 212, 479); + private final Rect rectCenter = new Rect(rectLeft.x + rectLeft.width, 1, 212, 479); + private final Rect rectRight = new Rect(rectCenter.x + rectCenter.width, 1, 212, 479); private Mat subMat = new Mat(); private Mat rotatedMat = new Mat(); private Mat hsvMat = new Mat(); @@ -53,8 +53,8 @@ public class TeamPropVisionProcessor implements VisionProcessor { Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV); saturationLeft = averageSaturation(hsvMat, rectLeft); - saturationCenter = averageSaturation(hsvMat, rectLeft); - saturationRight = averageSaturation(hsvMat, rectLeft); + saturationCenter = averageSaturation(hsvMat, rectCenter); + saturationRight = averageSaturation(hsvMat, rectRight); if (saturationLeft > saturationCenter && saturationLeft > saturationRight) { location = Location.LEFT; @@ -79,12 +79,21 @@ public class TeamPropVisionProcessor implements VisionProcessor { Paint notSelectedPaint = new Paint(); notSelectedPaint.setColor(Color.WHITE); + notSelectedPaint.setAlpha(127); notSelectedPaint.setStyle(Paint.Style.STROKE); notSelectedPaint.setStrokeWidth(scaleCanvasDensity * 4); selectedPaint.setTextSize(scaleCanvasDensity * 28); selectedPaint.setTextAlign(Paint.Align.CENTER); selectedPaint.setTypeface(Typeface.MONOSPACE); + Paint whitePaint = new Paint(); + whitePaint.setColor(Color.WHITE); + whitePaint.setStyle(Paint.Style.FILL_AND_STROKE); + whitePaint.setStrokeWidth(scaleCanvasDensity * 4); + whitePaint.setTextSize(scaleCanvasDensity * 28); + whitePaint.setTextAlign(Paint.Align.CENTER); + whitePaint.setTypeface(Typeface.MONOSPACE); + android.graphics.Rect drawRectLeft = makeDrawableRect(rectLeft, scaleBmpPxToCanvasPx); android.graphics.Rect drawRectCenter = makeDrawableRect(rectCenter, scaleBmpPxToCanvasPx); android.graphics.Rect drawRectRight = makeDrawableRect(rectRight, scaleBmpPxToCanvasPx); @@ -97,21 +106,21 @@ public class TeamPropVisionProcessor implements VisionProcessor { canvas.drawRect(drawRectCenter, notSelectedPaint); canvas.drawRect(drawRectRight, notSelectedPaint); - canvas.drawText("LEFT", drawRectCenter.centerX(), drawRectCenter.bottom - textYOffset, selectedPaint); + canvas.drawText("LEFT", drawRectLeft.centerX(), 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); + canvas.drawText("CENTER", drawRectCenter.centerX(), 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); + canvas.drawText("RIGHT", drawRectRight.centerX(), textYOffset, selectedPaint); break; case NONE: canvas.drawRect(drawRectLeft, notSelectedPaint); @@ -119,6 +128,10 @@ public class TeamPropVisionProcessor implements VisionProcessor { canvas.drawRect(drawRectRight, notSelectedPaint); break; } + canvas.drawText(String.format("%.4f", getSaturationLeft()), drawRectLeft.centerX(), textYOffset + whitePaint.getTextSize() + textYOffset, whitePaint); + canvas.drawText(String.format("%.4f", getSaturationCenter()), drawRectCenter.centerX(), textYOffset + whitePaint.getTextSize() + textYOffset, whitePaint); + canvas.drawText(String.format("%.4f", getSaturationRight()), drawRectRight.centerX(), textYOffset + whitePaint.getTextSize() + textYOffset, whitePaint); + } private double averageSaturation(Mat input, Rect rect) { 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 index 3ede577..44f571b 100644 --- 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 @@ -1 +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 +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-19 21:55:01 -0600","spec_version":2,"revision":403},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx5000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix8000"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx2000"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxPower","value":"Dx0.35"},{"name":"minPower","value":"Dx0.25"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot 0","enabled":true,"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":"04-00","comment":"@PathEnactor - Enact path","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90 degrees","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxPower","value":"Dx0.3"},{"name":"minPower","value":"Dx0.12"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceDEGREES","value":"Dx1"}]},{"name":"06-00","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx3000"},{"name":"lerpMM_DOWN","value":"Dx1000"},{"name":"lerpMM_UP","value":"Dx1000"},{"name":"maxPower","value":"Dx0.5"},{"name":"minPower","value":"Dx0.15"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"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.0"},{"name":"kF","value":"Dx0.1"},{"name":"kI","value":"Dx0.0"},{"name":"kP","value":"Dx0.5"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix1"},{"name":"velocityDEGREES","value":"Dx180.0"}]},{"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.5"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx16.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":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"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.3"},{"name":"minPower","value":"Dx0.12"},{"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/states/PathEnactor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java index 528cead..bea9898 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java @@ -13,7 +13,7 @@ public class PathEnactor extends CyberarmState { @Override public void start() { String path; - switch (engine.blackboardGetInt("AutonomousPath")) { + switch (engine.blackboardGetInt("autonomousPath")) { case 1: path = "CENTER"; break; 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 b36aaf2..b6cc8c9 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 @@ -10,6 +10,8 @@ import java.util.List; import dev.cyberarm.engine.V2.CyberarmState; import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.red_crab.SpikeMarkDetectorVisionProcessor; +import dev.cyberarm.minibots.red_crab.TeamPropVisionProcessor; public class PathSelector extends CyberarmState { private final RedCrabMinibot robot; @@ -28,14 +30,14 @@ public class PathSelector extends CyberarmState { @Override public void init() { - robot.tfPixel = TfodProcessor.easyCreateWithDefaults(); + robot.teamProp = new TeamPropVisionProcessor(); robot.visionPortal = VisionPortal.easyCreateWithDefaults( - engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.tfPixel); + engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.teamProp); - robot.tfPixel.setClippingMargins(0, 0, 0, 0); - robot.tfPixel.setMinResultConfidence((float) minConfidence); +// robot.tfPixel.setClippingMargins(0, 0, 0, 0); +// robot.tfPixel.setMinResultConfidence((float) minConfidence); - engine.blackboardSet("autonomousPath", 0); + engine.blackboardSet("autonomousPath", path); } @Override @@ -46,14 +48,26 @@ public class PathSelector extends CyberarmState { return; } - recognitions = robot.tfPixel.getRecognitions(); - for (Recognition recognition : recognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2; - double y = (recognition.getTop() + recognition.getBottom()) / 2; +// 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")) { +// engine.blackboardSet("autonomousPath", path); +// } +// } - if (recognition.getLabel().equals("pixel")) { - engine.blackboardSet("autonomousPath", path); - } + switch (robot.teamProp.getLocation()) { + case LEFT: + engine.blackboardSet("autonomousPath", 0); + break; + case CENTER: + engine.blackboardSet("autonomousPath", 1); + break; + case RIGHT: + engine.blackboardSet("autonomousPath", 2); + break; } if (runTime() >= timeoutMS) { @@ -65,24 +79,25 @@ public class PathSelector extends CyberarmState { private void stopVision() { robot.visionPortal.close(); - robot.tfPixel.shutdown(); } @Override public void telemetry() { engine.telemetry.addLine(); - engine.telemetry.addData("# Objects Detected", recognitions.size()); +// engine.telemetry.addData("Saturation", robot.spikeMark.getSaturation()); - for(Recognition recognition : recognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2; - double y = (recognition.getTop() + recognition.getBottom()) / 2; - - engine.telemetry.addLine(); - - engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - engine.telemetry.addData("- Position", "%.0f / %.0f", x, y); - engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - } +// engine.telemetry.addData("# Objects Detected", recognitions.size()); +// +// for(Recognition recognition : recognitions) { +// double x = (recognition.getLeft() + recognition.getRight()) / 2; +// double y = (recognition.getTop() + recognition.getBottom()) / 2; +// +// engine.telemetry.addLine(); +// +// engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); +// engine.telemetry.addData("- Position", "%.0f / %.0f", x, y); +// engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); +// } } }