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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2023-12-18 17:00:54 -06:00
parent b8bd65a4b3 d2b2096af2
commit c7a8a7c440
19 changed files with 607 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/dev/cyberarm/drivers/EncoderAdafruitP4991.java
View File

@@ -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<I2cDeviceSynch> {
final Object i2cLock = new Object();
I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x36);

56
TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java
View File

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

3
TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java
View File

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

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

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

117
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java Normal file
View File

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

6
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TODO.md Normal file
View File

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

156
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java Normal file
View File

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

1
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json Normal file
View File

File diff suppressed because one or more lines are too long

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java
View File

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

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java
View File

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

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java
View File

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

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java
View File

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

7
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java
View File

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

12
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java
View File

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

16
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java
View File

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

25
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java
View File

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

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

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

9
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java
View File

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

19
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java
View File

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