TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 08:42:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

RedCrab: Initial Team Prop vision test finished, added 'telemetry' data to camera preview, fixed PathEnactor checking for 'AutonomousPath' instead of 'autonomousPath' from the blackboard, started building Red Backstage autonomous.

Browse Source
This commit is contained in:
Cyberarm
2023-12-19 22:00:35 -06:00
parent ef1a6e9d0c
commit 1bf25d9feb
6 changed files with 79 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -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);
}
}

4
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java
View File

@@ -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);

29
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java
View File

@@ -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) {

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json
View File

File diff suppressed because one or more lines are too long

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java
View File

@@ -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;

63
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java
View File

@@ -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());
// }
}
}