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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2024-01-06 12:47:34 -06:00
parent 1dc28841f1 d778d1be69
commit dffb883427
11 changed files with 69 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,9 +1,6 @@
package dev.cyberarm.minibots.red_crab;
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;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -12,7 +9,6 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
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;
@@ -46,6 +42,7 @@ public class RedCrabMinibot {
public static double CLAW_ARM_MAX_SPEED = 0.5;
public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10;
private static double CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = 1588.0;
private static long CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = 5000;
public static double CLAW_ARM_kP = 0.0;
public static double CLAW_ARM_kI = 0.0;
public static double CLAW_ARM_kD = 0.0;
@@ -92,6 +89,8 @@ public class RedCrabMinibot {
private final PIDFController clawArmPIDFController;
public final String webcamName = "Webcam 1";
private long lastClawArmOverCurrentAnnounced = 0;
private boolean clawArmOverCurrent = false;
public enum Path {
LEFT,
CENTER,
@@ -258,6 +257,7 @@ public class RedCrabMinibot {
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();
RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value();
RedCrabMinibot.CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = config.variable("Robot", "ClawArm_Tuning", "warn_overcurrent_after_ms").value();
/// WINCH
@@ -412,6 +412,18 @@ public class RedCrabMinibot {
public void controlClawArm() {
Action action = config.action("Robot", "ClawArm_Tuning");
long milliseconds = System.currentTimeMillis();
if (clawArm.isOverCurrent())
{
if (milliseconds - lastClawArmOverCurrentAnnounced >= CLAW_ARM_WARN_OVERCURRENT_AFTER_MS) {
lastClawArmOverCurrentAnnounced = System.currentTimeMillis();
engine.telemetry.speak("WARNING. ARM. OVER. CURRENT.");
}
} else {
lastClawArmOverCurrentAnnounced = milliseconds;
}
double ticksInDegree = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1);
for (Variable v : action.getVariables()) {
@@ -450,6 +462,18 @@ public class RedCrabMinibot {
double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES);
double currentAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition());
double targetAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition());
double angleDiff = Math.abs(Utilities.angleDiff(currentAngle, targetAngle));
// Turn off motor if it is stowed or all the way down
if (targetAngle <= CLAW_ARM_STOW_ANGLE + 5.0 && angleDiff <= 5.0) {
velocity = 0.0;
} else if (targetAngle >= CLAW_ARM_COLLECT_ANGLE - 5.0 && angleDiff <= 5.0)
{
velocity = 0.0;
}
clawArm.setVelocity(velocity);
}

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

@@ -158,6 +158,15 @@ public class TeamPropVisionProcessor implements VisionProcessor {
return saturationRight;
}
public double getHighestSaturation() {
if (getSaturationLeft() > getSaturationCenter() && getSaturationLeft() > getSaturationRight())
return getSaturationLeft();
if (getSaturationCenter() > getSaturationLeft() && getSaturationCenter() > getSaturationRight())
return getSaturationCenter();
return getSaturationRight();
}
private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) {
int left = Math.round(rect.x * scaleBmpPxToCanvasPx);
int top = Math.round(rect.y * scaleBmpPxToCanvasPx);

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

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

6
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java
View File

@@ -2,12 +2,18 @@ package dev.cyberarm.minibots.red_crab.engines;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
protected RedCrabMinibot robot;
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
if (robot != null)
robot.standardTelemetry();
super.loop();
}
}

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

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

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
setupFromConfig(

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

@@ -25,7 +25,7 @@ public class PathEnactor extends CyberarmState {
break;
}
this.pathGroupName = String.format("AutonomousPixelPath_%s", path);
this.pathGroupName = String.format("Autonomous_SpikePath_%s", path);
}
@Override

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

@@ -18,7 +18,6 @@ public class PathSelector extends CyberarmState {
private final int timeoutMS;
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;
@@ -34,40 +33,27 @@ public class PathSelector extends CyberarmState {
robot.visionPortal = VisionPortal.easyCreateWithDefaults(
engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.teamProp);
// robot.tfPixel.setClippingMargins(0, 0, 0, 0);
// robot.tfPixel.setMinResultConfidence((float) minConfidence);
engine.blackboardSet("autonomousPath", path);
}
@Override
public void exec() {
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")) {
// 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 we've got enough Saturation for our min confidence then do the needful.
if (robot.teamProp.getHighestSaturation() >= minConfidence) {
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;
}
} else
{
engine.blackboardSet("autonomousPath", path); // min confidence not meant, default to center.
}
if (runTime() >= timeoutMS) {
@@ -85,19 +71,9 @@ public class PathSelector extends CyberarmState {
public void telemetry() {
engine.telemetry.addLine();
// engine.telemetry.addData("Saturation", robot.spikeMark.getSaturation());
// 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());
// }
engine.telemetry.addData("Location", robot.teamProp.getLocation());
engine.telemetry.addData("Saturation Left", robot.teamProp.getSaturationLeft());
engine.telemetry.addData("Saturation Center", robot.teamProp.getSaturationCenter());
engine.telemetry.addData("Saturation Right", robot.teamProp.getSaturationRight());
}
}

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

@@ -209,17 +209,6 @@ public class Pilot extends CyberarmState {
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION);
break;
}
if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT &&
robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) {
robot.clawArm.setPower(0);
} else {
robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
}
}