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

Removed rotation from openCV frame processor, got claw arm working as intended, winch now locks position when trigger is released, wip: toggling between claw arm and winch manual control (disabled for now), fixed claw arm deposit angle not correctly set from config (was using wrong key), updated hook arm hooking position.

Browse Source
This commit is contained in:
Cyberarm
2023-12-30 13:21:49 -06:00
parent 4e9e646947
commit ad68fbb7c0
6 changed files with 120 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -45,6 +45,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;
public static double CLAW_ARM_kP = 0.0;
public static double CLAW_ARM_kI = 0.0;
public static double CLAW_ARM_kD = 0.0;
@@ -181,6 +182,7 @@ public class RedCrabMinibot {
winch.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- RUN MODE
winch.setTargetPosition(winch.getCurrentPosition());
winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
/// CLAW and Co. ///
@@ -249,12 +251,13 @@ public class RedCrabMinibot {
RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "max_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();
RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "deposit_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();
RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value();
/// WINCH
@@ -362,6 +365,7 @@ public class RedCrabMinibot {
PIDFCoefficients clawArmPIDFPosition = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION);
PIDFCoefficients clawArmPIDFEncoder = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER);
int clawArmError = clawArm.getCurrentPosition() - clawArm.getTargetPosition();
engine.telemetry.addData(
"Claw Arm",
"Power: %.2f, Current: %.2f mAmp, Position: %d, Angle: %.2f, Velocity: %.2f (%.2f degrees/s)",
@@ -371,6 +375,16 @@ public class RedCrabMinibot {
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition()),
clawArm.getVelocity(),
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, (int)clawArm.getVelocity()));
engine.telemetry.addData(
"Claw Arm",
"TPos: %d, TAngle: %.2f, ErrPos: %d ErrAngle: %.2f",
clawArm.getTargetPosition(),
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition()),
clawArmError,
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArmError),
clawArm.getVelocity(),
Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, (int)clawArm.getVelocity()));
engine.telemetry.addData("Current Alarm?", clawArm.isOverCurrent());
engine.telemetry.addData(
" PIDF", "P: %.4f, I: %.4f, D: %.4f, F: %.4f, PosP: %.4f",
clawArmPIDFEncoder.p,
@@ -432,6 +446,8 @@ public class RedCrabMinibot {
clawArm.setVelocityPIDFCoefficients(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF);
clawArm.setPositionPIDFCoefficients(CLAW_ARM_kPosP);
clawArm.setCurrentAlert(CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS, CurrentUnit.MILLIAMPS);
double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES);
clawArm.setVelocity(velocity);

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

@@ -41,8 +41,8 @@ public class SpikeMarkDetectorVisionProcessor implements VisionProcessor {
@Override
public Object processFrame(Mat frame, long captureTimeNanos) {
Core.rotate(frame, rotatedMat,Core.ROTATE_180);
Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV);
// Core.rotate(frame, rotatedMat,Core.ROTATE_180);
Imgproc.cvtColor(frame, hsvMat, Imgproc.COLOR_RGB2HSV);
saturation = averageSaturation(hsvMat, rect);

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

@@ -49,8 +49,8 @@ public class TeamPropVisionProcessor implements VisionProcessor {
@Override
public Object processFrame(Mat frame, long captureTimeNanos) {
Core.rotate(frame, rotatedMat,Core.ROTATE_180);
Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV);
// Core.rotate(frame, rotatedMat,Core.ROTATE_180);
Imgproc.cvtColor(frame, hsvMat, Imgproc.COLOR_RGB2HSV);
saturationLeft = averageSaturation(hsvMat, rectLeft);
saturationCenter = averageSaturation(hsvMat, rectCenter);

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

47
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java Normal file
View File

@@ -0,0 +1,47 @@
package dev.cyberarm.minibots.red_crab.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
@TeleOp(name = "Cyberarm Red Crab TeleOp DEBUGGING", group = "MINIBOT")
public class RedCrabTeleOpDebuggingOnlyEngine extends CyberarmEngine {
@Override
public void setup() {
threadless();
addState(new CyberarmState() {
final RedCrabMinibot robot = new RedCrabMinibot(false);
@Override
public void exec() {
double velocity = -engine.gamepad1.left_stick_y * Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES);
// robot.clawArm.setVelocity(velocity);
robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.clawArm.setPower(-engine.gamepad1.left_stick_y * 0.5);
}
@Override
public void telemetry() {
robot.standardTelemetry();
}
});
}
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
super.loop();
}
}

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

@@ -1,5 +1,6 @@
package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@@ -20,6 +21,9 @@ public class Pilot extends CyberarmState {
private boolean droneLaunchRequested = false;
private double droneLastLaunchRequestStartMS = 0;
private boolean robotSlowMode = false;
private boolean triggersControlClawArm = false;
private double deltaTime = 0;
private double lastLoopTimeMS = 0;
public Pilot(RedCrabMinibot robot) {
this.robot = robot;
@@ -27,6 +31,8 @@ public class Pilot extends CyberarmState {
@Override
public void exec() {
deltaTime = runTime() - lastLoopTimeMS;
drivetrain();
clawArmAndWristController();
@@ -34,6 +40,8 @@ public class Pilot extends CyberarmState {
droneLatchController();
hookArmController(); // disabled for swrist debug
winchController();
lastLoopTimeMS = runTime();
}
@Override
@@ -71,6 +79,17 @@ public class Pilot extends CyberarmState {
case "dpad_right":
clawArmPosition = RedCrabMinibot.ClawArm_COLLECT_FLOAT;
break;
case "back":
// FIXME: trigger controls don't yet work well.
triggersControlClawArm = false; //!triggersControlClawArm;
if (triggersControlClawArm)
{
engine.telemetry.speak("Claw Arm Manual");
} else {
engine.telemetry.speak("Winch Manual");
}
break;
case "start":
robot.reloadConfig();
break;
@@ -140,6 +159,23 @@ public class Pilot extends CyberarmState {
}
private void clawArmAndWristController() {
if (triggersControlClawArm)
{
double triggerPower = engine.gamepad1.right_trigger - engine.gamepad1.left_trigger;
robot.clawArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
double maxVelocity = Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES);
robot.clawArm.setTargetPosition((int)(deltaTime * maxVelocity));
robot.clawArm.setVelocity(maxVelocity * triggerPower);
return;
} else {
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
switch (clawArmPosition) {
case RedCrabMinibot.ClawArm_STOW:
robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks(
@@ -180,10 +216,10 @@ public class Pilot extends CyberarmState {
robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) {
// robot.clawArm.setPower(0);
// } else {
// robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) {
robot.clawArm.setPower(0);
} else {
robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
}
}
@@ -206,12 +242,22 @@ public class Pilot extends CyberarmState {
}
private void winchController() {
if (triggersControlClawArm)
{
return;
}
if (engine.gamepad1.right_trigger > 0) {
robot.winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.winch.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
robot.winch.setTargetPosition(robot.winch.getCurrentPosition());
} else if (engine.gamepad1.left_trigger > 0) {
robot.winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.winch.setPower(-engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
robot.winch.setTargetPosition(robot.winch.getCurrentPosition());
} else {
robot.winch.setPower(0);
robot.winch.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.winch.setPower(RedCrabMinibot.WINCH_MAX_SPEED);
}
}
}