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
2024-01-02 20:36:21 -06:00
parent 0f205fed3a 1dd8051202
commit 28933fc5c8
8 changed files with 172 additions and 18 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);
}
}
}

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
View File

@@ -19,7 +19,7 @@ public class SodiPizzaMinibotObject extends Robot {
public HardwareMap hardwareMap;
public DcMotor leftFront, rightFront, leftBack, rightBack;
public Servo shoulder, gripper;
public Servo shoulder, gripper, launcher;
public IMU imu;
public Rev2mDistanceSensor distSensor;
private String string;
@@ -72,6 +72,7 @@ public class SodiPizzaMinibotObject extends Robot {
//Servo Defining
shoulder = engine.hardwareMap.servo.get("arm");
gripper = engine.hardwareMap.servo.get("gripper");
launcher = engine.hardwareMap.servo.get("launcher");
//Distance Sensor

56
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
View File

@@ -9,6 +9,7 @@ import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.GRIPPER
import com.qualcomm.robotcore.hardware.DcMotor;
import org.checkerframework.checker.units.qual.A;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
@@ -20,12 +21,15 @@ import dev.cyberarm.engine.V2.GamepadChecker;
public class SodiPizzaTeleOPState extends CyberarmState {
final private SodiPizzaMinibotObject robot;
private long lastMoveTime;
private long lastMoveTime, lastDistRead /* <- last time Distance was read*/;
public double drivePower;
public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
public double lastToldAngle /* <- The angle the bot was last told to stop at */;
private int armPos;
public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
private int armPos, objectPos;
private boolean droneLaunched;
private char buttonPressed;
private GamepadChecker gp1checker, gp2checker;
YawPitchRollAngles imuInitAngle;
@@ -34,12 +38,24 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.setup();
}
public float getApproxObjPos() {
if (System.currentTimeMillis() - lastDistRead >= 500) {
/*Pseudocode: take objData1, wait, take 2, wait, take 3*/
}
approxObjPos = (objData1 + objData2 + objData3)/3;
return approxObjPos;
}
@Override
public void telemetry() {
engine.telemetry.addData("Arm should be at Position ", armPos);
engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
engine.telemetry.addData("Button Pressed = ", buttonPressed);
engine.telemetry.addLine();
engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
engine.telemetry.addData("Drone Launched?", droneLaunched);
}
@Override
@@ -50,12 +66,15 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.leftBack.setPower(0);
robot.rightBack.setPower(0);
robot.launcher.setPosition(0);
droneLaunched = false;
robot.imu.resetYaw();
imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
gp1checker = new GamepadChecker(engine, engine.gamepad1);
gp2checker = new GamepadChecker(engine, engine.gamepad2);
lastMoveTime = System.currentTimeMillis();
armPos = 0;
@@ -139,6 +158,30 @@ public class SodiPizzaTeleOPState extends CyberarmState {
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
if (engine.gamepad2.left_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.launcher.setPosition(0.98);
lastMoveTime = System.currentTimeMillis();
}
} else if (engine.gamepad2.right_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 100) {
robot.launcher.setPosition(robot.launcher.getPosition() - 0.2);
lastMoveTime = System.currentTimeMillis();
}
} else if (robot.launcher.getPosition() >= 0.95) {
if (System.currentTimeMillis() - lastMoveTime >= 1000) {
droneLaunched = true;
lastMoveTime = System.currentTimeMillis();
}
}
if (!engine.gamepad2.left_stick_button && droneLaunched) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.launcher.setPosition(robot.launcher.getPosition() - 0.025);
lastMoveTime = System.currentTimeMillis();
}
}
// This moves the arm to Collect position, which is at servo position 0.00.
if (engine.gamepad2.a && !engine.gamepad2.start) {
armPos = 1;
@@ -274,7 +317,8 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.gripper.setPosition(GRIPPER_CLOSED);
}
gp1checker.update();
gp2checker.update();
}